diff --git a/CMakeLists.txt b/CMakeLists.txt
index 6f3245e..557d8d0 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -56,10 +56,14 @@ if(DEFINED ENV{ROS_VERSION})
     # For ros_control
     install(FILES plugins.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
 
-    install(DIRECTORY launch
-      DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-      USE_SOURCE_PERMISSIONS
-    )
+    # 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()
 
diff --git a/src/plugin/BodyROS2Item.cpp b/src/plugin/BodyROS2Item.cpp
index 0d02f28..51cd62a 100644
--- a/src/plugin/BodyROS2Item.cpp
+++ b/src/plugin/BodyROS2Item.cpp
@@ -13,12 +13,38 @@
 #include <sensor_msgs/msg/laser_scan.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
 
+#include <type_traits>
+
 #include "gettext.h"
 
 using namespace cnoid;
 using std::placeholders::_1;
 using std::placeholders::_2;
 
+namespace {
+
+template<typename T, typename = void>
+struct has_dispatch : std::false_type {};
+
+template<typename T>
+struct has_dispatch<T, std::void_t<decltype(std::declval<T>().dispatch(std::declval<std::function<void()>>()))>>
+    : std::true_type {};
+
+template<typename T>
+std::enable_if_t<has_dispatch<T>::value> dispatch(T& obj, std::function<void()> func)
+{
+    obj.dispatch(func);
+}
+
+template<typename T>
+std::enable_if_t<!has_dispatch<T>::value> dispatch(T& obj, std::function<void()> func)
+{
+    obj.start(func);
+}
+
+}
+
+
 void BodyROS2Item::initializeClass(ExtensionManager *ext)
 {
     ext->itemManager().registerClass<BodyROS2Item>(N_("BodyROS2Item"));
@@ -27,7 +53,8 @@ void BodyROS2Item::initializeClass(ExtensionManager *ext)
 
 
 BodyROS2Item::BodyROS2Item()
-    : os(MessageView::instance()->cout())
+    : os(MessageView::instance()->cout()),
+      threadPoolForPublishing(10)
 {
     bodyItem = nullptr;
     io = nullptr;
@@ -36,8 +63,9 @@ BodyROS2Item::BodyROS2Item()
 
 
 BodyROS2Item::BodyROS2Item(const BodyROS2Item &org)
-    : ControllerItem(org)
-    , os(MessageView::instance()->cout())
+    : ControllerItem(org),
+      os(MessageView::instance()->cout()),
+      threadPoolForPublishing(10)
 {
     io = nullptr;
     jointStateUpdateRate = 100.0;
@@ -55,6 +83,7 @@ Item *BodyROS2Item::doDuplicate() const
     return new BodyROS2Item(*this);
 }
 
+
 bool BodyROS2Item::store(Archive &archive)
 {
     archive.write("body_ros_version", 0);
@@ -151,6 +180,12 @@ bool BodyROS2Item::initialize(ControllerIO *io)
 }
 
 
+double BodyROS2Item::timeStep() const
+{
+    return timeStep_;
+}
+
+
 bool BodyROS2Item::start()
 {
     imageTransport = std::make_shared<image_transport::ImageTransport>(rosNode);
@@ -194,6 +229,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,15 +317,39 @@ 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();
     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]() {
+        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,
@@ -412,155 +472,181 @@ bool BodyROS2Item::control()
 
 
 void BodyROS2Item::updateForceSensor(
-    ForceSensor *sensor,
+    ForceSensor* sensor,
     rclcpp::Publisher<geometry_msgs::msg::WrenchStamped>::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);
+    
+    auto wrench = std::make_shared<geometry_msgs::msg::WrenchStamped>();
+    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,
+    RateGyroSensor* sensor,
     rclcpp::Publisher<sensor_msgs::msg::Imu>::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);
+    auto imu = std::make_shared<sensor_msgs::msg::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];
+
+    dispatch(threadPoolForPublishing, [publisher, imu]{ publisher->publish(*imu); });
 }
 
 
 void BodyROS2Item::updateAccelSensor(
-    AccelerationSensor *sensor,
+    AccelerationSensor* sensor,
+    rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr publisher)
+{
+    if (!sensor->on()) {
+        return;
+    }
+    
+    auto imu = std::make_shared<sensor_msgs::msg::Imu>();
+    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<sensor_msgs::msg::Imu>::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);
+    auto imu = std::make_shared<sensor_msgs::msg::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;
+
+    dispatch(threadPoolForPublishing, [publisher, imu]{ publisher->publish(*imu); });
 }
 
 
 void BodyROS2Item::updateVisionSensor(
-    Camera *sensor,
-    image_transport::Publisher & publisher)
+    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 {
+    auto image = std::make_shared<sensor_msgs::msg::Image>();
+    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());
     }
