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..d6030f9 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 + ) + 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,19 @@ 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(DIRECTORY launch - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} - USE_SOURCE_PERMISSIONS - ) + install(DIRECTORY launch + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + USE_SOURCE_PERMISSIONS + ) + elseif($ENV{ROS_VERSION} EQUAL 2) + ament_package() + 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/package.xml b/package.xml index fff9266..38b51fa 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,29 @@ http://choreonoid.org https://github.com/choreonoid/choreonoid_ros.git - catkin + catkin + ament_cmake choreonoid - roscpp + + roscpp + rclcpp + std_msgs + std_srvs sensor_msgs image_transport - pluginlib - angles - controller_manager - hardware_interface - joint_limits_interface - transmission_interface - urdf + + pluginlib + angles + controller_manager + hardware_interface + joint_limits_interface + transmission_interface + urdf - cmake - + cmake + ament_cmake + diff --git a/src/node/CMakeLists.txt b/src/node/CMakeLists.txt index 1e4b70f..90b3585 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 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..3b710dc --- /dev/null +++ b/src/node/choreonoid_ros2.cpp @@ -0,0 +1,28 @@ +#include +#include +#include +#include +#include +#include +#include +#include + +int main(int argc, char** argv) +{ + cnoid::App app(argc, argv, "Choreonoid-ROS2", "Choreonoid"); + + auto plugin_manager = cnoid::PluginManager::instance(); + + if(auto pluginPath = getenv("CNOID_PLUGIN_PATH")){ + plugin_manager->addPluginPath(cnoid::toUTF8(pluginPath)); + } + + auto plugin_path = ament_index_cpp::get_package_share_directory("choreonoid_ros") + "/../../lib/choreonoid_ros"; + plugin_manager->addPluginPath(plugin_path); + + app.requirePluginToCustomizeApplication("ROS2"); + + int ret = app.exec(); + + return ret; +} diff --git a/src/plugin/BodyROS2Item.cpp b/src/plugin/BodyROS2Item.cpp new file mode 100644 index 0000000..daa3c0b --- /dev/null +++ b/src/plugin/BodyROS2Item.cpp @@ -0,0 +1,702 @@ +#include "BodyROS2Item.h" +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "gettext.h" +#include + +using namespace cnoid; +using fmt::format; +using std::placeholders::_1; +using std::placeholders::_2; + +void BodyROS2Item::initializeClass(ExtensionManager *ext) +{ + ext->itemManager().registerClass(N_("BodyROS2Item")); + ext->itemManager().addCreationPanel(); +} + + +BodyROS2Item::BodyROS2Item() + : os(MessageView::instance()->cout()) +{ + io = nullptr; + jointStateUpdateRate = 100.0; +} + + +BodyROS2Item::BodyROS2Item(const BodyROS2Item &org) + : ControllerItem(org) + , os(MessageView::instance()->cout()) +{ + 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)); +} + + +bool BodyROS2Item::initialize(ControllerIO *io) +{ + if (!io->body()) { + MessageView::instance() + ->putln(format(_("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; +} + + +bool BodyROS2Item::start() +{ + rosContext = std::make_shared(); + rosContext->init(0, nullptr); + std::string name = simulationBody->name(); + std::replace(name.begin(), name.end(), '-', '_'); + rosNode = std::make_unique(name, rclcpp::NodeOptions().context(rosContext)); + + 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->name(); + jointState.position[i] = joint->q(); + jointState.velocity[i] = joint->dq(); + jointState.effort[i] = joint->u(); + } + + createSensors(simulationBody); + + jointStatePublisher + = rosNode->create_publisher(getROS2Name("joint_states"), + 1000); + 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()); + 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()); + } + + visionSensorPublishers.clear(); + visionSensorPublishers.reserve(visionSensors_.size()); + visionSensorSwitchServers.clear(); + visionSensorSwitchServers.reserve(visionSensors_.size()); + for (CameraPtr sensor : visionSensors_) { + std::string name = getROS2Name(sensor->name()); + visionSensorPublishers.push_back(imageTransport->advertise(name, 1)); + auto & publisher = visionSensorPublishers.back(); + 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->numPitchSamples() > 1) { + 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; + } + + // spin some with custom context + { + rclcpp::ExecutorOptions options; + options.context = rosContext; + rclcpp::executors::SingleThreadedExecutor executor(options); + executor.add_node(rosNode, false); + executor.spin_some(); + executor.remove_node(rosNode, false); + } + + return true; +} + + +void BodyROS2Item::updateForceSensor( + ForceSensor *sensor, + rclcpp::Publisher::SharedPtr publisher) +{ + if (!sensor->on()) { + return; + } + geometry_msgs::msg::WrenchStamped force; + force.header.stamp = getStampMsgFromSec(io->currentTime()); + force.header.frame_id = sensor->name(); + force.wrench.force.x = sensor->F()[0] / 1000.0; + force.wrench.force.y = sensor->F()[1] / 1000.0; + force.wrench.force.z = sensor->F()[2] / 1000.0; + force.wrench.torque.x = sensor->F()[3] / 1000.0; + force.wrench.torque.y = sensor->F()[4] / 1000.0; + force.wrench.torque.z = sensor->F()[5] / 1000.0; + publisher->publish(force); +} + + +void BodyROS2Item::updateRateGyroSensor( + RateGyroSensor *sensor, + rclcpp::Publisher::SharedPtr publisher) +{ + if (!sensor->on()) { + return; + } + sensor_msgs::msg::Imu gyro; + gyro.header.stamp = getStampMsgFromSec(io->currentTime()); + gyro.header.frame_id = sensor->name(); + gyro.angular_velocity.x = sensor->w()[0]; + gyro.angular_velocity.y = sensor->w()[1]; + gyro.angular_velocity.z = sensor->w()[2]; + publisher->publish(gyro); +} + + +void BodyROS2Item::updateAccelSensor( + AccelerationSensor *sensor, + rclcpp::Publisher::SharedPtr publisher) +{ + if (!sensor->on()) { + return; + } + sensor_msgs::msg::Imu accel; + accel.header.stamp = getStampMsgFromSec(io->currentTime()); + accel.header.frame_id = sensor->name(); + accel.linear_acceleration.x = sensor->dv()[0] / 10.0; + accel.linear_acceleration.y = sensor->dv()[1] / 10.0; + accel.linear_acceleration.z = sensor->dv()[2] / 10.0; + publisher->publish(accel); +} + + +void BodyROS2Item::updateVisionSensor( + Camera *sensor, + image_transport::Publisher & publisher) +{ + if (!sensor->on()) { + return; + } + + sensor_msgs::msg::Image vision; + vision.header.stamp = getStampMsgFromSec(io->currentTime()); + vision.header.frame_id = sensor->name(); + vision.height = sensor->image().height(); + vision.width = sensor->image().width(); + if (sensor->image().numComponents() == 3) + vision.encoding = "rgb8"; + else if (sensor->image().numComponents() == 1) + vision.encoding = "mono8"; + else { + RCLCPP_WARN(rosNode->get_logger(), + "unsupported image component number: %i", + sensor->image().numComponents()); + } + vision.is_bigendian = 0; + vision.step = sensor->image().width() * sensor->image().numComponents(); + vision.data.resize(vision.step * vision.height); + std::memcpy(&(vision.data[0]), + &(sensor->image().pixels()[0]), + vision.step * vision.height); + publisher.publish(vision); +} + + +void BodyROS2Item::updateRangeVisionSensor( + RangeCamera *sensor, + rclcpp::Publisher::SharedPtr publisher) +{ + if (!sensor->on()) { + return; + } + sensor_msgs::msg::PointCloud2 range; + range.header.stamp = getStampMsgFromSec(io->currentTime()); + range.header.frame_id = sensor->name(); + range.width = sensor->resolutionX(); + range.height = sensor->resolutionY(); + range.is_bigendian = false; + range.is_dense = true; + range.row_step = range.point_step * range.width; + if (sensor->imageType() == cnoid::Camera::COLOR_IMAGE) { + range.fields.resize(6); + range.fields[3].name = "rgb"; + range.fields[3].offset = 12; + range.fields[3].count = 1; + range.fields[3].datatype = sensor_msgs::msg::PointField::FLOAT32; + /* + range.fields[3].name = "r"; + range.fields[3].offset = 12; + range.fields[3].datatype = sensor_msgs::PointField::UINT8; + range.fields[3].count = 1; + range.fields[4].name = "g"; + range.fields[4].offset = 13; + range.fields[4].datatype = sensor_msgs::PointField::UINT8; + range.fields[4].count = 1; + range.fields[5].name = "b"; + range.fields[5].offset = 14; + range.fields[5].datatype = sensor_msgs::PointField::UINT8; + range.fields[5].count = 1; + */ + range.point_step = 16; + } else { + range.fields.resize(3); + range.point_step = 12; + } + range.fields[0].name = "x"; + range.fields[0].offset = 0; + range.fields[0].datatype = sensor_msgs::msg::PointField::FLOAT32; + range.fields[0].count = 4; + range.fields[1].name = "y"; + range.fields[1].offset = 4; + range.fields[1].datatype = sensor_msgs::msg::PointField::FLOAT32; + range.fields[1].count = 4; + range.fields[2].name = "z"; + range.fields[2].offset = 8; + range.fields[2].datatype = sensor_msgs::msg::PointField::FLOAT32; + range.fields[2].count = 4; + const std::vector &points = sensor->constPoints(); + const unsigned char *pixels = sensor->constImage().pixels(); + range.data.resize(points.size() * range.point_step); + unsigned char *dst = (unsigned char *) &(range.data[0]); + + Matrix3f Ro = Matrix3f::Identity(); + bool hasRo = !sensor->opticalFrameRotation().isIdentity(); + if(hasRo) { + Ro = sensor->opticalFrameRotation().cast(); + } + + for (size_t j = 0; j < points.size(); ++j) { + float x, y, z; + + if (hasRo) { + const Vector3f point = Ro * points[j]; + x = point.x(); + y = - point.y(); + z = - point.z(); + } else { + x = points[j].x(); + y = - points[j].y(); + z = - points[j].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 += range.point_step; + } + publisher->publish(range); +} + + +void BodyROS2Item::updateRangeSensor( + RangeSensor *sensor, + rclcpp::Publisher::SharedPtr publisher) +{ + if (!sensor->on()) { + return; + } + sensor_msgs::msg::LaserScan range; + range.header.stamp = getStampMsgFromSec(io->currentTime()); + range.header.frame_id = sensor->name(); + range.range_max = sensor->maxDistance(); + range.range_min = sensor->minDistance(); + if (sensor->yawRange() == 0.0) { + range.angle_max = sensor->pitchRange() / 2.0; + range.angle_min = -sensor->pitchRange() / 2.0; + range.angle_increment = sensor->pitchStep(); + } else { + range.angle_max = sensor->yawRange() / 2.0; + range.angle_min = -sensor->yawRange() / 2.0; + range.angle_increment = sensor->yawStep(); + } + range.ranges.resize(sensor->rangeData().size()); + //range.intensities.resize(sensor->rangeData().size()); + // for (size_t j = 0; j < sensor->rangeData().size(); ++j) { + for (size_t j = 0; j < sensor->numYawSamples(); ++j) { + range.ranges[j] = sensor->rangeData()[j]; + //range.intensities[j] = -900000; + } + publisher->publish(range); +} + + +void BodyROS2Item::update3DRangeSensor( + RangeSensor *sensor, + rclcpp::Publisher::SharedPtr publisher) +{ + if (!sensor->on()) { + return; + } + sensor_msgs::msg::PointCloud2 range; + // Header Info + range.header.stamp = getStampMsgFromSec(io->currentTime()); + range.header.frame_id = sensor->name(); + + // Calculate Point Cloud data + const int numPitchSamples = sensor->numPitchSamples(); + const double pitchStep = sensor->pitchStep(); + const int numYawSamples = sensor->numYawSamples(); + const double yawStep = sensor->yawStep(); + + range.height = numPitchSamples; + range.width = numYawSamples; + range.point_step = 12; + range.row_step = range.width * range.point_step; + range.fields.resize(3); + range.fields[0].name = "x"; + range.fields[0].offset = 0; + range.fields[0].datatype = sensor_msgs::msg::PointField::FLOAT32; + range.fields[0].count = 4; + range.fields[1].name = "y"; + range.fields[1].offset = 4; + range.fields[1].datatype = sensor_msgs::msg::PointField::FLOAT32; + range.fields[1].count = 4; + range.fields[2].name = "z"; + range.fields[2].offset = 8; + range.fields[2].datatype = sensor_msgs::msg::PointField::FLOAT32; + range.fields[2].count = 4; + + range.data.resize(numPitchSamples * numYawSamples * range.point_step); + unsigned char* dst = (unsigned char*)&(range.data[0]); + + auto Ro = [&]() -> std::optional { + if(sensor->opticalFrameRotation().isIdentity()){ + return std::nullopt; + }else{ + return sensor->opticalFrameRotation().cast(); + } + }(); + + for(int pitchIndex = 0; pitchIndex < numPitchSamples; ++pitchIndex){ + const double pitchAngle = + pitchIndex * pitchStep - sensor->pitchRange() / 2.0; + const double cosPitchAngle = cos(pitchAngle); + const double sinPitchAngle = sin(pitchAngle); + const int srctop = pitchIndex * numYawSamples; + + for (int yawIndex = 0; yawIndex < numYawSamples; ++yawIndex) { + const double distance = sensor->rangeData()[srctop + yawIndex]; + const double yawAngle = yawIndex * yawStep - sensor->yawRange() / 2.0; + Vector3f p; + p.x() = distance * cosPitchAngle * sin(-yawAngle); + p.y() = distance * sin(pitchAngle); + p.z() = - distance * cosPitchAngle * cos(-yawAngle); + if (Ro) { + p = Ro.value() * p; + } + std::memcpy(&dst[0], &p.x(), 4); + std::memcpy(&dst[4], &p.y(), 4); + std::memcpy(&dst[8], &p.z(), 4); + dst += range.point_step; + } + } + + publisher->publish(range); +} + + +void BodyROS2Item::input() {} + +void BodyROS2Item::output() {} + +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()); +} + +void BodyROS2Item::stop() +{ + if(rclcpp::ok(rosContext)) { + rclcpp::shutdown(rosContext); + rosContext = nullptr; + } + if (rosNode) { + rosNode = nullptr; + } +} + +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..13cd97c --- /dev/null +++ b/src/plugin/BodyROS2Item.h @@ -0,0 +1,173 @@ +#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 "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 bool start() override; + virtual double timeStep() const override { return timeStep_; }; + virtual void input() override; + virtual bool control() override; + virtual void output() override; + virtual void stop() 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 &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 bool store(Archive &archive) override; + virtual bool restore(const Archive &archive) override; + virtual void doPutProperties(PutPropertyFunction &putProperty) override; + +private: + BodyPtr simulationBody; + DeviceList forceSensors_; + DeviceList gyroSensors_; + DeviceList accelSensors_; + 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::Context::SharedPtr rosContext; + rclcpp::Node::SharedPtr rosNode; + + std::string bodyName; + + std::vector::SharedPtr> + forceSensorPublishers; + std::vector::SharedPtr> + rateGyroSensorPublishers; + std::vector::SharedPtr> + accelSensorPublishers; + + std::shared_ptr imageTransport = nullptr; + 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> visionSensorSwitchServers; + std::vector> + rangeVisionSensorSwitchServers; + std::vector> rangeSensorSwitchServers; + std::vector> rangeSensorPcSwitchServers; + + 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 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/CMakeLists.txt b/src/plugin/CMakeLists.txt index d056657..32f569c 100644 --- a/src/plugin/CMakeLists.txt +++ b/src/plugin/CMakeLists.txt @@ -2,43 +2,75 @@ 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" + ) + + target_link_libraries(${target} PUBLIC + Choreonoid::CnoidBodyPlugin + ) -# RobotHW inherited ros_control Choreonoid interface class -add_library(RobotHWCnoid RobotHWSim.h RobotHWCnoid.cpp) -target_link_libraries(RobotHWCnoid ${catkin_LIBRARIES} Choreonoid::CnoidBody Choreonoid::CnoidBase) + if(CHOREONOID_ENABLE_PYTHON) + # add_subdirectory(pybind11) + endif () -install(TARGETS RobotHWCnoid - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} - ) + install(TARGETS ${target} + LIBRARY DESTINATION lib/${PROJECT_NAME}) -if(CHOREONOID_ENABLE_PYTHON) - add_subdirectory(pybind11) + endif() endif() diff --git a/src/plugin/ROS2Plugin.cpp b/src/plugin/ROS2Plugin.cpp new file mode 100644 index 0000000..85d9ad7 --- /dev/null +++ b/src/plugin/ROS2Plugin.cpp @@ -0,0 +1,24 @@ +#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; +} + +CNOID_IMPLEMENT_PLUGIN_ENTRY(ROS2Plugin) diff --git a/src/plugin/ROS2Plugin.hpp b/src/plugin/ROS2Plugin.hpp new file mode 100644 index 0000000..2abcb14 --- /dev/null +++ b/src/plugin/ROS2Plugin.hpp @@ -0,0 +1,14 @@ +#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 initialize(); +}; + +#endif // CNOID_ROS_PLUGIN_ROS2_PLUGIN_HPP_ diff --git a/src/plugin/WorldROS2Item.cpp b/src/plugin/WorldROS2Item.cpp new file mode 100644 index 0000000..cfa78f7 --- /dev/null +++ b/src/plugin/WorldROS2Item.cpp @@ -0,0 +1,261 @@ +/** + \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::Context::SharedPtr rosContext; + + 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 clearWorld(); + void onSimulationAboutToStart(SimulatorItem *simulatorItem); + void setCurrentSimulatorItem(SimulatorItem *simulatorItem); + void clearCurrentSimulatorItem(); + void onSimulationStarted(); + void onSimulationStep(); + void onSimulationFinished(); + void setUpROS2(); + void tearDownROS2(); +}; + +} // 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(); + clearWorld(); +} + +Item *WorldROS2Item::doDuplicate() const +{ + return new WorldROS2Item(*this); +} + +void WorldROS2Item::onPositionChanged() +{ + WorldItem *worldItem = nullptr; + if (isConnectedToRoot()) { + worldItem = findOwnerItem(); + if (worldItem && worldItem != impl->worldItem) { + impl->initializeWorld(worldItem); + } + } + if (!worldItem) { + impl->clearWorld(); + } +} + +void WorldROS2Item::Impl::initializeWorld(WorldItem *worldItem) +{ + clearWorld(); + + this->worldItem = worldItem; + + setUpROS2(); +} + +void WorldROS2Item::Impl::initializeClockPublisher() +{ + clockPublisher.reset(); + if (maxClockPublishingRate > 0.0) { + clockPublisher = rosNode->create_publisher("/clock", + 1); + } +} + +void WorldROS2Item::setMaxClockPublishingRate(double rate) +{ + impl->maxClockPublishingRate = rate; + impl->initializeClockPublisher(); +} + +void WorldROS2Item::Impl::clearWorld() { + tearDownROS2(); +} + +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) * 10e9; + 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; +} + +void WorldROS2Item::Impl::setUpROS2() +{ + if(not rosContext or not rclcpp::ok(rosContext)) + { + rosContext = std::make_shared(); + rosContext->init(0, nullptr); + } + + string name = worldItem->name(); + std::replace(name.begin(), name.end(), '-', '_'); + rosNode = std::make_unique(name, rclcpp::NodeOptions().context(rosContext)); + + initializeClockPublisher(); +} + +void WorldROS2Item::Impl::tearDownROS2() +{ + if(rclcpp::ok(rosContext)) + { + rclcpp::shutdown(rosContext); + } + if(rosContext) + { + rosContext = nullptr; + } +} diff --git a/src/plugin/WorldROS2Item.h b/src/plugin/WorldROS2Item.h new file mode 100644 index 0000000..50694a3 --- /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 onPositionChanged() 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