Merge branch 'feature/imu_ros2'

choreonoid · Oct 9, 2024 · 2d335ed · 2d335ed
2 parents de948af + d3d7851
commit 2d335ed
Show file tree

Hide file tree

Showing 3 changed files with 54 additions and 2 deletions.
diff --git a/src/plugin/BodyROS2Item.cpp b/src/plugin/BodyROS2Item.cpp
@@ -194,6 +194,7 @@ void BodyROS2Item::createSensors(BodyPtr body)
     forceSensors_.assign(devices.extract<ForceSensor>());
     gyroSensors_.assign(devices.extract<RateGyroSensor>());
     accelSensors_.assign(devices.extract<AccelerationSensor>());
+    imus_.assign(devices.extract<Imu>());
     visionSensors_.assign(devices.extract<Camera>());
     rangeVisionSensors_.assign(devices.extract<RangeCamera>());
     rangeSensors_.assign(devices.extract<RangeSensor>());
@@ -281,6 +282,30 @@ void BodyROS2Item::createSensors(BodyPtr body)
                     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<sensor_msgs::msg::Imu>(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<std_srvs::srv::SetBool>(name + "/set_enabled",
+                                                          requestCallback));
+        RCLCPP_INFO(rosNode->get_logger(),
+                    "Create IMU %s",
+                    sensor->name().c_str());
+    }
+
     visionSensorPublishers.clear();
     visionSensorPublishers.reserve(visionSensors_.size());
     visionSensorSwitchServers.clear();
@@ -464,6 +489,24 @@ void BodyROS2Item::updateAccelSensor(
     publisher->publish(accel);
 }
 
+void BodyROS2Item::updateImu(
+    Imu *sensor,
+    rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr publisher)
+{
+    if (!sensor->on()) {
+        return;
+    }
+    sensor_msgs::msg::Imu imu;
+    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;
+    publisher->publish(imu);
+}
 
 void BodyROS2Item::updateVisionSensor(
     Camera *sensor,

diff --git a/src/plugin/BodyROS2Item.h b/src/plugin/BodyROS2Item.h
@@ -8,6 +8,7 @@
 #include <cnoid/ControllerItem>
 #include <cnoid/Device>
 #include <cnoid/DeviceList>
+#include <cnoid/Imu>
 #include <cnoid/RangeCamera>
 #include <cnoid/RangeSensor>
 
@@ -63,6 +64,7 @@ class CNOID_EXPORT BodyROS2Item : public ControllerItem
     {
         return accelSensors_;
     }
+    const DeviceList<Imu>& imus() const { return imus_; }
     const DeviceList<Camera> &visionSensors() const { return visionSensors_; }
     const DeviceList<RangeCamera> &rangeVisionSensors() const
     {
@@ -91,6 +93,7 @@ class CNOID_EXPORT BodyROS2Item : public ControllerItem
     DeviceList<ForceSensor> forceSensors_;
     DeviceList<RateGyroSensor> gyroSensors_;
     DeviceList<AccelerationSensor> accelSensors_;
+    DeviceList<Imu> imus_;
     DeviceList<Camera> visionSensors_;
     DeviceList<RangeCamera> rangeVisionSensors_;
     DeviceList<RangeSensor> rangeSensors_;
@@ -120,6 +123,8 @@ class CNOID_EXPORT BodyROS2Item : public ControllerItem
         rateGyroSensorPublishers;
     std::vector<rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr>
         accelSensorPublishers;
+    std::vector<rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr>
+        imuPublishers;
 
     std::shared_ptr<image_transport::ImageTransport> imageTransport = nullptr;
     std::vector<image_transport::Publisher> visionSensorPublishers;
@@ -134,6 +139,7 @@ class CNOID_EXPORT BodyROS2Item : public ControllerItem
     std::vector<std::shared_ptr<rclcpp::ServiceBase>> forceSensorSwitchServers;
     std::vector<std::shared_ptr<rclcpp::ServiceBase>> rateGyroSensorSwitchServers;
     std::vector<std::shared_ptr<rclcpp::ServiceBase>> accelSensorSwitchServers;
+    std::vector<std::shared_ptr<rclcpp::ServiceBase>> imuSwitchServers;
     std::vector<std::shared_ptr<rclcpp::ServiceBase>> visionSensorSwitchServers;
     std::vector<std::shared_ptr<rclcpp::ServiceBase>>
         rangeVisionSensorSwitchServers;
@@ -153,6 +159,9 @@ class CNOID_EXPORT BodyROS2Item : public ControllerItem
     void updateAccelSensor(
         AccelerationSensor *sensor,
         rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr publisher);
+    void updateImu(
+        Imu *sensor,
+        rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr publisher);
     void updateVisionSensor(
         Camera *sensor,
         image_transport::Publisher & publisher);

diff --git a/src/plugin/BodyROSItem.cpp b/src/plugin/BodyROSItem.cpp
@@ -244,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<bool (std_srvs::SetBoolRequest&, std_srvs::SetBoolResponse&)> 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());
     }