|
| 1 | +# MoveModelDirectRos Plugin |
| 2 | + |
| 3 | +As we understood in the last method that ros-gz-brige has limited supported msgs. So we are going undertand to add direct ros2 rclcpp into gazebo sim system plugin. |
| 4 | + |
| 5 | +rclcpp we can direct use standard ros2 ```publisher/subscriber```,```service/client``` rclcpp everything of a ros2 node syntex is same. |
| 6 | + |
| 7 | + |
| 8 | + |
| 9 | + |
| 10 | + |
| 11 | +## Need A Different thread for ros2 in plugin why? |
| 12 | + |
| 13 | + |
| 14 | + ros executor code snap |
| 15 | + |
| 16 | +```c++ |
| 17 | + rclcpp::executors::SingleThreadedExecutor executor; |
| 18 | + executor.add_node(node); |
| 19 | + executor.spin(); // blocks this thread, one callback at a time |
| 20 | +``` |
| 21 | + |
| 22 | +<br> |
| 23 | + |
| 24 | + |
| 25 | + |
| 26 | +image beautifully display how ros executors work. For example 2 msg are received from topic than ros executors send one method to ros subscriber callback waits for it to process that is the time when it block main thread & once processed sends next msg. |
| 27 | + |
| 28 | + |
| 29 | +<br> |
| 30 | + |
| 31 | + |
| 32 | +```executor.spin()``` blocking loop until the node shuts down, centralizing event handling for subscriptions, timers, and services within a thread. |
| 33 | + |
| 34 | +that means using executor.spin() on gazebo sim will also ***get blocked(stop for some time)*** or ***become unsmooth*** |
| 35 | + |
| 36 | + |
| 37 | + |
| 38 | + |
| 39 | +<br> |
| 40 | +<br> |
| 41 | +<br> |
| 42 | + |
| 43 | +### Add New Thread For ros_executor |
| 44 | + |
| 45 | +so simple solution is make another thread for ros2 using ```std::thread()``` |
| 46 | + |
| 47 | + |
| 48 | + |
| 49 | +ros spin work with blocking but not harming gz main thread as it a different thread |
| 50 | + |
| 51 | +everthing of ros run on the new thread (name: cpp_thread) means ```OnRosMsg(...)``` subscriber callback is also part of new thread |
| 52 | + |
| 53 | + |
| 54 | +<br> |
| 55 | +<br> |
| 56 | + |
| 57 | +## Code |
| 58 | + |
| 59 | +create the ros2 node & create singe threaded executor. More on [ros2 executors](https://docs.ros.org/en/rolling/Concepts/Intermediate/About-Executors.html) |
| 60 | +```c++ |
| 61 | + |
| 62 | + // Create the ROS 2 node |
| 63 | + this->ros_node_ = std::make_shared<rclcpp::Node>("gz_sim_move_model_plugin"); |
| 64 | + // this->executor_ = std::make_shared<rclcpp::executors::MultiThreadedExecutor>(); |
| 65 | + this->ros_executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>(); |
| 66 | + this->ros_executor_->add_node(this->ros_node_); |
| 67 | +``` |
| 68 | +
|
| 69 | +
|
| 70 | +<br> |
| 71 | +
|
| 72 | +
|
| 73 | +lamda function contain ```ros_executor->spin()```<br> |
| 74 | +[std::thread()](https://www.geeksforgeeks.org/cpp/multithreading-in-cpp/) used for creating the new thread |
| 75 | +
|
| 76 | +```c++ |
| 77 | +
|
| 78 | +
|
| 79 | + //lamda function having ros spin |
| 80 | + auto ros_thread_spin = [this]() |
| 81 | + { |
| 82 | + this->ros_executor_->spin(); |
| 83 | + }; |
| 84 | +
|
| 85 | + //make a new thread for ros spin |
| 86 | + this->cpp_thread = std::thread(ros_thread_spin); |
| 87 | +
|
| 88 | +``` |
| 89 | + |
| 90 | + |
| 91 | +<details> |
| 92 | + <summary>.hh file</summary> |
| 93 | + |
| 94 | +```c++ |
| 95 | +#ifndef SYSTEM_MOVE_MODEL_DIRECTROS_PLUGIN_MODEL_HH_ |
| 96 | +#define SYSTEM_MOVE_MODEL_DIRECTROS_PLUGIN_MODEL_HH_ |
| 97 | + |
| 98 | +//! [header] |
| 99 | +#include <gz/sim/System.hh> |
| 100 | +#include <gz/math/Vector3.hh> |
| 101 | + |
| 102 | +// ROS 2 Includes |
| 103 | +#include <rclcpp/rclcpp.hpp> |
| 104 | +#include <std_msgs/msg/float64.hpp> |
| 105 | + |
| 106 | +namespace gz |
| 107 | +{ |
| 108 | +namespace sim |
| 109 | +{ |
| 110 | +namespace systems |
| 111 | +{ |
| 112 | + /// \brief plugin to move a model |
| 113 | + /// plugin interface. |
| 114 | + class MoveModelDirectRos : |
| 115 | + // This class is a system. |
| 116 | + public gz::sim::System, |
| 117 | + public gz::sim::ISystemConfigure, |
| 118 | + // This class also implements the ISystemPreUpdate interface. |
| 119 | + public gz::sim::ISystemUpdate |
| 120 | + { |
| 121 | + public: |
| 122 | + MoveModelDirectRos(); |
| 123 | + |
| 124 | + ~MoveModelDirectRos() override; |
| 125 | + |
| 126 | + void Configure(const Entity &_entity, |
| 127 | + const std::shared_ptr<const sdf::Element> &_sdf, |
| 128 | + EntityComponentManager &_ecm, |
| 129 | + EventManager &_eventMgr) override; |
| 130 | + |
| 131 | + void Update(const UpdateInfo &_info, |
| 132 | + EntityComponentManager &_ecm) override; |
| 133 | + |
| 134 | + private: |
| 135 | + std::string modelName; |
| 136 | + std::string rosTopicName; |
| 137 | + double zVelocity{0.0}; |
| 138 | + Entity targetEntity{kNullEntity}; |
| 139 | + // ROS 2 Members |
| 140 | + rclcpp::Node::SharedPtr ros_node_; |
| 141 | + rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr sub_; |
| 142 | + // rclcpp::executors::MultiThreadedExecutor::SharedPtr executor_ |
| 143 | + rclcpp::executors::SingleThreadedExecutor::SharedPtr ros_executor_; //Executor to spin the controller |
| 144 | + std::thread cpp_thread; //std thread to make new thread |
| 145 | + |
| 146 | + |
| 147 | + /* ROS 2 message callback */ |
| 148 | + /** |
| 149 | + * @brief ROS 2 callback function for received velocity messages. |
| 150 | + * @param _msg The shared pointer to the incoming std_msgs/Float64 message. |
| 151 | + */ |
| 152 | + void OnRosMsg(const std_msgs::msg::Float64::SharedPtr _msg); |
| 153 | + }; |
| 154 | +} |
| 155 | +} |
| 156 | +} |
| 157 | + |
| 158 | +//! [header] |
| 159 | + |
| 160 | +#endif |
| 161 | +``` |
| 162 | + |
| 163 | +</details> |
| 164 | + |
| 165 | + |
| 166 | + |
| 167 | +<details> |
| 168 | + <summary>.cc file</summary> |
| 169 | + |
| 170 | +```c++ |
| 171 | +#include "tutorial_gazebo_plugins/MoveModelDirectRos.hh" |
| 172 | + |
| 173 | +#include "gz/sim/Model.hh" |
| 174 | +#include "gz/sim/Util.hh" |
| 175 | +#include "gz/sim/components/LinearVelocity.hh" |
| 176 | +#include "gz/sim/components/LinearVelocityCmd.hh" |
| 177 | +#include "gz/sim/components/Name.hh" |
| 178 | +#include <gz/sim/System.hh> |
| 179 | +#include <gz/plugin/Register.hh> |
| 180 | + |
| 181 | +#include <rclcpp/executors.hpp> // New: ROS 2 includes |
| 182 | + |
| 183 | + |
| 184 | +using namespace gz; |
| 185 | +using namespace sim; |
| 186 | +using namespace systems; |
| 187 | + |
| 188 | +MoveModelDirectRos::MoveModelDirectRos(){ |
| 189 | + std::cout<<"MoveModelDirectRos Plugin Started!!"<<std::endl; |
| 190 | + |
| 191 | +} |
| 192 | +MoveModelDirectRos::~MoveModelDirectRos() { |
| 193 | + std::cout<<"MoveModelDirectRos Plugin Stopped!!"<<std::endl; |
| 194 | + |
| 195 | +} |
| 196 | + |
| 197 | +void MoveModelDirectRos::Configure(const Entity &_entity, |
| 198 | + const std::shared_ptr<const sdf::Element> &_sdf, |
| 199 | + EntityComponentManager &_ecm, |
| 200 | + EventManager &/*_eventMgr*/) |
| 201 | +{ |
| 202 | + // --- Gazebo Sim Configuration --- |
| 203 | + if (!_sdf->HasElement("model_name")) |
| 204 | + { |
| 205 | + gzerr << "MoveModel plugin requires a <model_name> element." << std::endl; |
| 206 | + return; |
| 207 | + } |
| 208 | + this->modelName = _sdf->Get<std::string>("model_name"); |
| 209 | + gzmsg << "Target Model Name: " << this->modelName << std::endl; |
| 210 | + |
| 211 | + if (!_sdf->HasElement("ros_topic_name")) |
| 212 | + { |
| 213 | + gzerr << "MoveModel plugin requires a <topic_name> element." << std::endl; |
| 214 | + return; |
| 215 | + } |
| 216 | + |
| 217 | + this->rosTopicName = _sdf->Get<std::string>("ros_topic_name"); |
| 218 | + gzmsg << "Cmd vel Z ROS 2 Topic Name: " << this->rosTopicName << std::endl; |
| 219 | + |
| 220 | + if (_sdf->HasElement("z_velocity")) |
| 221 | + { |
| 222 | + this->zVelocity = _sdf->Get<double>("z_velocity"); |
| 223 | + } |
| 224 | + gzmsg << "Initial Z Velocity: " << this->zVelocity << " m/s" << std::endl; |
| 225 | + |
| 226 | + |
| 227 | + // --- ROS 2 Initialization --- |
| 228 | + // Initialize ROS 2 (safe to call multiple times) |
| 229 | + if (!rclcpp::ok()) |
| 230 | + { |
| 231 | + // Pass in dummy arguments. |
| 232 | + rclcpp::init(0, nullptr); |
| 233 | + } |
| 234 | + |
| 235 | + // ### Explanation ### |
| 236 | + /* |
| 237 | + |
| 238 | + ros executor code snap: |
| 239 | + rclcpp::executors::SingleThreadedExecutor executor; |
| 240 | + executor.add_node(node); |
| 241 | + executor.spin(); // blocks this thread, one callback at a time |
| 242 | + |
| 243 | + gz main thread: |
| 244 | + ------ |
| 245 | + |
| 246 | + in loop: |
| 247 | + PreUpdate() -> using ros spin (executor.spin()) in main thread will block gazebo sim |
| 248 | + Update() |
| 249 | + PostUpdate() |
| 250 | + |
| 251 | + |
| 252 | + solution: |
| 253 | + |
| 254 | + main gz thread --->(make another thread for ros) using std::thread |
| 255 | + ------ | |
| 256 | + | -> ros spin work with blocking but not harm gz main thread |
| 257 | + in loop: | as it a different thread |
| 258 | + PreUpdate() -> no blocking | |
| 259 | + Update() | -> std::thread -> make new thread cpp_thread having ros_executor->spin() |
| 260 | + PostUpdate() | |
| 261 | + |
| 262 | + */ |
| 263 | + |
| 264 | + // Create the ROS 2 node |
| 265 | + this->ros_node_ = std::make_shared<rclcpp::Node>("gz_sim_move_model_plugin"); |
| 266 | +// this->executor_ = std::make_shared<rclcpp::executors::MultiThreadedExecutor>(); |
| 267 | + this->ros_executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>(); |
| 268 | + this->ros_executor_->add_node(this->ros_node_); |
| 269 | + |
| 270 | + //lamda function having ros spin |
| 271 | + auto ros_thread_spin = [this]() |
| 272 | + { |
| 273 | + this->ros_executor_->spin(); |
| 274 | + }; |
| 275 | + |
| 276 | + //make a new thread for ros spin |
| 277 | + this->cpp_thread = std::thread(ros_thread_spin); |
| 278 | + |
| 279 | + |
| 280 | + // Create the ROS 2 subscriber |
| 281 | + this->sub_ = this->ros_node_->create_subscription<std_msgs::msg::Float64>( |
| 282 | + this->rosTopicName, |
| 283 | + 10, // QoS history depth |
| 284 | + std::bind(&MoveModelDirectRos::OnRosMsg, this, std::placeholders::_1) |
| 285 | + ); |
| 286 | + |
| 287 | + gzmsg << "ROS 2 Subscriber created for topic: " << this->rosTopicName << std::endl; |
| 288 | +} |
| 289 | + |
| 290 | + |
| 291 | +void MoveModelDirectRos::OnRosMsg(const std_msgs::msg::Float64::SharedPtr _msg){ |
| 292 | + // Extract the double value from the ROS 2 Float64 message |
| 293 | + this->zVelocity = _msg->data; |
| 294 | + gzdbg << "Received new Z-velocity command from ROS 2: " << this->zVelocity << std::endl; |
| 295 | +} |
| 296 | + |
| 297 | +void MoveModelDirectRos::Update(const UpdateInfo &_info, |
| 298 | + EntityComponentManager &_ecm) |
| 299 | +{ |
| 300 | + // Only run if the simulation is not paused |
| 301 | + if (_info.paused) |
| 302 | + return; |
| 303 | + |
| 304 | + // 1. Find the target model entity by name (if not found yet) |
| 305 | + if (this->targetEntity == kNullEntity) |
| 306 | + { |
| 307 | + auto entityOpt = _ecm.EntityByName(this->modelName); |
| 308 | + if (!entityOpt.has_value()) |
| 309 | + { |
| 310 | + gzdbg << "Model [" << this->modelName |
| 311 | + << "] not found yet. Skipping velocity application." << std::endl; |
| 312 | + return; |
| 313 | + } |
| 314 | + |
| 315 | + this->targetEntity = entityOpt.value(); |
| 316 | + gzmsg << "Found target model entity: " << this->targetEntity << std::endl; |
| 317 | + } |
| 318 | + |
| 319 | + // 2. Set the Z-axis linear velocity: (0, 0, zVelocity) |
| 320 | + const gz::math::Vector3d vel(0.0, 0.0, this->zVelocity); |
| 321 | + |
| 322 | + // 3. Apply the velocity command component |
| 323 | + _ecm.SetComponentData<components::LinearVelocityCmd>(this->targetEntity,{vel}); |
| 324 | +} |
| 325 | + |
| 326 | +// Register the plugin with Gazebo Sim |
| 327 | +GZ_ADD_PLUGIN(gz::sim::systems::MoveModelDirectRos, |
| 328 | + gz::sim::System, |
| 329 | + gz::sim::ISystemConfigure, |
| 330 | + gz::sim::ISystemUpdate) |
| 331 | + |
| 332 | +GZ_ADD_PLUGIN_ALIAS(gz::sim::systems::MoveModelDirectRos, |
| 333 | + "gz::sim::systems::MoveModelDirectRos") |
| 334 | +``` |
| 335 | +
|
| 336 | +</details> |
| 337 | +
|
| 338 | +every other thing is same as previous [MoveModel Using ros-gz-birdge](movemodel_using-ros-gazebo-bridge.md) |
| 339 | +
|
| 340 | +## Build ros2 pkg |
| 341 | +
|
| 342 | +``` |
| 343 | +cd ros2_ws |
| 344 | +colcon build --symlink-install |
| 345 | +``` |
| 346 | +
|
| 347 | +## Run ros2 pkg |
| 348 | +
|
| 349 | +``` |
| 350 | +source install/setup.bash |
| 351 | +ros2 launch yt_tutorial_gazebo_ros move_model_direct_ros.launch.py |
| 352 | +``` |
| 353 | +
|
0 commit comments