diff --git a/README.md b/README.md
index 8df537b..a7922d4 100644
--- a/README.md
+++ b/README.md
@@ -7,7 +7,7 @@ This package provides you with a gazebo plugin that allows you to simulate the u
Add in your robot model the following, as done in [This sample model](https://github.com/MOCAP4ROS2-Project/mocap4ros2_gazebo/blob/main/models/waffle.model):
-```
+```xml
"0 0 0.577 0 0 0
@@ -33,22 +33,37 @@ Add in your robot model the following, as done in [This sample model](https://gi
robot
- base_footprint
+ base_footprint
```
+If what you want is to add markers, you can do so by adding the following line in your plugin. For this case, we would add a marker in base_footprint:
+```xml
+
+ robot
+ base_link
+
+ ```
+
+And if you have 3 or more markers within your environment, you can create new rigid_bodies from them. To do this, you have to choose the orientation through a link and add the index of the markers you want to add.
+```bash
+ros2 service call /create_rigid_body mocap4r2_msgs/srv/CreateRigidBody 'rigid_body_name: '\'new_rigid''\''
+link_parent: '\'base_link''\''
+markers: [1, 2, 5, 8]'
+```
+
## Run the sample
To see it in action, just type:
-```
+```bash
ros2 launch gazebo_mocap4r2_plugin tb3_simulation_launch.py
```
Open gzclient in other terminal:
-```
+```bash
gzclient
```
diff --git a/models/waffle.model b/models/waffle.model
index 95eb160..5c9cb7d 100644
--- a/models/waffle.model
+++ b/models/waffle.model
@@ -111,7 +111,7 @@
0.125
-
+
-
+
@@ -557,13 +557,10 @@
- base_mocap
-
+ base_link
+ base_mocap
+ camera_link
+ base_footprint
diff --git a/src/gazebo_ros_mocap.cpp b/src/gazebo_ros_mocap.cpp
index 2b63c3d..81e86bd 100644
--- a/src/gazebo_ros_mocap.cpp
+++ b/src/gazebo_ros_mocap.cpp
@@ -13,6 +13,7 @@
// limitations under the License.
// Author: Jose Miguel Guerrero Hernandez
+// Modified by: Juan Carlos Manzanares Serrano
#include
@@ -25,6 +26,7 @@
#include "mocap4r2_msgs/msg/markers.hpp"
#include "mocap4r2_msgs/msg/rigid_bodies.hpp"
+#include "mocap4r2_msgs/srv/create_rigid_body.hpp"
#include "lifecycle_msgs/msg/state.hpp"
#include "rclcpp/rclcpp.hpp"
@@ -51,16 +53,27 @@ class GazeboRosMocapPrivate : public mocap4r2_control::ControlledLifecycleNode
void control_start(const mocap4r2_control_msgs::msg::Control::SharedPtr msg) override;
void control_stop(const mocap4r2_control_msgs::msg::Control::SharedPtr msg) override;
+ void handleCreateRigidBody(
+ const std::shared_ptr request_header,
+ const std::shared_ptr request,
+ const std::shared_ptr response);
+
/// Connection to world update event. Callback is called while this is alive.
gazebo::event::ConnectionPtr update_connection_;
physics::WorldPtr world_;
- physics::LinkPtr link_;
- std::string link_name_;
+ std::vector rigid_links_;
+ std::vector marker_links_;
+ std::vector rigid_link_names_;
+ std::vector marker_link_names_;
+ std::map markers_;
+ std::map> rigid_body_markers_;
+ std::map rigid_body_orientation;
/// ROS communication.
rclcpp_lifecycle::LifecyclePublisher::SharedPtr mocap_markers_pub_;
rclcpp_lifecycle::LifecyclePublisher::SharedPtr
mocap_rigid_body_pub_;
+ rclcpp::Service::SharedPtr mocap_create_rigid_body_service_;
int frame_number_{0};
};
@@ -78,6 +91,11 @@ GazeboRosMocapPrivate::on_configure(const rclcpp_lifecycle::State & state)
"markers", rclcpp::QoS(1000));
mocap_rigid_body_pub_ = create_publisher(
"rigid_bodies", rclcpp::QoS(1000));
+ mocap_create_rigid_body_service_ = create_service(
+ "create_rigid_body",
+ std::bind(
+ &GazeboRosMocapPrivate::handleCreateRigidBody, this, std::placeholders::_1,
+ std::placeholders::_2, std::placeholders::_3));
return ControlledLifecycleNode::on_configure(state);
}
@@ -116,6 +134,20 @@ GazeboRosMocapPrivate::control_stop(const mocap4r2_control_msgs::msg::Control::S
RCLCPP_INFO(get_logger(), "Stopping mocap gazebo");
}
+void GazeboRosMocapPrivate::handleCreateRigidBody(
+ const std::shared_ptr request_header,
+ const std::shared_ptr request,
+ const std::shared_ptr response)
+{
+ (void)request_header;
+
+ RCLCPP_INFO(get_logger(), "Creating rigid body [%s]", request->rigid_body_name.c_str());
+ rigid_body_markers_[request->rigid_body_name] = request->markers;
+ rigid_body_orientation[request->rigid_body_name] = request->link_parent;
+
+ response->success = true;
+}
+
GazeboRosMocap::GazeboRosMocap()
: impl_(std::make_unique())
{
@@ -161,29 +193,70 @@ void GazeboRosMocap::Load(physics::ModelPtr _parent, sdf::ElementPtr sdf)
auto logger = impl_->get_logger();
- if (!sdf->HasElement("link_name")) {
- RCLCPP_INFO(
- logger, "Gidmap plugin missing wasn't set,"
- "therefore it's been set as 'base_mocap'.");
- impl_->link_name_ = "base_mocap";
- } else {
- impl_->link_name_ = sdf->GetElement("link_name")->Get();
+ if (sdf->HasElement("rigid_link")) {
+ auto link_name_element = sdf->GetElement("rigid_link");
+ while (link_name_element) {
+ impl_->rigid_link_names_.push_back(link_name_element->Get());
+ link_name_element = link_name_element->GetNextElement("rigid_link");
+ }
+ }
+
+ if (sdf->HasElement("marker_link")) {
+ auto link_name_element = sdf->GetElement("marker_link");
+ while (link_name_element) {
+ impl_->marker_link_names_.push_back(link_name_element->Get());
+ link_name_element = link_name_element->GetNextElement("marker_link");
+ }
}
+
impl_->world_ = _parent->GetWorld();
- for (const auto & model : impl_->world_->Models()) {
- auto links = model->GetLinks();
- impl_->link_ = find_link(links, impl_->link_name_);
+ for (const auto & link_name : impl_->rigid_link_names_) {
+ for (const auto & model : impl_->world_->Models()) {
+ auto links = model->GetLinks();
+ auto link = find_link(links, link_name);
- if (impl_->link_ != nullptr) {
- break;
+ if (link != nullptr) {
+ impl_->rigid_links_.push_back(link);
+ RCLCPP_INFO(logger, "Plugin MOCAP loaded for rigid link [%s]", link_name.c_str());
+ break;
+ }
}
}
- if (impl_->link_ != nullptr) {
- RCLCPP_INFO(logger, "Plugin MOCAP loaded for [%s] body", impl_->link_name_.c_str());
- } else {
- RCLCPP_ERROR(logger, "No link [%s] found for Plugin MOCAP", impl_->link_name_.c_str());
+ for (const auto & link_name : impl_->marker_link_names_) {
+ for (const auto & model : impl_->world_->Models()) {
+ auto links = model->GetLinks();
+ auto link = find_link(links, link_name);
+
+ if (link != nullptr) {
+ impl_->marker_links_.push_back(link);
+ RCLCPP_INFO(logger, "Plugin MOCAP loaded for marker link [%s]", link_name.c_str());
+ break;
+ }
+ }
+ }
+
+ for (const auto & link_name : impl_->rigid_link_names_) {
+ if (std::find_if(
+ impl_->rigid_links_.begin(), impl_->rigid_links_.end(),
+ [&](const physics::LinkPtr & link) {
+ return link->GetName() == link_name;
+ }) == impl_->rigid_links_.end())
+ {
+ RCLCPP_ERROR(logger, "No rigid link [%s] found for Plugin MOCAP", link_name.c_str());
+ }
+ }
+
+ for (const auto & link_name : impl_->marker_link_names_) {
+ if (std::find_if(
+ impl_->marker_links_.begin(), impl_->marker_links_.end(),
+ [&](const physics::LinkPtr & link) {
+ return link->GetName() == link_name;
+ }) == impl_->marker_links_.end())
+ {
+ RCLCPP_ERROR(logger, "No marker link [%s] found for Plugin MOCAP", link_name.c_str());
+ }
}
}
@@ -195,82 +268,130 @@ void GazeboRosMocap::OnUpdate()
return;
}
- ignition::math::v6::Pose3d pose = impl_->link_->WorldPose();
+ mocap4r2_msgs::msg::Markers ms;
+ ms.header.stamp = impl_->now();
+ ms.frame_number = impl_->frame_number_;
+ ms.header.frame_id = "map";
- auto & pos = pose.Pos();
- auto & rot = pose.Rot();
+ mocap4r2_msgs::msg::RigidBodies rbs;
+ rbs.header.stamp = impl_->now();
+ rbs.frame_number = impl_->frame_number_;
+ rbs.header.frame_id = "map";
+
+ impl_->frame_number_;
+ int index = 1;
+
+ for (const auto & link : impl_->rigid_links_) {
+ ignition::math::v6::Pose3d pose = link->WorldPose();
+
+ auto & pos = pose.Pos();
+ auto & rot = pose.Rot();
- if (impl_->mocap_markers_pub_->get_subscription_count() > 0) {
mocap4r2_msgs::msg::Marker m1;
m1.id_type = mocap4r2_msgs::msg::Marker::USE_INDEX;
- m1.marker_index = 1;
+ m1.marker_index = index++;
m1.translation.x = pos.X();
m1.translation.y = pos.Y();
m1.translation.z = pos.Z() + 0.05;
mocap4r2_msgs::msg::Marker m2;
m2.id_type = mocap4r2_msgs::msg::Marker::USE_INDEX;
- m2.marker_index = 2;
+ m2.marker_index = index++;
m2.translation.x = pos.X() + 0.02;
m2.translation.y = pos.Y();
m2.translation.z = pos.Z() + 0.03;
mocap4r2_msgs::msg::Marker m3;
m3.id_type = mocap4r2_msgs::msg::Marker::USE_INDEX;
- m3.marker_index = 3;
+ m3.marker_index = index++;
m3.translation.x = pos.X();
m3.translation.y = pos.Y() + 0.015;
m3.translation.z = pos.Z() + 0.03;
- mocap4r2_msgs::msg::Markers ms;
- ms.header.stamp = impl_->now();
- ms.header.frame_id = "map";
- ms.frame_number = impl_->frame_number_++;
- ms.markers = {m1, m2, m3};
-
- impl_->mocap_markers_pub_->publish(ms);
+ mocap4r2_msgs::msg::RigidBody rb;
+ rb.rigid_body_name = "rigid_body_" + link->GetName();
+ rb.pose.position.x = pos.X();
+ rb.pose.position.y = pos.Y();
+ rb.pose.position.z = pos.Z();
+ rb.pose.orientation.x = rot.X();
+ rb.pose.orientation.y = rot.Y();
+ rb.pose.orientation.z = rot.Z();
+ rb.pose.orientation.w = rot.W();
+
+ rb.markers = {m1, m2, m3};
+
+ ms.markers.push_back(m1);
+ ms.markers.push_back(m2);
+ ms.markers.push_back(m3);
+ rbs.rigidbodies.push_back(rb);
+
+ impl_->markers_[m1.marker_index] = m1;
+ impl_->markers_[m2.marker_index] = m2;
+ impl_->markers_[m3.marker_index] = m3;
}
- if (impl_->mocap_rigid_body_pub_->get_subscription_count() > 0) {
- mocap4r2_msgs::msg::RigidBody rb1;
- rb1.rigid_body_name = "rigid_body_1";
- rb1.pose.position.x = pos.X();
- rb1.pose.position.y = pos.Y();
- rb1.pose.position.z = pos.Z();
- rb1.pose.orientation.x = rot.X();
- rb1.pose.orientation.y = rot.Y();
- rb1.pose.orientation.z = rot.Z();
- rb1.pose.orientation.w = rot.W();
+ for (const auto & link : impl_->marker_links_) {
+ ignition::math::v6::Pose3d pose = link->WorldPose();
+
+ auto & pos = pose.Pos();
mocap4r2_msgs::msg::Marker m1;
m1.id_type = mocap4r2_msgs::msg::Marker::USE_INDEX;
- m1.marker_index = 1;
+ m1.marker_index = index++;
m1.translation.x = pos.X();
m1.translation.y = pos.Y();
- m1.translation.z = pos.Z() + 0.05;
+ m1.translation.z = pos.Z();
- mocap4r2_msgs::msg::Marker m2;
- m2.id_type = mocap4r2_msgs::msg::Marker::USE_INDEX;
- m2.marker_index = 2;
- m2.translation.x = pos.X() + 0.02;
- m2.translation.y = pos.Y();
- m2.translation.z = pos.Z() + 0.03;
+ ms.markers.push_back(m1);
+ impl_->markers_[m1.marker_index] = m1;
+ }
- mocap4r2_msgs::msg::Marker m3;
- m3.id_type = mocap4r2_msgs::msg::Marker::USE_INDEX;
- m3.marker_index = 3;
- m3.translation.x = pos.X();
- m3.translation.y = pos.Y() + 0.015;
- m3.translation.z = pos.Z() + 0.03;
+ for (const auto & [rigid_body_name, markers] : impl_->rigid_body_markers_) {
+ mocap4r2_msgs::msg::RigidBody rb;
+ geometry_msgs::msg::Point rigid_body_pose;
+ rb.rigid_body_name = rigid_body_name;
+
+ for (const auto & marker_index : markers) {
+ auto it = impl_->markers_.find(marker_index);
+ if (it != impl_->markers_.end()) {
+ rb.markers.push_back(it->second);
+ rigid_body_pose.x += it->second.translation.x;
+ rigid_body_pose.y += it->second.translation.y;
+ rigid_body_pose.z += it->second.translation.z;
+ }
+ }
+
+ geometry_msgs::msg::Point centroid;
+ centroid.x = rigid_body_pose.x / rb.markers.size();;
+ centroid.y = rigid_body_pose.y / rb.markers.size();;
+ centroid.z = rigid_body_pose.z / rb.markers.size();;
- rb1.markers = {m1, m2, m3};
+ rb.pose.position = centroid;
- mocap4r2_msgs::msg::RigidBodies rbs;
- rbs.header.stamp = impl_->now();
- rbs.header.frame_id = "map";
- rbs.frame_number = impl_->frame_number_++;
- rbs.rigidbodies = {rb1};
+ for (const auto & model : impl_->world_->Models()) {
+ auto links = model->GetLinks();
+ auto link = find_link(links, impl_->rigid_body_orientation[rigid_body_name]);
+ if (link != nullptr) {
+ ignition::math::v6::Pose3d pose = link->WorldPose();
+ auto & rot = pose.Rot();
+
+ rb.pose.orientation.x = rot.X();
+ rb.pose.orientation.y = rot.Y();
+ rb.pose.orientation.z = rot.Z();
+ rb.pose.orientation.w = rot.W();
+ break;
+ }
+ }
+
+ rbs.rigidbodies.push_back(rb);
+ }
+
+ if (impl_->mocap_markers_pub_->get_subscription_count() > 0) {
+ impl_->mocap_markers_pub_->publish(ms);
+ }
+
+ if (impl_->mocap_rigid_body_pub_->get_subscription_count() > 0) {
impl_->mocap_rigid_body_pub_->publish(rbs);
}
}