From c4e65d1f33ad62d3e34bf5b279511e4b5231a730 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Fri, 23 Aug 2024 22:10:12 +0900 Subject: [PATCH 1/2] [BodyROS] Use correct lists for imu publishers/switch servers --- src/plugin/BodyROSItem.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/plugin/BodyROSItem.cpp b/src/plugin/BodyROSItem.cpp index 73b3836..8654062 100644 --- 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 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()); } From d3d78519936d9eccd1373fac6a65764ee13f4045 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Fri, 23 Aug 2024 23:02:04 +0900 Subject: [PATCH 2/2] [BodyROS2] Support IMU (porting from BodyROS) --- src/plugin/BodyROS2Item.cpp | 43 +++++++++++++++++++++++++++++++++++++ src/plugin/BodyROS2Item.h | 9 ++++++++ 2 files changed, 52 insertions(+) diff --git a/src/plugin/BodyROS2Item.cpp b/src/plugin/BodyROS2Item.cpp index 0d02f28..899fe09 100644 --- a/src/plugin/BodyROS2Item.cpp +++ b/src/plugin/BodyROS2Item.cpp @@ -194,6 +194,7 @@ void BodyROS2Item::createSensors(BodyPtr body) 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()); @@ -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(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(); @@ -464,6 +489,24 @@ void BodyROS2Item::updateAccelSensor( publisher->publish(accel); } +void BodyROS2Item::updateImu( + Imu *sensor, + rclcpp::Publisher::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 index 1e36404..5262d46 100644 --- a/src/plugin/BodyROS2Item.h +++ b/src/plugin/BodyROS2Item.h @@ -8,6 +8,7 @@ #include #include #include +#include #include #include @@ -63,6 +64,7 @@ class CNOID_EXPORT BodyROS2Item : public ControllerItem { return accelSensors_; } + const DeviceList& imus() const { return imus_; } const DeviceList &visionSensors() const { return visionSensors_; } const DeviceList &rangeVisionSensors() const { @@ -91,6 +93,7 @@ class CNOID_EXPORT BodyROS2Item : public ControllerItem DeviceList forceSensors_; DeviceList gyroSensors_; DeviceList accelSensors_; + DeviceList imus_; DeviceList visionSensors_; DeviceList rangeVisionSensors_; DeviceList rangeSensors_; @@ -120,6 +123,8 @@ class CNOID_EXPORT BodyROS2Item : public ControllerItem rateGyroSensorPublishers; std::vector::SharedPtr> accelSensorPublishers; + std::vector::SharedPtr> + imuPublishers; std::shared_ptr imageTransport = nullptr; std::vector visionSensorPublishers; @@ -134,6 +139,7 @@ class CNOID_EXPORT BodyROS2Item : public ControllerItem std::vector> forceSensorSwitchServers; std::vector> rateGyroSensorSwitchServers; std::vector> accelSensorSwitchServers; + std::vector> imuSwitchServers; std::vector> visionSensorSwitchServers; std::vector> rangeVisionSensorSwitchServers; @@ -153,6 +159,9 @@ class CNOID_EXPORT BodyROS2Item : public ControllerItem void updateAccelSensor( AccelerationSensor *sensor, rclcpp::Publisher::SharedPtr publisher); + void updateImu( + Imu *sensor, + rclcpp::Publisher::SharedPtr publisher); void updateVisionSensor( Camera *sensor, image_transport::Publisher & publisher);