Skip to content

Commit 6ac6f0c

Browse files
committed
added move model direct ros plugin
1 parent 483bacb commit 6ac6f0c

25 files changed

Lines changed: 620 additions & 16 deletions

File tree

40.7 KB
Loading

docs/assets/images/new_thread.png

64 KB
Loading
30.7 KB
Loading
Lines changed: 353 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,353 @@
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+
![ros thread](assets/images/ros_executor_from_doc.png)
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+
![block main thread](assets/images/block_main_thread.png)
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+
![new thread](assets/images/new_thread.png)
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+

mkdocs.yml

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -66,6 +66,7 @@ nav:
6666
- ROS2_WS:
6767
- Common In All Ros2 Ws Plugins: common-in-all-ros2-ws-plugins.md
6868
- Move Model Using ros-gz-bridge: movemodel_using-ros-gazebo-bridge.md
69+
- Move Model Using Direct ROS Plugin: move-model-direct-ros-plugin.md
6970
# - Tutorials:
7071
# - Plugin basics: basics.md
7172
# - World plugin: world_plugin.md

site/404.html

Lines changed: 1 addition & 1 deletion
Large diffs are not rendered by default.
40.7 KB
Loading

site/assets/images/new_thread.png

64 KB
Loading
30.7 KB
Loading

0 commit comments

Comments
 (0)