-    vision.is_bigendian = 0;
-    vision.step = sensor->image().width() * sensor->image().numComponents();
-    vision.data.resize(vision.step * vision.height);
-    std::memcpy(&(vision.data[0]),
+    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]),
-                vision.step * vision.height);
-    publisher.publish(vision);
+                image->step * image->height);
+
+    dispatch(threadPoolForPublishing, [publisher, image]{ publisher.publish(image); });
 }
 
 
 void BodyROS2Item::updateRangeVisionSensor(
-    RangeCamera *sensor,
+    RangeCamera* sensor,
     rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::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;
+
+    auto pointCloud = std::make_shared<sensor_msgs::msg::PointCloud2>();
+    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) {
-        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;
+        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;
         /*
-          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;
+          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;
         */
-        range.point_step = 16;
+        pointCloud->point_step = 16;
     } else {
-        range.fields.resize(3);
-        range.point_step = 12;
+        pointCloud->fields.resize(3);
+        pointCloud->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;
+    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<Vector3f> &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]);
-
-    auto Ro = [&]() -> std::optional<Matrix3f> {
-        if(sensor->opticalFrameRotation().isIdentity()){
-            return std::nullopt;
-        }else{
-            return sensor->opticalFrameRotation().cast<float>();
-        }
-    }();
+    pointCloud->data.resize(points.size() * pointCloud->point_step);
+    unsigned char *dst = (unsigned char *) &(pointCloud->data[0]);
+
+    std::optional<Matrix3f> Ro;
+    if(!sensor->opticalFrameRotation().isIdentity()){
+        Ro = sensor->opticalFrameRotation().cast<float>();
+    }
 
     for (size_t j = 0; j < points.size(); ++j) {
         Vector3f point = points[j];
@@ -589,55 +675,60 @@ void BodyROS2Item::updateRangeVisionSensor(
             dst[12] = *pixels++;
             dst[15] = 0;
         }
-        dst += range.point_step;
+        dst += pointCloud->point_step;
     }
-    publisher->publish(range);
+
+    dispatch(threadPoolForPublishing, [publisher, pointCloud]{ publisher->publish(*pointCloud); });
 }
 
 
 void BodyROS2Item::updateRangeSensor(
-    RangeSensor *sensor,
+    RangeSensor* sensor,
     rclcpp::Publisher<sensor_msgs::msg::LaserScan>::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();
+    
+    auto laserScan = std::make_shared<sensor_msgs::msg::LaserScan>();
+    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) {
-        range.angle_max = sensor->pitchRange() / 2.0;
-        range.angle_min = -sensor->pitchRange() / 2.0;
-        range.angle_increment = sensor->pitchStep();
+        laserScan->angle_max = sensor->pitchRange() / 2.0;
+        laserScan->angle_min = -sensor->pitchRange() / 2.0;
+        laserScan->angle_increment = sensor->pitchStep();
     } else {
-        range.angle_max = sensor->yawRange() / 2.0;
-        range.angle_min = -sensor->yawRange() / 2.0;
-        range.angle_increment = sensor->yawStep();
+        laserScan->angle_max = sensor->yawRange() / 2.0;
+        laserScan->angle_min = -sensor->yawRange() / 2.0;
+        laserScan->angle_increment = sensor->yawStep();
     }
-    range.ranges.resize(sensor->rangeData().size());
-    //range.intensities.resize(sensor->rangeData().size());
+    laserScan->ranges.resize(sensor->rangeData().size());
+    //laserScan->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;
+        laserScan->ranges[j] = sensor->rangeData()[j];
+        //laserScan->intensities[j] = -900000;
     }
