diff --git a/.github/workflows/build_test.yaml b/.github/workflows/build_test.yaml new file mode 100644 index 0000000..e399005 --- /dev/null +++ b/.github/workflows/build_test.yaml @@ -0,0 +1,69 @@ +name: build test + +on: + workflow_dispatch: + pull_request: + +jobs: + build: + name: build + runs-on: ubuntu-22.04 + timeout-minutes: 120 + strategy: + fail-fast: false + env: + ROS_DISTRO: humble + container: + image: osrf/ros:humble-desktop-jammy + steps: + - name: Checkout + uses: actions/checkout@v4 + with: + fetch-depth: 0 + + - name: Install dependencies of choreonoid + run: | + apt-get update && \ + apt-get -y upgrade && \ + apt-get -y install \ + build-essential \ + cmake-curses-gui \ + libboost-dev \ + libboost-system-dev \ + libboost-program-options-dev \ + libboost-iostreams-dev \ + libeigen3-dev \ + uuid-dev \ + libxfixes-dev \ + libyaml-dev \ + libfmt-dev \ + gettext \ + zlib1g-dev \ + libjpeg-dev \ + libpng-dev \ + libfreetype-dev \ + qtbase5-dev \ + libqt5x11extras5-dev \ + libqt5svg5-dev \ + qttranslations5-l10n \ + python3-dev \ + python3-numpy \ + libassimp-dev \ + libode-dev \ + libfcl-dev \ + libpulse-dev \ + libsndfile1-dev \ + libgstreamer1.0-dev \ + libgstreamer-plugins-base1.0-dev \ + libzip-dev \ + python3-colcon-coveragepy-result \ + python3-colcon-lcov-result + + - name: Run build test + uses: ros-tooling/action-ros-ci@v0.3 + with: + target-ros2-distro: humble + vcs-repo-file-url: dependency.repos + extra-cmake-args: -DWITH_INTEGRATION_TEST=ON + # If possible, pin the repository in the workflow to a specific commit to avoid + # changes in colcon-mixin-repository from breaking your tests. diff --git a/CMakeLists.txt b/CMakeLists.txt index c7f9ea7..557d8d0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -7,25 +7,39 @@ if(NOT CMAKE_BUILD_TYPE) FORCE) endif() -find_package( - catkin REQUIRED COMPONENTS - choreonoid - roscpp - std_msgs - sensor_msgs - image_transport - - # For ros_control - pluginlib - angles - controller_manager - hardware_interface - joint_limits_interface - transmission_interface - urdf - ) - -catkin_package(SKIP_CMAKE_CONFIG_GENERATION SKIP_PKG_CONFIG_GENERATION) +if(DEFINED ENV{ROS_VERSION}) + if($ENV{ROS_VERSION} EQUAL 1) + find_package( + catkin REQUIRED COMPONENTS + roscpp + pluginlib + angles + controller_manager + hardware_interface + joint_limits_interface + transmission_interface + urdf + ) + elseif ($ENV{ROS_VERSION} EQUAL 2) + find_package( + ament_cmake REQUIRED COMPONENTS + rclcpp ament_index_cpp + ) + endif() +endif() + +find_package(std_msgs REQUIRED) +find_package(std_srvs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(image_transport REQUIRED) +find_package(choreonoid REQUIRED) + +if(DEFINED ENV{ROS_VERSION}) + if($ENV{ROS_VERSION} EQUAL 1) + catkin_package(SKIP_CMAKE_CONFIG_GENERATION SKIP_PKG_CONFIG_GENERATION) + include_directories(${catkin_INCLUDE_DIRS}) + endif() +endif() if(CHOREONOID_CXX_STANDARD) set(CMAKE_CXX_STANDARD ${CHOREONOID_CXX_STANDARD}) @@ -34,14 +48,28 @@ else() add_compile_options(-std=c++14) endif() -include_directories(${catkin_INCLUDE_DIRS}) add_subdirectory(src) -# For ros_control -install(FILES plugins.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) +if(DEFINED ENV{ROS_VERSION}) + if($ENV{ROS_VERSION} EQUAL 1) + # For ros_control + install(FILES plugins.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + + # Install `launch` dir. only if `--install` option is set in catkin config + if(${CMAKE_INSTALL_PREFIX} MATCHES .*/install) + install(DIRECTORY launch + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + USE_SOURCE_PERMISSIONS + PATTERN "ros2" EXCLUDE + ) + endif() + elseif($ENV{ROS_VERSION} EQUAL 2) + ament_package() -install(DIRECTORY launch - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} - USE_SOURCE_PERMISSIONS - ) + install(DIRECTORY + launch/ros2 + DESTINATION share/${PROJECT_NAME}/launch + ) + endif() +endif() diff --git a/dependency.repos b/dependency.repos new file mode 100644 index 0000000..0599050 --- /dev/null +++ b/dependency.repos @@ -0,0 +1,5 @@ +repositories: + choreonoid: + type: git + url: https://github.com/choreonoid/choreonoid.git + version: master diff --git a/launch/ros2/choreonoid.launch.xml b/launch/ros2/choreonoid.launch.xml new file mode 100644 index 0000000..b872dd1 --- /dev/null +++ b/launch/ros2/choreonoid.launch.xml @@ -0,0 +1,5 @@ + + + + diff --git a/package.xml b/package.xml index fff9266..fda67db 100644 --- a/package.xml +++ b/package.xml @@ -1,5 +1,5 @@ - + choreonoid_ros 1.7.0 The package for using Choreonoid as a ROS node @@ -10,22 +10,32 @@ http://choreonoid.org https://github.com/choreonoid/choreonoid_ros.git - catkin + catkin + ament_cmake choreonoid - roscpp + + roscpp + rclcpp + std_msgs + std_srvs + geometry_msgs sensor_msgs image_transport - pluginlib - angles - controller_manager - hardware_interface - joint_limits_interface - transmission_interface - urdf + image_transport_plugins + + pluginlib + angles + controller_manager + hardware_interface + joint_limits_interface + transmission_interface + urdf + ament_index_cpp - cmake - + cmake + ament_cmake + diff --git a/src/node/CMakeLists.txt b/src/node/CMakeLists.txt index 1e4b70f..89f1364 100644 --- a/src/node/CMakeLists.txt +++ b/src/node/CMakeLists.txt @@ -1,5 +1,17 @@ -set(target choreonoid_ros) -add_executable(${target} choreonoid_ros.cpp) -set_target_properties(${target} PROPERTIES OUTPUT_NAME choreonoid) -target_link_libraries(${target} Choreonoid::CnoidBase ${roscpp_LIBRARIES}) -install(TARGETS ${target} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) +set(target choreonoid) + +if(DEFINED ENV{ROS_VERSION}) + if($ENV{ROS_VERSION} EQUAL 1) + add_executable(${target} choreonoid_ros.cpp) + set_target_properties(${target} PROPERTIES OUTPUT_NAME choreonoid) + target_link_libraries(${target} Choreonoid::CnoidBase ${roscpp_LIBRARIES}) + install(TARGETS ${target} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) + elseif($ENV{ROS_VERSION} EQUAL 2) + add_executable(${target} choreonoid_ros2.cpp) + ament_target_dependencies(${target} rclcpp ament_index_cpp std_msgs sensor_msgs image_transport) + set_target_properties(${target} PROPERTIES OUTPUT_NAME choreonoid) + target_link_libraries(${target} Choreonoid::CnoidBase) + install(TARGETS ${target} + RUNTIME DESTINATION lib/${PROJECT_NAME}) + endif() +endif() diff --git a/src/node/choreonoid_ros2.cpp b/src/node/choreonoid_ros2.cpp new file mode 100644 index 0000000..1a043b3 --- /dev/null +++ b/src/node/choreonoid_ros2.cpp @@ -0,0 +1,73 @@ +#include +#include + +#include +#include +#include + +#include +#include +#include + +int main(int argc, char** argv) +{ + // removes ros-dependent arguments + // rclcpp::remove_ros_arguments throws an error if it fails parsing + std::vector nonRosArgvString = rclcpp::remove_ros_arguments(argc, argv); + int nonRosArgc = nonRosArgvString.size(); + char* nonRosArgv[nonRosArgc]; + for (int i = 0; i < nonRosArgc; ++i) { + nonRosArgv[i] = nonRosArgvString.at(i).data(); + } + if (nonRosArgvString.at(nonRosArgc - 1).empty()) { + // ignores the final nonRosArgv + // because remove_ros_arguments potentially returns an empty string, + // which causes [Warning: Input file "" was not processed.] on Choreonoid + --nonRosArgc; + } + + cnoid::App app(nonRosArgc, nonRosArgv, "Choreonoid-ROS2", "Choreonoid"); + + auto pluginManager = cnoid::PluginManager::instance(); + + const char* prefixVar = getenv("AMENT_PREFIX_PATH"); + if(prefixVar){ + do { + const char* begin = prefixVar; + while(*prefixVar != ':' && *prefixVar) prefixVar++; + pluginManager->addPluginDirectoryAsPrefix(cnoid::toUTF8(std::string(begin, prefixVar))); + } while (0 != *prefixVar++); + } else { + try { + pluginManager->addPluginDirectoryAsPrefix( + ament_index_cpp::get_package_prefix("choreonoid_ros")); + } + catch(const ament_index_cpp::PackageNotFoundError& ex){ + std::cerr << "Error: The choreonoid_ros package is not found." << std::endl; + return 1; + } + } + + if(!app.requirePluginToCustomizeApplication("ROS2")){ + if(app.error() == cnoid::App::PluginNotFound){ + auto message = app.errorMessage(); + if(message.empty()){ + std::cerr << "ROS2 plugin is not found." << std::endl; + } else { + std::cerr << "ROS2 plugin cannot be loaded.\n"; + std::cerr << message << std::endl; + } + } else { + std::cerr << "ROS2 plugin does not work corerctly." << std::endl; + } + return 1; + } + + rclcpp::init(argc, argv); + + int ret = app.exec(); + + rclcpp::shutdown(); + + return ret; +} diff --git a/src/plugin/BodyROS2Item.cpp b/src/plugin/BodyROS2Item.cpp new file mode 100644 index 0000000..233e2f4 --- /dev/null +++ b/src/plugin/BodyROS2Item.cpp @@ -0,0 +1,852 @@ +#include "BodyROS2Item.h" +#include "Format.h" +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include + +#include "gettext.h" + +using namespace cnoid; +using std::placeholders::_1; +using std::placeholders::_2; + +namespace { + +template +struct has_dispatch : std::false_type {}; + +template +struct has_dispatch().dispatch(std::declval>()))>> + : std::true_type {}; + +template +std::enable_if_t::value> dispatch(T& obj, std::function func) +{ + obj.dispatch(func); +} + +template +std::enable_if_t::value> dispatch(T& obj, std::function func) +{ + obj.start(func); +} + +} + + +void BodyROS2Item::initializeClass(ExtensionManager *ext) +{ + ext->itemManager().registerClass(N_("BodyROS2Item")); + ext->itemManager().addCreationPanel(); +} + + +BodyROS2Item::BodyROS2Item() + : os(MessageView::instance()->cout()), + threadPoolForPublishing(10) +{ + bodyItem = nullptr; + io = nullptr; + jointStateUpdateRate = 100.0; +} + + +BodyROS2Item::BodyROS2Item(const BodyROS2Item &org) + : ControllerItem(org), + os(MessageView::instance()->cout()), + threadPoolForPublishing(10) +{ + io = nullptr; + jointStateUpdateRate = 100.0; +} + + +BodyROS2Item::~BodyROS2Item() +{ + stop(); +} + + +Item *BodyROS2Item::doDuplicate() const +{ + return new BodyROS2Item(*this); +} + + +bool BodyROS2Item::store(Archive &archive) +{ + archive.write("body_ros_version", 0); + archive.write("joint_state_update_rate", jointStateUpdateRate); + + return true; +} + + +bool BodyROS2Item::restore(const Archive &archive) +{ + archive.read({"joint_state_update_rate", "jointStateUpdateRate"}, + jointStateUpdateRate); + return true; +} + + +void BodyROS2Item::doPutProperties(PutPropertyFunction &putProperty) +{ + putProperty.decimals(2).min(0.0)("Update rate", + jointStateUpdateRate, + changeProperty(jointStateUpdateRate)); +} + + +void BodyROS2Item::onTreePathChanged() +{ + bool hasValidTargetBody = false; + if(isConnectedToRoot()){ + auto bodyItem = findOwnerItem(); + if(bodyItem){ + if(bodyItem != this->bodyItem) { + initializeRosNode(bodyItem); + } + hasValidTargetBody = true; + } + } + if(!hasValidTargetBody){ + finalizeRosNode(); + } +} + + +void BodyROS2Item::initializeRosNode(BodyItem* bodyItem) +{ + finalizeRosNode(); + + this->bodyItem = bodyItem; + std::string name = bodyItem->name(); + std::replace(name.begin(), name.end(), '-', '_'); + rosNode = std::make_shared(name); + + executor = std::make_unique(); + executor->add_node(rosNode); + executorThread = std::thread([this](){ executor->spin(); }); + + jointStatePublisher = rosNode->create_publisher( + getROS2Name("joint_states"), 1); +} + + +void BodyROS2Item::finalizeRosNode() +{ + if(executor){ + executor->cancel(); + executorThread.join(); + executor->remove_node(rosNode); + executor.reset(); + } + + rosNode.reset(); + + bodyItem = nullptr; +} + + +bool BodyROS2Item::initialize(ControllerIO *io) +{ + if (!io->body()) { + MessageView::instance() + ->putln(formatR(_("BodyROS2Item \"{0}\" is invalid because it is " + "not assigned to a body."), + displayName()), + MessageView::WARNING); + return false; + } + + this->io = io; + simulationBody = io->body(); + timeStep_ = io->worldTimeStep(); + controlTime_ = io->currentTime(); + + return true; +} + + +double BodyROS2Item::timeStep() const +{ + return timeStep_; +} + + +bool BodyROS2Item::start() +{ + imageTransport = std::make_shared(rosNode); + // buffer of preserve currently state of joints. + jointState.header.stamp = getStampMsgFromSec(controlTime_); + jointState.name.resize(body()->numAllJoints()); + jointState.position.resize(body()->numAllJoints()); + jointState.velocity.resize(body()->numAllJoints()); + jointState.effort.resize(body()->numAllJoints()); + + // preserve initial state of joints. + for (size_t i = 0; i < body()->numAllJoints(); i++) { + Link *joint = body()->joint(i); + + jointState.name[i] = joint->jointName(); + jointState.position[i] = joint->q(); + jointState.velocity[i] = joint->dq(); + jointState.effort[i] = joint->u(); + } + + createSensors(simulationBody); + + jointStateUpdatePeriod = 1.0 / jointStateUpdateRate; + jointStateLastUpdate = io->currentTime(); + RCLCPP_DEBUG(rosNode->get_logger(), + "Joint state update rate %f", + jointStateUpdateRate); + + return true; +} + + +void BodyROS2Item::createSensors(BodyPtr body) +{ + using SetBoolCallback = std::function< + void(const std::shared_ptr, + std::shared_ptr)>; + + DeviceList<> devices = body->devices(); + + forceSensors_.assign(devices.extract()); + gyroSensors_.assign(devices.extract()); + accelSensors_.assign(devices.extract()); + imus_.assign(devices.extract()); + visionSensors_.assign(devices.extract()); + rangeVisionSensors_.assign(devices.extract()); + rangeSensors_.assign(devices.extract()); + + for (size_t i = 0; i < visionSensors_.size(); ++i) { + if (Camera *sensor = visionSensors_[i]) { + RangeCamera *camera = dynamic_cast(sensor); + if (camera) { + rangeVisionSensors_.push_back(camera); + } + } + } + + forceSensorPublishers.clear(); + forceSensorPublishers.reserve(forceSensors_.size()); + forceSensorSwitchServers.clear(); + forceSensorSwitchServers.reserve(forceSensors_.size()); + for (auto sensor : forceSensors_) { + std::string name = getROS2Name(sensor->name()); + auto publisher + = rosNode->create_publisher(name, + 1); + sensor->sigStateChanged().connect([this, sensor, publisher]() { + updateForceSensor(sensor, publisher); + }); + forceSensorPublishers.push_back(publisher); + SetBoolCallback requestCallback = std::bind(&BodyROS2Item::switchDevice, + this, + _1, + _2, + sensor); + forceSensorSwitchServers.push_back( + rosNode->create_service(name + "/set_enabled", + requestCallback)); + RCLCPP_INFO(rosNode->get_logger(), + "Create force sensor %s", + sensor->name().c_str()); + } + + rateGyroSensorPublishers.clear(); + rateGyroSensorPublishers.reserve(gyroSensors_.size()); + rateGyroSensorSwitchServers.clear(); + rateGyroSensorSwitchServers.reserve(gyroSensors_.size()); + for (auto sensor : gyroSensors_) { + std::string name = getROS2Name(sensor->name()); + auto publisher = rosNode->create_publisher(name, 1); + sensor->sigStateChanged().connect([this, sensor, publisher]() { + updateRateGyroSensor(sensor, publisher); + }); + rateGyroSensorPublishers.push_back(publisher); + SetBoolCallback requestCallback = std::bind(&BodyROS2Item::switchDevice, + this, + _1, + _2, + sensor); + rateGyroSensorSwitchServers.push_back( + rosNode->create_service(name + "/set_enabled", + requestCallback)); + RCLCPP_INFO(rosNode->get_logger(), + "Create gyro sensor %s", + sensor->name().c_str()); + } + + accelSensorPublishers.clear(); + accelSensorPublishers.reserve(accelSensors_.size()); + accelSensorSwitchServers.clear(); + accelSensorSwitchServers.reserve(accelSensors_.size()); + for (auto sensor : accelSensors_) { + std::string name = getROS2Name(sensor->name()); + auto publisher = rosNode->create_publisher(name, 1); + sensor->sigStateChanged().connect([this, sensor, publisher]() { + updateAccelSensor(sensor, publisher); + }); + accelSensorPublishers.push_back(publisher); + SetBoolCallback requestCallback = std::bind(&BodyROS2Item::switchDevice, + this, + _1, + _2, + sensor); + accelSensorSwitchServers.push_back( + rosNode->create_service(name + "/set_enabled", + requestCallback)); + RCLCPP_INFO(rosNode->get_logger(), + "Create accel sensor %s", + sensor->name().c_str()); + } + + imuPublishers.clear(); + imuPublishers.reserve(imus_.size()); + imuSwitchServers.clear(); + imuSwitchServers.reserve(imus_.size()); + for (auto sensor : imus_) { + std::string name = getROS2Name(sensor->name()); + auto publisher = rosNode->create_publisher(name, 1); + sensor->sigStateChanged().connect([this, sensor, publisher]() { + updateImu(sensor, publisher); + }); + imuPublishers.push_back(publisher); + SetBoolCallback requestCallback = std::bind(&BodyROS2Item::switchDevice, + this, + _1, + _2, + sensor); + imuSwitchServers.push_back( + rosNode->create_service(name + "/set_enabled", + requestCallback)); + RCLCPP_INFO(rosNode->get_logger(), + "Create IMU %s", + sensor->name().c_str()); + } + + visionSensorPublishers.clear(); + visionSensorPublishers.reserve(visionSensors_.size()); + visionSensorSwitchServers.clear(); + visionSensorSwitchServers.reserve(visionSensors_.size()); + for (CameraPtr sensor : visionSensors_) { + std::string name = getROS2Name(sensor->name()); + auto publisher = imageTransport->advertise(name, 1); + visionSensorPublishers.push_back(publisher); + sensor->sigStateChanged().connect([this, sensor, publisher]() { + updateVisionSensor(sensor, publisher); + }); + SetBoolCallback requestCallback = std::bind(&BodyROS2Item::switchDevice, + this, + _1, + _2, + sensor); + visionSensorSwitchServers.push_back( + rosNode->create_service(name + "/set_enabled", + requestCallback)); + RCLCPP_INFO(rosNode->get_logger(), + "Create RGB camera %s (%f Hz)", + sensor->name().c_str(), + sensor->frameRate()); + } + + rangeVisionSensorPublishers.clear(); + rangeVisionSensorPublishers.reserve(rangeVisionSensors_.size()); + rangeVisionSensorSwitchServers.clear(); + rangeVisionSensorSwitchServers.reserve(rangeVisionSensors_.size()); + for (auto sensor : rangeVisionSensors_) { + std::string name = getROS2Name(sensor->name()); + auto publisher = rosNode->create_publisher( + name + "/point_cloud", 1); + sensor->sigStateChanged().connect([this, sensor, publisher]() { + updateRangeVisionSensor(sensor, publisher); + }); + rangeVisionSensorPublishers.push_back(publisher); + // adds a server only for the camera whose type is COLOR_DEPTH or POINT_CLOUD. + // Without this exception, a new service server may be a duplicate + // of one added to 'visionSensorSwitchServers'. + if (sensor->imageType() == Camera::NO_IMAGE) { + SetBoolCallback requestCallback + = std::bind(&BodyROS2Item::switchDevice, this, _1, _2, sensor); + rangeVisionSensorSwitchServers.push_back( + rosNode->create_service( + name + "/set_enabled", requestCallback)); + RCLCPP_INFO(rosNode->get_logger(), + "Create depth camera %s (%f Hz)", + sensor->name().c_str(), + sensor->frameRate()); + } else { + RCLCPP_INFO(rosNode->get_logger(), + "Create RGBD camera %s (%f Hz)", + sensor->name().c_str(), + sensor->frameRate()); + } + } + + rangeSensorPublishers.clear(); + rangeSensorPublishers.reserve(rangeSensors_.size()); + rangeSensorSwitchServers.clear(); + rangeSensorSwitchServers.reserve(rangeSensors_.size()); + rangeSensorPcPublishers.clear(); + rangeSensorPcPublishers.reserve(rangeSensors_.size()); + rangeSensorPcSwitchServers.clear(); + rangeSensorPcSwitchServers.reserve(rangeSensors_.size()); + for (auto sensor : rangeSensors_) { + if (sensor->pitchRange() > 0.0){ + std::string name = getROS2Name(sensor->name()); + auto pc_publisher = rosNode->create_publisher< + sensor_msgs::msg::PointCloud2>(name + "/point_cloud", 1); + sensor->sigStateChanged().connect([this, sensor, pc_publisher]() { + update3DRangeSensor(sensor, pc_publisher); + }); + rangeSensorPcPublishers.push_back(pc_publisher); + SetBoolCallback requestCallback + = std::bind(&BodyROS2Item::switchDevice, this, _1, _2, sensor); + rangeSensorPcSwitchServers.push_back( + rosNode->create_service( + name + "/set_enabled", requestCallback)); + RCLCPP_DEBUG(rosNode->get_logger(), + "Create 3d range sensor %s (%f Hz)", + sensor->name().c_str(), + sensor->scanRate()); + } else { + std::string name = getROS2Name(sensor->name()); + auto publisher = rosNode->create_publisher< + sensor_msgs::msg::LaserScan>(name + "/scan", 1); + sensor->sigStateChanged().connect([this, sensor, publisher]() { + updateRangeSensor(sensor, publisher); + }); + rangeSensorPublishers.push_back(publisher); + SetBoolCallback requestCallback + = std::bind(&BodyROS2Item::switchDevice, this, _1, _2, sensor); + rangeSensorSwitchServers.push_back( + rosNode->create_service( + name + "/set_enabled", requestCallback)); + RCLCPP_DEBUG(rosNode->get_logger(), + "Create 2d range sensor %s (%f Hz)", + sensor->name().c_str(), + sensor->scanRate()); + } + } +} + +bool BodyROS2Item::control() +{ + controlTime_ = io->currentTime(); + double updateSince = controlTime_ - jointStateLastUpdate; + + if (updateSince > jointStateUpdatePeriod) { + // publish current joint states + jointState.header.stamp = getStampMsgFromSec(controlTime_); + + for (int i = 0; i < body()->numAllJoints(); i++) { + Link *joint = body()->joint(i); + + jointState.position[i] = joint->q(); + jointState.velocity[i] = joint->dq(); + jointState.effort[i] = joint->u(); + } + + jointStatePublisher->publish(jointState); + jointStateLastUpdate += jointStateUpdatePeriod; + } + + return true; +} + + +void BodyROS2Item::updateForceSensor( + ForceSensor* sensor, + rclcpp::Publisher::SharedPtr publisher) +{ + if (!sensor->on()) { + return; + } + + auto wrench = std::make_shared(); + wrench->header.stamp = getStampMsgFromSec(io->currentTime()); + wrench->header.frame_id = sensor->name(); + wrench->wrench.force.x = sensor->F()[0] / 1000.0; + wrench->wrench.force.y = sensor->F()[1] / 1000.0; + wrench->wrench.force.z = sensor->F()[2] / 1000.0; + wrench->wrench.torque.x = sensor->F()[3] / 1000.0; + wrench->wrench.torque.y = sensor->F()[4] / 1000.0; + wrench->wrench.torque.z = sensor->F()[5] / 1000.0; + + + dispatch(threadPoolForPublishing, [publisher, wrench]{ publisher->publish(*wrench); }); +} + + +void BodyROS2Item::updateRateGyroSensor( + RateGyroSensor* sensor, + rclcpp::Publisher::SharedPtr publisher) +{ + if (!sensor->on()) { + return; + } + auto imu = std::make_shared(); + imu->header.stamp = getStampMsgFromSec(io->currentTime()); + imu->header.frame_id = sensor->name(); + imu->angular_velocity.x = sensor->w()[0]; + imu->angular_velocity.y = sensor->w()[1]; + imu->angular_velocity.z = sensor->w()[2]; + + dispatch(threadPoolForPublishing, [publisher, imu]{ publisher->publish(*imu); }); +} + + +void BodyROS2Item::updateAccelSensor( + AccelerationSensor* sensor, + rclcpp::Publisher::SharedPtr publisher) +{ + if (!sensor->on()) { + return; + } + + auto imu = std::make_shared(); + imu->header.stamp = getStampMsgFromSec(io->currentTime()); + imu->header.frame_id = sensor->name(); + imu->linear_acceleration.x = sensor->dv()[0] / 10.0; + imu->linear_acceleration.y = sensor->dv()[1] / 10.0; + imu->linear_acceleration.z = sensor->dv()[2] / 10.0; + + dispatch(threadPoolForPublishing, [publisher, imu]{ publisher->publish(*imu); }); +} + + +void BodyROS2Item::updateImu( + Imu* sensor, + rclcpp::Publisher::SharedPtr publisher) +{ + if (!sensor->on()) { + return; + } + auto imu = std::make_shared(); + imu->header.stamp = getStampMsgFromSec(io->currentTime()); + imu->header.frame_id = sensor->name(); + imu->angular_velocity.x = sensor->w()[0]; + imu->angular_velocity.y = sensor->w()[1]; + imu->angular_velocity.z = sensor->w()[2]; + imu->linear_acceleration.x = sensor->dv()[0] / 10.0; + imu->linear_acceleration.y = sensor->dv()[1] / 10.0; + imu->linear_acceleration.z = sensor->dv()[2] / 10.0; + + dispatch(threadPoolForPublishing, [publisher, imu]{ publisher->publish(*imu); }); +} + + +void BodyROS2Item::updateVisionSensor( + Camera* sensor, + image_transport::Publisher publisher) +{ + if (!sensor->on()) { + return; + } + + auto image = std::make_shared(); + image->header.stamp = getStampMsgFromSec(io->currentTime()); + image->header.frame_id = sensor->name(); + image->height = sensor->image().height(); + image->width = sensor->image().width(); + if (sensor->image().numComponents() == 3){ + image->encoding = "rgb8"; + } else if (sensor->image().numComponents() == 1) { + image->encoding = "mono8"; + } else { + RCLCPP_WARN(rosNode->get_logger(), + "unsupported image component number: %i", + sensor->image().numComponents()); + } + image->is_bigendian = 0; + image->step = sensor->image().width() * sensor->image().numComponents(); + image->data.resize(image->step * image->height); + std::memcpy(&(image->data[0]), + &(sensor->image().pixels()[0]), + image->step * image->height); + + dispatch(threadPoolForPublishing, [publisher, image]{ publisher.publish(image); }); +} + + +void BodyROS2Item::updateRangeVisionSensor( + RangeCamera* sensor, + rclcpp::Publisher::SharedPtr publisher) +{ + if (!sensor->on()) { + return; + } + + auto pointCloud = std::make_shared(); + pointCloud->header.stamp = getStampMsgFromSec(io->currentTime()); + pointCloud->header.frame_id = sensor->name(); + pointCloud->width = sensor->resolutionX(); + pointCloud->height = sensor->resolutionY(); + pointCloud->is_bigendian = false; + pointCloud->is_dense = true; + pointCloud->row_step = pointCloud->point_step * pointCloud->width; + if (sensor->imageType() == cnoid::Camera::COLOR_IMAGE) { + pointCloud->fields.resize(6); + pointCloud->fields[3].name = "rgb"; + pointCloud->fields[3].offset = 12; + pointCloud->fields[3].count = 1; + pointCloud->fields[3].datatype = sensor_msgs::msg::PointField::FLOAT32; + /* + pointCloud->fields[3].name = "r"; + pointCloud->fields[3].offset = 12; + pointCloud->fields[3].datatype = sensor_msgs::PointField::UINT8; + pointCloud->fields[3].count = 1; + pointCloud->fields[4].name = "g"; + pointCloud->fields[4].offset = 13; + pointCloud->fields[4].datatype = sensor_msgs::PointField::UINT8; + pointCloud->fields[4].count = 1; + pointCloud->fields[5].name = "b"; + pointCloud->fields[5].offset = 14; + pointCloud->fields[5].datatype = sensor_msgs::PointField::UINT8; + pointCloud->fields[5].count = 1; + */ + pointCloud->point_step = 16; + } else { + pointCloud->fields.resize(3); + pointCloud->point_step = 12; + } + pointCloud->fields[0].name = "x"; + pointCloud->fields[0].offset = 0; + pointCloud->fields[0].datatype = sensor_msgs::msg::PointField::FLOAT32; + pointCloud->fields[0].count = 4; + pointCloud->fields[1].name = "y"; + pointCloud->fields[1].offset = 4; + pointCloud->fields[1].datatype = sensor_msgs::msg::PointField::FLOAT32; + pointCloud->fields[1].count = 4; + pointCloud->fields[2].name = "z"; + pointCloud->fields[2].offset = 8; + pointCloud->fields[2].datatype = sensor_msgs::msg::PointField::FLOAT32; + pointCloud->fields[2].count = 4; + const std::vector &points = sensor->constPoints(); + const unsigned char *pixels = sensor->constImage().pixels(); + pointCloud->data.resize(points.size() * pointCloud->point_step); + unsigned char *dst = (unsigned char *) &(pointCloud->data[0]); + + std::optional Ro; + if(!sensor->opticalFrameRotation().isIdentity()){ + Ro = sensor->opticalFrameRotation().cast(); + } + + for (size_t j = 0; j < points.size(); ++j) { + Vector3f point = points[j]; + + // transforms a point into the Choreonoid-default camera coordinate + // if optical frame rotation is set + if (Ro) { + // image frame = optical frame rotation (Ro) * camera frame + // => camera frame = Ro^T * image frame + point = Ro.value().transpose() * point; + } + + // converts camera coordinate from Choreonoid to ROS (URDF) + // Choreonoid: X-right, Y-upward, Z-backward + // ROS (URDF): X-right, Y-downward, Z-forward + const float x = point.x(); + const float y = - point.y(); + const float z = - point.z(); + + std::memcpy(&dst[0], &x, 4); + std::memcpy(&dst[4], &y, 4); + std::memcpy(&dst[8], &z, 4); + if (sensor->imageType() == cnoid::Camera::COLOR_IMAGE) { + dst[14] = *pixels++; + dst[13] = *pixels++; + dst[12] = *pixels++; + dst[15] = 0; + } + dst += pointCloud->point_step; + } + + dispatch(threadPoolForPublishing, [publisher, pointCloud]{ publisher->publish(*pointCloud); }); +} + + +void BodyROS2Item::updateRangeSensor( + RangeSensor* sensor, + rclcpp::Publisher::SharedPtr publisher) +{ + if (!sensor->on()) { + return; + } + + auto laserScan = std::make_shared(); + laserScan->header.stamp = getStampMsgFromSec(io->currentTime()); + laserScan->header.frame_id = sensor->name(); + laserScan->range_max = sensor->maxDistance(); + laserScan->range_min = sensor->minDistance(); + if (sensor->yawRange() == 0.0) { + laserScan->angle_max = sensor->pitchRange() / 2.0; + laserScan->angle_min = -sensor->pitchRange() / 2.0; + laserScan->angle_increment = sensor->pitchStep(); + } else { + laserScan->angle_max = sensor->yawRange() / 2.0; + laserScan->angle_min = -sensor->yawRange() / 2.0; + laserScan->angle_increment = sensor->yawStep(); + } + + const auto& rangeData = sensor->constRangeData(); + laserScan->ranges.resize(rangeData.size()); + //laserScan->intensities.resize(rangeData.size()); + for (size_t i = 0; i < rangeData.size(); ++i) { + laserScan->ranges[i] = rangeData[i]; + //laserScan->intensities[i] = -900000; + } + + dispatch(threadPoolForPublishing, [publisher, laserScan]{ publisher->publish(*laserScan); }); +} + +static inline void addPointToPointCloud +(double yawAngle, double pitchAngle, double distance, const std::optional& Ro, unsigned char* dst) +{ + double cosPitch = std::cos(pitchAngle); + Vector3f p; + p.x() = distance * cosPitch * std::sin(-yawAngle); + p.y() = distance * std::sin(pitchAngle); + p.z() = -distance * cosPitch * std::cos(yawAngle); + if(Ro){ + p = (*Ro) * p; + } + std::memcpy(&dst[0], &p.x(), 4); + std::memcpy(&dst[4], &p.y(), 4); + std::memcpy(&dst[8], &p.z(), 4); +} + + + +void BodyROS2Item::update3DRangeSensor( + RangeSensor* sensor, + rclcpp::Publisher::SharedPtr publisher) +{ + if (!sensor->on()) { + return; + } + + auto pointCloud = std::make_shared(); + constexpr int pointStep = 12; + + // Header Info + pointCloud->header.stamp = getStampMsgFromSec(io->currentTime()); + pointCloud->header.frame_id = sensor->name(); + pointCloud->height = 1; + pointCloud->fields.resize(3); + pointCloud->fields[0].name = "x"; + pointCloud->fields[0].offset = 0; + pointCloud->fields[0].datatype = sensor_msgs::msg::PointField::FLOAT32; + pointCloud->fields[0].count = 4; + pointCloud->fields[1].name = "y"; + pointCloud->fields[1].offset = 4; + pointCloud->fields[1].datatype = sensor_msgs::msg::PointField::FLOAT32; + pointCloud->fields[1].count = 4; + pointCloud->fields[2].name = "z"; + pointCloud->fields[2].offset = 8; + pointCloud->fields[2].datatype = sensor_msgs::msg::PointField::FLOAT32; + pointCloud->fields[2].count = 4; + pointCloud->is_bigendian = false; + pointCloud->point_step = pointStep; + pointCloud->is_dense = true; + + const auto& rangeData = sensor->constRangeData(); + auto& pointCloudData = pointCloud->data; + pointCloudData.resize(rangeData.size() * pointCloud->point_step); + unsigned char* dst = (unsigned char*)&(pointCloudData[0]); + + std::optional Ro; + if(!sensor->opticalFrameRotation().isIdentity()){ + Ro = sensor->opticalFrameRotation().cast(); + } + + int numPoints = 0; + double maxDistance = sensor->maxDistance(); + +#if CNOID_VERSION >= 0x020300 + for(int i=0; i < rangeData.size(); ++i){ + double distance = rangeData[i]; + if(distance <= maxDistance){ + Vector2 angles = sensor->getSphericalAngle(i); + double yawAngle = angles[0]; + double pitchAngle = angles[1]; + addPointToPointCloud(yawAngle, pitchAngle, distance, Ro, dst); + dst += pointStep; + ++numPoints; + } + } +#else + const int numPitchSamples = sensor->numPitchSamples(); + const double minPitchAngle = -sensor->pitchRange() / 2.0; + const double pitchStep = sensor->pitchStep(); + const int numYawSamples = sensor->numYawSamples(); + const double minYawAngle = -sensor->yawRange() / 2.0; + const double yawStep = sensor->yawStep(); + + for(int pitchIndex = 0; pitchIndex < numPitchSamples; ++pitchIndex){ + const double pitchAngle = pitchIndex * pitchStep + minPitchAngle; + const double cosPitchAngle = cos(pitchAngle); + const double sinPitchAngle = sin(pitchAngle); + const int pitchTopIndex = pitchIndex * numYawSamples; + + for (int yawIndex = 0; yawIndex < numYawSamples; ++yawIndex) { + const double distance = rangeData[pitchTopIndex + yawIndex]; + if(distance <= maxDistance){ + const double yawAngle = yawIndex * yawStep + minYawAngle; + addPointToPointCloud(yawAngle, pitchAngle, distance, Ro, dst); + dst += pointStep; + ++numPoints; + } + } + } +#endif + + pointCloudData.resize(numPoints * pointStep); + pointCloud->width = numPoints; + pointCloud->row_step = pointCloudData.size(); + + dispatch(threadPoolForPublishing, [publisher, pointCloud]{ publisher->publish(*pointCloud); }); +} + + +void BodyROS2Item::switchDevice( + std_srvs::srv::SetBool::Request::ConstSharedPtr request, + std_srvs::srv::SetBool::Response::SharedPtr response, + Device* sensor) +{ + sensor->on(request->data); + response->success = (request->data == sensor->on()); +} + + +builtin_interfaces::msg::Time BodyROS2Item::getStampMsgFromSec(double sec) +{ + builtin_interfaces::msg::Time msg; + msg.sec = int(sec); + msg.nanosec = (sec - int(sec)) * 1000000000; + return msg; +} + + +std::string BodyROS2Item::getROS2Name(const std::string &name) const { + std::string rosName = std::string(rosNode->get_fully_qualified_name()) + "/" + name; + std::replace(rosName.begin(), rosName.end(), '-', '_'); + return rosName; +} diff --git a/src/plugin/BodyROS2Item.h b/src/plugin/BodyROS2Item.h new file mode 100644 index 0000000..c975025 --- /dev/null +++ b/src/plugin/BodyROS2Item.h @@ -0,0 +1,189 @@ +#ifndef CNOID_ROS_PLUGIN_BODY_ROS2_ITEM_H +#define CNOID_ROS_PLUGIN_BODY_ROS2_ITEM_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include + +#include "exportdecl.h" + +namespace cnoid { + +class CNOID_EXPORT BodyROS2Item : public ControllerItem +{ +public: + static void initializeClass(ExtensionManager *ext); + + BodyROS2Item(); + BodyROS2Item(const BodyROS2Item &org); + virtual ~BodyROS2Item(); + void createSensors(BodyPtr body); + + virtual bool initialize(ControllerIO *io) override; + virtual double timeStep() const override; + virtual bool start() override; + virtual bool control() override; + + const Body *body() const { return simulationBody; }; + const DeviceList &forceSensors() const + { + return forceSensors_; + } + const DeviceList &gyroSensors() const + { + return gyroSensors_; + } + const DeviceList &accelSensors() const + { + return accelSensors_; + } + const DeviceList& imus() const { return imus_; } + const DeviceList &visionSensors() const { return visionSensors_; } + const DeviceList &rangeVisionSensors() const + { + return rangeVisionSensors_; + } + const DeviceList &rangeSensors() const + { + return rangeSensors_; + } + + double controlTime() const { return controlTime_; } + + void setModuleName(const std::string &name); + +protected: + virtual Item *doDuplicate() const override; + virtual void onTreePathChanged() override; + virtual bool store(Archive &archive) override; + virtual bool restore(const Archive &archive) override; + virtual void doPutProperties(PutPropertyFunction &putProperty) override; + +private: + BodyItem* bodyItem; + + BodyPtr simulationBody; + DeviceList forceSensors_; + DeviceList gyroSensors_; + DeviceList accelSensors_; + DeviceList imus_; + DeviceList visionSensors_; + DeviceList rangeVisionSensors_; + DeviceList rangeSensors_; + double timeStep_; + + /* joint states */ + sensor_msgs::msg::JointState jointState; + std::shared_ptr> + jointStatePublisher; + double jointStateUpdateRate; + double jointStateUpdatePeriod; + double jointStateLastUpdate; + + ControllerIO *io; + double controlTime_; + std::ostream &os; + + rclcpp::Node::SharedPtr rosNode; + std::unique_ptr executor; + std::thread executorThread; + + ThreadPool threadPoolForPublishing; + + std::string bodyName; + + std::vector::SharedPtr> + forceSensorPublishers; + std::vector::SharedPtr> + rateGyroSensorPublishers; + std::vector::SharedPtr> + accelSensorPublishers; + std::vector::SharedPtr> + imuPublishers; + + std::shared_ptr imageTransport; + std::vector visionSensorPublishers; + + std::vector::SharedPtr> + rangeVisionSensorPublishers; + std::vector::SharedPtr> + rangeSensorPublishers; + std::vector::SharedPtr> + rangeSensorPcPublishers; + + std::vector> forceSensorSwitchServers; + std::vector> rateGyroSensorSwitchServers; + std::vector> accelSensorSwitchServers; + std::vector> imuSwitchServers; + std::vector> visionSensorSwitchServers; + std::vector> + rangeVisionSensorSwitchServers; + std::vector> rangeSensorSwitchServers; + std::vector> rangeSensorPcSwitchServers; + + void initializeRosNode(BodyItem* bodyItem); + void finalizeRosNode(); + + void updateForceSensor( + ForceSensor* sensor, + rclcpp::Publisher::SharedPtr publisher); + void updateRateGyroSensor( + RateGyroSensor* sensor, + rclcpp::Publisher::SharedPtr publisher); + void updateAccelSensor( + AccelerationSensor* sensor, + rclcpp::Publisher::SharedPtr publisher); + void updateImu( + Imu *sensor, + rclcpp::Publisher::SharedPtr publisher); + void updateVisionSensor( + Camera* sensor, + image_transport::Publisher publisher); + void updateRangeVisionSensor( + RangeCamera* sensor, + rclcpp::Publisher::SharedPtr publisher); + void updateRangeSensor( + RangeSensor* sensor, + rclcpp::Publisher::SharedPtr publisher); + void update3DRangeSensor( + RangeSensor* sensor, + rclcpp::Publisher::SharedPtr publisher); + + void switchDevice(std_srvs::srv::SetBool::Request::ConstSharedPtr request, + std_srvs::srv::SetBool::Response::SharedPtr response, + Device* sensor); + builtin_interfaces::msg::Time getStampMsgFromSec(double sec); + + std::string getROS2Name(const std::string &name) const; +}; + +typedef ref_ptr BodyROS2ItemPtr; +} // namespace cnoid + +#endif diff --git a/src/plugin/BodyROSItem.cpp b/src/plugin/BodyROSItem.cpp index e1ed394..8654062 100644 --- a/src/plugin/BodyROSItem.cpp +++ b/src/plugin/BodyROSItem.cpp @@ -1,4 +1,5 @@ #include "BodyROSItem.h" +#include "Format.h" #include #include #include @@ -21,13 +22,11 @@ typedef sensor_msgs::PointCloud PointCloudTypeForRangeSensor; typedef sensor_msgs::PointCloud2 PointCloudTypeForRangeSensor; #endif -#include #include #include #include "gettext.h" using namespace cnoid; -using fmt::format; void BodyROSItem::initializeClass(ExtensionManager* ext) @@ -41,6 +40,7 @@ BodyROSItem::BodyROSItem() : os(MessageView::instance()->cout()) { io = nullptr; + jointStatePublication = true; jointStateUpdateRate = 100.0; } @@ -50,6 +50,7 @@ BodyROSItem::BodyROSItem(const BodyROSItem& org) , os(MessageView::instance()->cout()) { io = nullptr; + jointStatePublication = true; jointStateUpdateRate = 100.0; } @@ -69,6 +70,7 @@ Item* BodyROSItem::doDuplicate() const bool BodyROSItem::store(Archive& archive) { archive.write("body_ros_version", 0); + archive.write("joint_state_publication", jointStatePublication); archive.write("joint_state_update_rate", jointStateUpdateRate); return true; @@ -77,14 +79,17 @@ bool BodyROSItem::store(Archive& archive) bool BodyROSItem::restore(const Archive& archive) { + archive.read({ "joint_state_publication", "jointStatePublication" }, jointStatePublication); archive.read({ "joint_state_update_rate", "jointStateUpdateRate" }, jointStateUpdateRate); + return true; } void BodyROSItem::doPutProperties(PutPropertyFunction& putProperty) { - putProperty.decimals(2).min(0.0)("Update rate", jointStateUpdateRate, changeProperty(jointStateUpdateRate)); + putProperty("JointState Publication", jointStatePublication, changeProperty(jointStatePublication)); + putProperty.decimals(2).min(0.0)("JointState Update rate", jointStateUpdateRate, changeProperty(jointStateUpdateRate)); } @@ -92,7 +97,7 @@ bool BodyROSItem::initialize(ControllerIO* io) { if (!io->body()) { MessageView::instance()->putln( - format(_("BodyROSItem \"{0}\" is invalid because it is not assigned to a body."), displayName()), + formatR(_("BodyROSItem \"{0}\" is invalid because it is not assigned to a body."), displayName()), MessageView::WARNING); return false; } @@ -130,8 +135,15 @@ bool BodyROSItem::start() rosNode.reset(new ros::NodeHandle(name)); createSensors(simulationBody); + if (!jointStatePublication) { + return true; + } jointStatePublisher = rosNode->advertise("joint_states", 1000); - jointStateUpdatePeriod = 1.0 / jointStateUpdateRate; + if (jointStateUpdateRate == 0.0) { + jointStateUpdatePeriod = 0.0; + } else { + jointStateUpdatePeriod = 1.0 / jointStateUpdateRate; + } jointStateLastUpdate = io->currentTime(); ROS_DEBUG("Joint state update rate %f", jointStateUpdateRate); @@ -232,10 +244,10 @@ void BodyROSItem::createSensors(BodyPtr body) sensor->sigStateChanged().connect([this, sensor, publisher]() { updateImu(sensor, publisher); }); - accelSensorPublishers.push_back(publisher); + imuPublishers.push_back(publisher); boost::function requestCallback = boost::bind(&BodyROSItem::switchDevice, this, _1, _2, sensor); - accelSensorSwitchServers.push_back( + imuSwitchServers.push_back( rosNode->advertiseService(name + "/set_enabled", requestCallback)); ROS_INFO("Create IMU %s", sensor->name().c_str()); } @@ -344,7 +356,7 @@ bool BodyROSItem::control() controlTime_ = io->currentTime(); double updateSince = controlTime_ - jointStateLastUpdate; - if (updateSince > jointStateUpdatePeriod) { + if (jointStatePublication && (updateSince > jointStateUpdatePeriod)) { // publish current joint states joint_state_.header.stamp.fromSec(controlTime_); diff --git a/src/plugin/BodyROSItem.h b/src/plugin/BodyROSItem.h index 796ea34..745b0a5 100644 --- a/src/plugin/BodyROSItem.h +++ b/src/plugin/BodyROSItem.h @@ -78,10 +78,13 @@ class CNOID_EXPORT BodyROSItem : public ControllerItem DeviceList rangeSensors_; double timeStep_; + /* properties */ + bool jointStatePublication; + double jointStateUpdateRate; + /* joint states */ sensor_msgs::JointState joint_state_; ros::Publisher jointStatePublisher; - double jointStateUpdateRate; double jointStateUpdatePeriod; double jointStateLastUpdate; diff --git a/src/plugin/CMakeLists.txt b/src/plugin/CMakeLists.txt index d056657..9228202 100644 --- a/src/plugin/CMakeLists.txt +++ b/src/plugin/CMakeLists.txt @@ -2,43 +2,72 @@ set(target CnoidROSPlugin) choreonoid_make_gettext_mo_files(${target} mofiles) -choreonoid_add_plugin(${target} - ROSPlugin.cpp - WorldROSItem.cpp - BodyROSItem.cpp - ROSControlItem.cpp - RobotHWSim.h - deprecated/BodyPublisherItem.cpp - ${mofiles} - ) - -target_link_libraries(${target} - PUBLIC - ${roscpp_LIBRARIES} - ${std_msgs_LIBRARIES} - ${sensor_msgs_LIBRARIES} - ${image_transport_LIBRARIES} - ${catkin_LIBRARIES} - Choreonoid::CnoidBodyPlugin - ) - -option(USE_POINTCLOUD1_IN_BODY_ROS_ITEM "Use the PointCloud type instead of PointCoud2 for publishing range sensor data" OFF) -mark_as_advanced(USE_POINTCLOUD1_IN_BODY_ROS_ITEM) - -if(USE_POINTCLOUD1_IN_BODY_ROS_ITEM) - set_property(DIRECTORY APPEND PROPERTY COMPILE_DEFINITIONS CNOID_ROS_PLUGIN_USE_POINTCLOUD1) -endif() +if(DEFINED ENV{ROS_VERSION}) + if($ENV{ROS_VERSION} EQUAL 1) + choreonoid_add_plugin(${target} + ROSPlugin.cpp + WorldROSItem.cpp + BodyROSItem.cpp + ROSControlItem.cpp + RobotHWSim.h + deprecated/BodyPublisherItem.cpp + ${mofiles} + ) + + target_link_libraries(${target} + PUBLIC + ${roscpp_LIBRARIES} + ${std_msgs_LIBRARIES} + ${sensor_msgs_LIBRARIES} + ${image_transport_LIBRARIES} + ${catkin_LIBRARIES} + Choreonoid::CnoidBodyPlugin + ) + + # PointCloud(1) is deprecated in ROS 2, but it is still used in ROS 1 + option(USE_POINTCLOUD1_IN_BODY_ROS_ITEM "Use the PointCloud type instead of PointCloud2 for publishing range sensor data with ROS 1" OFF) + mark_as_advanced(USE_POINTCLOUD1_IN_BODY_ROS_ITEM) + + if(USE_POINTCLOUD1_IN_BODY_ROS_ITEM) + set_property(DIRECTORY APPEND PROPERTY COMPILE_DEFINITIONS CNOID_ROS_PLUGIN_USE_POINTCLOUD1) + endif() + + # RobotHW inherited ros_control Choreonoid interface class + add_library(RobotHWCnoid RobotHWSim.h RobotHWCnoid.cpp) + target_link_libraries(RobotHWCnoid ${catkin_LIBRARIES} Choreonoid::CnoidBody Choreonoid::CnoidBase) + + install(TARGETS RobotHWCnoid + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} + ) + + if(CHOREONOID_ENABLE_PYTHON) + add_subdirectory(pybind11) + endif() + elseif($ENV{ROS_VERSION} EQUAL 2) + choreonoid_add_plugin(${target} + ROS2Plugin.cpp + WorldROS2Item.cpp + BodyROS2Item.cpp + ${mofiles} + ) + + ament_target_dependencies(${target} PUBLIC + "rclcpp" + "std_msgs" + "std_srvs" + "sensor_msgs" + "image_transport" + ) -# RobotHW inherited ros_control Choreonoid interface class -add_library(RobotHWCnoid RobotHWSim.h RobotHWCnoid.cpp) -target_link_libraries(RobotHWCnoid ${catkin_LIBRARIES} Choreonoid::CnoidBody Choreonoid::CnoidBase) + target_link_libraries(${target} PUBLIC + Choreonoid::CnoidBodyPlugin + ) -install(TARGETS RobotHWCnoid - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} - ) + if(CHOREONOID_ENABLE_PYTHON) + # add_subdirectory(pybind11) + endif () -if(CHOREONOID_ENABLE_PYTHON) - add_subdirectory(pybind11) + endif() endif() diff --git a/src/plugin/Format.h b/src/plugin/Format.h new file mode 100644 index 0000000..af183b5 --- /dev/null +++ b/src/plugin/Format.h @@ -0,0 +1,75 @@ +#ifndef CNOID_ROS_PLUGIN_FORMAT_H +#define CNOID_ROS_PLUGIN_FORMAT_H + +#include + +#if CNOID_VERSION >= 0x020200 +#include + +#else + +#include + +namespace cnoid { + +#if __cplusplus >= 202002L && FMT_VERSION >= 80000 + +template +std::string formatC(fmt::format_string s, Args&&... args) +{ + return fmt::vformat(s, fmt::make_format_args(args...)); +} + +template +std::string formatR(fmt::string_view s, Args&&... args) +{ + return fmt::format(fmt::runtime(s), args...); +} + +#elif __cplusplus >= 201703L + +template +std::string formatC(fmt::string_view s, Args&&... args) +{ + return fmt::format(s, args...); +} + +template +std::string formatR(fmt::string_view s, Args&&... args) +{ + return fmt::format(s, args...); +} + +#else + +template +std::string formatC(const char* s, Args&&... args) +{ + return fmt::format(s, args...); +} + +template +std::string formatC(const std::string& s, Args&&... args) +{ + return fmt::format(s, args...); +} + +template +std::string formatR(const char* s, Args&&... args) +{ + return fmt::format(s, args...); +} + +template +std::string formatR(const std::string& s, Args&&... args) +{ + return fmt::format(s, args...); +} + +#endif + +} // name space cnoid + +#endif // CNOID_VERSION < 0x020200 + +#endif // CNOID_UTIL_FORMAT_H diff --git a/src/plugin/ROS2Plugin.cpp b/src/plugin/ROS2Plugin.cpp new file mode 100644 index 0000000..fcca956 --- /dev/null +++ b/src/plugin/ROS2Plugin.cpp @@ -0,0 +1,30 @@ +#include "ROS2Plugin.hpp" +#include "BodyROS2Item.h" +#include "WorldROS2Item.h" +#include "deprecated/BodyPublisherItem.h" +#include + + +using namespace std; +using namespace cnoid; + +ROS2Plugin::ROS2Plugin() + : Plugin("ROS2") +{ + require("Body"); +} + +bool ROS2Plugin::initialize() +{ + WorldROS2Item::initializeClass(this); + BodyROS2Item::initializeClass(this); + + return true; +} + +bool ROS2Plugin::customizeApplication(AppCustomizationUtil& /* app */) +{ + return true; +} + +CNOID_IMPLEMENT_PLUGIN_ENTRY(ROS2Plugin) diff --git a/src/plugin/ROS2Plugin.hpp b/src/plugin/ROS2Plugin.hpp new file mode 100644 index 0000000..e64160d --- /dev/null +++ b/src/plugin/ROS2Plugin.hpp @@ -0,0 +1,15 @@ +#ifndef CNOID_ROS_PLUGIN_ROS2_PLUGIN_HPP_ +#define CNOID_ROS_PLUGIN_ROS2_PLUGIN_HPP_ + +#include +#include + +class ROS2Plugin : public cnoid::Plugin +{ +public: + ROS2Plugin(); + virtual bool customizeApplication(cnoid::AppCustomizationUtil& app) override; + virtual bool initialize() override; +}; + +#endif // CNOID_ROS_PLUGIN_ROS2_PLUGIN_HPP_ diff --git a/src/plugin/ROSControlItem.cpp b/src/plugin/ROSControlItem.cpp index da34c5e..b142177 100644 --- a/src/plugin/ROSControlItem.cpp +++ b/src/plugin/ROSControlItem.cpp @@ -3,17 +3,16 @@ ///////////////////////////////// #include "ROSControlItem.h" +#include "Format.h" #include #include #include #include -#include #include "gettext.h" using namespace std; using namespace cnoid; using namespace hardware_interface; -using fmt::format; void ROSControlItem::initializeClass(ExtensionManager* ext) @@ -79,8 +78,8 @@ bool ROSControlItem::initialize(ControllerIO* io) // Check body // if(!io->body()){ mv->putln( - format(_("ROSControlItem \"{0}\" is invalid because it is not assigned to a body."), - displayName()), + formatR(_("ROSControlItem \"{0}\" is invalid because it is not assigned to a body."), + displayName()), MessageView::Error); return false; } @@ -113,7 +112,7 @@ bool ROSControlItem::initialize(ControllerIO* io) // load hardware_interface // if(!robotHWSim->initSim(nodeHandle, io)){ mv->putln( - format(_("The hardware interface of {0} cannot be initialized."), displayName()), + formatR(_("The hardware interface of {0} cannot be initialized."), displayName()), MessageView::Error); } } @@ -125,12 +124,12 @@ bool ROSControlItem::initialize(ControllerIO* io) } catch(pluginlib::LibraryLoadException& ex){ mv->putln( - format(_("Failed to create robot simulation interface loader : {0}"), ex.what()), + formatR(_("Failed to create robot simulation interface loader : {0}"), ex.what()), MessageView::Error); return false; } - mv->putln(format(_("{0} has successfully been initialized"), displayName())); + mv->putln(formatR(_("{0} has successfully been initialized"), displayName())); return true; } diff --git a/src/plugin/RobotHWCnoid.cpp b/src/plugin/RobotHWCnoid.cpp index 46476fd..731b4cc 100644 --- a/src/plugin/RobotHWCnoid.cpp +++ b/src/plugin/RobotHWCnoid.cpp @@ -3,6 +3,7 @@ ///////////////////////////////// #include "RobotHWCnoid.h" +#include "Format.h" #include #include #include @@ -11,14 +12,12 @@ #include #include #include -#include #include #include #include "gettext.h" using namespace std; using namespace cnoid; -using fmt::format; namespace hardware_interface { @@ -72,16 +71,16 @@ bool RobotHWCnoid::initSim(const ros::NodeHandle& nh, cnoid::ControllerIO* args) } if(jointInterfaces.empty()){ mv->putln( - format(_("{0} on {1} does not specify any hardware interface."), - transmission[i].name_, transmission[i].joints_[0].name_), + formatR(_("{0} on {1} does not specify any hardware interface."), + transmission[i].name_, transmission[i].joints_[0].name_), MessageView::Warning); continue; } else if(jointInterfaces.size() > 1){ mv->putln( - format(_("{0} of transmission specifies multiple hardware interfaces. Currently the default " - "robot hardware simulation interface only supports one. Using the first entry"), - transmission[i].joints_[0].name_), + formatR(_("{0} of transmission specifies multiple hardware interfaces. Currently the default " + "robot hardware simulation interface only supports one. Using the first entry"), + transmission[i].joints_[0].name_), MessageView::Warning); } @@ -92,8 +91,8 @@ bool RobotHWCnoid::initSim(const ros::NodeHandle& nh, cnoid::ControllerIO* args) Link* link = io->body()->joint(jointNames[i].c_str()); if(!link){ mv->putln( - format(_("This robot has a joint named \"{0}\" which is not in the choreonoid model."), - jointNames[i]), + formatR(_("This robot has a joint named \"{0}\" which is not in the choreonoid model."), + jointNames[i]), MessageView::Error); return false; } @@ -182,13 +181,13 @@ bool RobotHWCnoid::loadURDF(const std::string& paramName) if(nodeHandle.searchParam(paramName, searchedParam)){ if(!nodeHandle.getParam(searchedParam, urdfString)){ mv->putln( - format(_("Waiting for URDF model.\nFinding parameter name : {0}"), - searchedParam), + formatR(_("Waiting for URDF model.\nFinding parameter name : {0}"), + searchedParam), MessageView::Warning); sleep(1); } } else { - mv->putln(format(_("There are no parameter related to : {0}"), paramName), + mv->putln(formatR(_("There are no parameter related to : {0}"), paramName), MessageView::Error); return false; } diff --git a/src/plugin/WorldROS2Item.cpp b/src/plugin/WorldROS2Item.cpp new file mode 100644 index 0000000..7bd2511 --- /dev/null +++ b/src/plugin/WorldROS2Item.cpp @@ -0,0 +1,223 @@ +/** + \note Alghough multiple instances of WorldROSItem can be created, + only one /clock topic can exist in a system. The current implementation + does not consider this limitation and the clock value may be inconsistent + when multiple WorldROSItem publish it simultaneously. The implementation + should be improved to avoid this problem. +*/ + +#include "WorldROS2Item.h" +#include "gettext.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace cnoid; + +namespace cnoid { + +class WorldROS2Item::Impl +{ +public: + WorldROS2Item *self; + WorldItem *worldItem; + ScopedConnection simulationBarConnection; + SimulatorItem *currentSimulatorItem; + ScopedConnectionSet currentSimulatorItemConnections; + + rclcpp::Node::SharedPtr rosNode; + + rclcpp::Publisher::SharedPtr clockPublisher; + double maxClockPublishingRate; + double clockPublishingInterval; + double nextClockPublishingTime; + + Impl(WorldROS2Item *self); + Impl(WorldROS2Item *self, const Impl &org); + ~Impl(); + void initialize(); + void initializeWorld(WorldItem *worldItem); + void initializeClockPublisher(); + void onSimulationAboutToStart(SimulatorItem *simulatorItem); + void setCurrentSimulatorItem(SimulatorItem *simulatorItem); + void clearCurrentSimulatorItem(); + void onSimulationStarted(); + void onSimulationStep(); + void onSimulationFinished(); +}; + +} // namespace cnoid + +void WorldROS2Item::initializeClass(ExtensionManager *ext) +{ + ext->itemManager().registerClass(N_("WorldROS2Item")); + ext->itemManager().addCreationPanel(); +} + +WorldROS2Item::WorldROS2Item() +{ + impl = new Impl(this); +} + +WorldROS2Item::Impl::Impl(WorldROS2Item *self) + : self(self) +{ + maxClockPublishingRate = 100.0; + initialize(); +} + +WorldROS2Item::WorldROS2Item(const WorldROS2Item &org) + : Item(org) +{ + impl = new Impl(this, *org.impl); +} + +WorldROS2Item::Impl::Impl(WorldROS2Item *self, const Impl &org) + : self(self) +{ + maxClockPublishingRate = org.maxClockPublishingRate; + initialize(); +} + +void WorldROS2Item::Impl::initialize() +{ + simulationBarConnection = SimulationBar::instance() + ->sigSimulationAboutToStart() + .connect([&](SimulatorItem *simulatorItem) { + onSimulationAboutToStart(simulatorItem); + }); +} + +WorldROS2Item::~WorldROS2Item() +{ + delete impl; +} + +WorldROS2Item::Impl::~Impl() +{ + clearCurrentSimulatorItem(); +} + +Item *WorldROS2Item::doDuplicate() const +{ + return new WorldROS2Item(*this); +} + +void WorldROS2Item::onTreePathChanged() +{ + WorldItem *worldItem = nullptr; + if (isConnectedToRoot()) { + worldItem = findOwnerItem(); + if (worldItem && worldItem != impl->worldItem) { + impl->initializeWorld(worldItem); + } + } +} + +void WorldROS2Item::Impl::initializeWorld(WorldItem *worldItem) +{ + this->worldItem = worldItem; + + string name = worldItem->name(); + std::replace(name.begin(), name.end(), '-', '_'); + rosNode = std::make_shared(name); + + initializeClockPublisher(); +} + +void WorldROS2Item::Impl::initializeClockPublisher() +{ + clockPublisher.reset(); + if (rosNode && maxClockPublishingRate > 0.0) { + clockPublisher = rosNode->create_publisher("/clock", 1); + } +} + +void WorldROS2Item::setMaxClockPublishingRate(double rate) +{ + impl->maxClockPublishingRate = rate; + impl->initializeClockPublisher(); +} + +void WorldROS2Item::Impl::onSimulationAboutToStart(SimulatorItem *simulatorItem) +{ + if (worldItem && worldItem == simulatorItem->findOwnerItem()) { + if (simulatorItem != currentSimulatorItem) { + setCurrentSimulatorItem(simulatorItem); + return; + } + } + clearCurrentSimulatorItem(); +} + +void WorldROS2Item::Impl::setCurrentSimulatorItem(SimulatorItem *simulatorItem) +{ + clearCurrentSimulatorItem(); + currentSimulatorItem = simulatorItem; + + currentSimulatorItemConnections.add( + simulatorItem->sigSimulationStarted().connect( + [&]() { onSimulationStarted(); })); + + currentSimulatorItemConnections.add( + simulatorItem->sigSimulationFinished().connect( + [&](bool /* isForced */) { clearCurrentSimulatorItem(); })); +} + +void WorldROS2Item::Impl::clearCurrentSimulatorItem() +{ + currentSimulatorItemConnections.disconnect(); + currentSimulatorItem = nullptr; +} + +void WorldROS2Item::Impl::onSimulationStarted() +{ + clockPublishingInterval = 1.0 / maxClockPublishingRate; + nextClockPublishingTime = 0.0; + + currentSimulatorItem->addMidDynamicsFunction([&]() { onSimulationStep(); }); +} + +void WorldROS2Item::Impl::onSimulationStep() +{ + double time = currentSimulatorItem->simulationTime(); + + // Publish clock + if (time >= nextClockPublishingTime) { + rosgraph_msgs::msg::Clock clock; + clock.clock.set__sec(static_cast(time)); + int nanosec = (time - clock.clock.sec) * 10e6; + clock.clock.set__nanosec(nanosec); + clockPublisher->publish(clock); + nextClockPublishingTime += clockPublishingInterval; + } +} + +void WorldROS2Item::doPutProperties(PutPropertyFunction &putProperty) +{ + putProperty.decimals(2).min(0.0)(_("Max clock publishing rate"), + impl->maxClockPublishingRate, + [&](double r) { + setMaxClockPublishingRate(r); + return true; + }); +} + +bool WorldROS2Item::store(Archive &archive) +{ + archive.write("max_clock_publishing_rate", impl->maxClockPublishingRate); + return true; +} + +bool WorldROS2Item::restore(const Archive &archive) +{ + archive.read("max_clock_publishing_rate", impl->maxClockPublishingRate); + return true; +} diff --git a/src/plugin/WorldROS2Item.h b/src/plugin/WorldROS2Item.h new file mode 100644 index 0000000..7b13683 --- /dev/null +++ b/src/plugin/WorldROS2Item.h @@ -0,0 +1,35 @@ +#ifndef CNOID_ROS_PLUGIN_WORLD_ROS2_ITEM_H +#define CNOID_ROS_PLUGIN_WORLD_ROS2_ITEM_H + +#include "exportdecl.h" +#include + +namespace cnoid { + +class CNOID_EXPORT WorldROS2Item : public Item +{ +public: + static void initializeClass(ExtensionManager* ext); + + WorldROS2Item(); + WorldROS2Item(const WorldROS2Item& org); + virtual ~WorldROS2Item(); + void setMaxClockPublishingRate(double rate); + +protected: + virtual Item* doDuplicate() const override; + virtual void onTreePathChanged() override; + virtual void doPutProperties(PutPropertyFunction& putProperty) override; + virtual bool store(Archive& archive) override; + virtual bool restore(const Archive& archive) override; + +private: + class Impl; + Impl* impl; +}; + +typedef ref_ptr WorldROSItemPtr; + +} // namespace cnoid + +#endif