-    publisher->publish(range);
+
+    dispatch(threadPoolForPublishing, [publisher, laserScan]{ publisher->publish(*laserScan); });
 }
 
 
 void BodyROS2Item::update3DRangeSensor(
-    RangeSensor *sensor,
+    RangeSensor* sensor,
     rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr publisher)
 {
     if (!sensor->on()) {
         return;
     }
-    sensor_msgs::msg::PointCloud2 range;
+
+    auto pointCloud = std::make_shared<sensor_msgs::msg::PointCloud2>();
+    
     // Header Info
-    range.header.stamp = getStampMsgFromSec(io->currentTime());
-    range.header.frame_id = sensor->name();
+    pointCloud->header.stamp = getStampMsgFromSec(io->currentTime());
+    pointCloud->header.frame_id = sensor->name();
 
     // Calculate Point Cloud data
     const int numPitchSamples = sensor->numPitchSamples();
@@ -645,38 +736,34 @@ void BodyROS2Item::update3DRangeSensor(
     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<Matrix3f> {
-        if(sensor->opticalFrameRotation().isIdentity()){
-            return std::nullopt;
-        }else{
-            return sensor->opticalFrameRotation().cast<float>();
-        }
-    }();
+    pointCloud->height = numPitchSamples;
+    pointCloud->width = numYawSamples;
+    pointCloud->point_step = 12;
+    pointCloud->row_step = pointCloud->width * pointCloud->point_step;
+    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->data.resize(numPitchSamples * numYawSamples * pointCloud->point_step);
+    unsigned char* dst = (unsigned char*)&(pointCloud->data[0]);
+
+    std::optional<Matrix3f> Ro;
+    if(!sensor->opticalFrameRotation().isIdentity()){
+        Ro = sensor->opticalFrameRotation().cast<float>();
+    }
 
     for(int pitchIndex = 0; pitchIndex < numPitchSamples; ++pitchIndex){
-        const double pitchAngle =
-                pitchIndex * pitchStep - sensor->pitchRange() / 2.0;
+        const double pitchAngle = pitchIndex * pitchStep - sensor->pitchRange() / 2.0;
         const double cosPitchAngle = cos(pitchAngle);
         const double sinPitchAngle = sin(pitchAngle);
         const int srctop = pitchIndex * numYawSamples;
@@ -694,27 +781,24 @@ void BodyROS2Item::update3DRangeSensor(
             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;
+            dst += pointCloud->point_step;
         }
     }
 
-    publisher->publish(range);
+    dispatch(threadPoolForPublishing, [publisher, pointCloud]{ publisher->publish(*pointCloud); });
 }
 
 
-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)
+    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;
@@ -723,6 +807,7 @@ builtin_interfaces::msg::Time BodyROS2Item::getStampMsgFromSec(double sec)
     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(), '-', '_');
diff --git a/src/plugin/BodyROS2Item.h b/src/plugin/BodyROS2Item.h
index 1e36404..c975025 100644
--- a/src/plugin/BodyROS2Item.h
+++ b/src/plugin/BodyROS2Item.h
@@ -8,8 +8,10 @@
 #include <cnoid/ControllerItem>
 #include <cnoid/Device>
 #include <cnoid/DeviceList>
+#include <cnoid/Imu>
 #include <cnoid/RangeCamera>
 #include <cnoid/RangeSensor>
+#include <cnoid/ThreadPool>
 
 #include <rclcpp/rclcpp.hpp>
 #include <sensor_msgs/msg/image.hpp>
@@ -44,11 +46,9 @@ class CNOID_EXPORT BodyROS2Item : public ControllerItem
     void createSensors(BodyPtr body);
 
     virtual bool initialize(ControllerIO *io) override;
+    virtual double timeStep() const override;
     virtual bool start() override;
-    virtual double timeStep() const override { return timeStep_; };
-    virtual void input() override;
     virtual bool control() override;
-    virtual void output() override;
 
     const Body *body() const { return simulationBody; };
     const DeviceList<ForceSensor> &forceSensors() const
@@ -63,6 +63,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 +92,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_;
@@ -112,6 +114,8 @@ class CNOID_EXPORT BodyROS2Item : public ControllerItem
     std::unique_ptr<rclcpp::executors::SingleThreadedExecutor> executor;
     std::thread executorThread;
 
+    ThreadPool threadPoolForPublishing;
+
     std::string bodyName;
 
     std::vector<rclcpp::Publisher<geometry_msgs::msg::WrenchStamped>::SharedPtr>
@@ -120,8 +124,10 @@ 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::shared_ptr<image_transport::ImageTransport> imageTransport;
     std::vector<image_transport::Publisher> visionSensorPublishers;
 
     std::vector<rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr>
@@ -134,6 +140,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;
@@ -144,31 +151,33 @@ class CNOID_EXPORT BodyROS2Item : public ControllerItem
     void finalizeRosNode();
     
     void updateForceSensor(
-        ForceSensor *sensor,
-        rclcpp::Publisher<geometry_msgs::msg::WrenchStamped>::SharedPtr
-            publisher);
+        ForceSensor* sensor,
+        rclcpp::Publisher<geometry_msgs::msg::WrenchStamped>::SharedPtr publisher);
     void updateRateGyroSensor(
-        RateGyroSensor *sensor,
+        RateGyroSensor* sensor,
         rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr publisher);
     void updateAccelSensor(
-        AccelerationSensor *sensor,
+        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);
+        Camera* sensor,
+        image_transport::Publisher publisher);
     void updateRangeVisionSensor(
-        RangeCamera *sensor,
+        RangeCamera* sensor,
         rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr publisher);
     void updateRangeSensor(
-        RangeSensor *sensor,
+        RangeSensor* sensor,
         rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr publisher);
     void update3DRangeSensor(
-        RangeSensor *sensor,
+        RangeSensor* sensor,
         rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr publisher);
 
     void switchDevice(std_srvs::srv::SetBool::Request::ConstSharedPtr request,
                       std_srvs::srv::SetBool::Response::SharedPtr response,
-                      Device *sensor);
+                      Device* sensor);
     builtin_interfaces::msg::Time getStampMsgFromSec(double sec);
 
     std::string getROS2Name(const std::string &name) const;
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<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());
     }