From 167ba9c173661d18507cad386e0c6eb92bdf4a2c Mon Sep 17 00:00:00 2001 From: Juancams Date: Tue, 28 May 2024 21:09:22 +0200 Subject: [PATCH 1/4] Add multiple rigid_bodies and markers Signed-off-by: Juancams --- models/waffle.model | 19 ++- src/gazebo_ros_mocap.cpp | 249 ++++++++++++++++++++++++--------------- 2 files changed, 164 insertions(+), 104 deletions(-) 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..5ece0a3 100644 --- a/src/gazebo_ros_mocap.cpp +++ b/src/gazebo_ros_mocap.cpp @@ -54,8 +54,10 @@ class GazeboRosMocapPrivate : public mocap4r2_control::ControlledLifecycleNode /// 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_; /// ROS communication. rclcpp_lifecycle::LifecyclePublisher::SharedPtr mocap_markers_pub_; @@ -161,29 +163,63 @@ 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 (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) { - break; + 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; + } } } - 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()); + // Verificar si todos los enlaces fueron encontrados + 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,83 +231,110 @@ void GazeboRosMocap::OnUpdate() return; } - ignition::math::v6::Pose3d pose = impl_->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.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.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.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); + 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.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.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.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); + rclcpp::spin_some(impl_->get_node_base_interface()); + } + + if (impl_->mocap_rigid_body_pub_->get_subscription_count() > 0) { + mocap4r2_msgs::msg::RigidBody rb1; + rb1.rigid_body_name = "rigid_body_" + link->GetName(); + 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(); + + mocap4r2_msgs::msg::Marker m1; + m1.id_type = mocap4r2_msgs::msg::Marker::USE_INDEX; + m1.marker_index = 1; + 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.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.translation.x = pos.X(); + m3.translation.y = pos.Y() + 0.015; + m3.translation.z = pos.Z() + 0.03; + + rb1.markers = {m1, m2, m3}; + + mocap4r2_msgs::msg::RigidBodies rbs; + rbs.header.stamp = impl_->now(); + rbs.header.frame_id = "map"; + rbs.rigidbodies = {rb1}; + + impl_->mocap_rigid_body_pub_->publish(rbs); + rclcpp::spin_some(impl_->get_node_base_interface()); + } } - 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(); - - mocap4r2_msgs::msg::Marker m1; - m1.id_type = mocap4r2_msgs::msg::Marker::USE_INDEX; - m1.marker_index = 1; - 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.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.translation.x = pos.X(); - m3.translation.y = pos.Y() + 0.015; - m3.translation.z = pos.Z() + 0.03; - - rb1.markers = {m1, m2, m3}; - - mocap4r2_msgs::msg::RigidBodies rbs; - rbs.header.stamp = impl_->now(); - rbs.header.frame_id = "map"; - rbs.frame_number = impl_->frame_number_++; - rbs.rigidbodies = {rb1}; - - impl_->mocap_rigid_body_pub_->publish(rbs); + for (const auto & link : impl_->marker_links_) { + ignition::math::v6::Pose3d pose = link->WorldPose(); + + auto & pos = pose.Pos(); + + 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.translation.x = pos.X(); + m1.translation.y = pos.Y(); + m1.translation.z = pos.Z(); + + mocap4r2_msgs::msg::Markers ms; + ms.header.stamp = impl_->now(); + ms.header.frame_id = "map"; + ms.frame_number = impl_->frame_number_++; + ms.markers = {m1}; + + impl_->mocap_markers_pub_->publish(ms); + rclcpp::spin_some(impl_->get_node_base_interface()); + } } } From 9e259a5e5634a51cdc5506e92a21aa1da13f18a1 Mon Sep 17 00:00:00 2001 From: Juancams Date: Wed, 29 May 2024 10:07:52 +0200 Subject: [PATCH 2/4] Solved problem with marker index Signed-off-by: Juancams --- src/gazebo_ros_mocap.cpp | 158 ++++++++++++++++----------------------- 1 file changed, 66 insertions(+), 92 deletions(-) diff --git a/src/gazebo_ros_mocap.cpp b/src/gazebo_ros_mocap.cpp index 5ece0a3..f44a4cc 100644 --- a/src/gazebo_ros_mocap.cpp +++ b/src/gazebo_ros_mocap.cpp @@ -207,7 +207,6 @@ void GazeboRosMocap::Load(physics::ModelPtr _parent, sdf::ElementPtr sdf) } } - // Verificar si todos los enlaces fueron encontrados 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()) { @@ -231,86 +230,62 @@ void GazeboRosMocap::OnUpdate() return; } + mocap4r2_msgs::msg::Markers ms; + ms.header.stamp = impl_->now(); + ms.frame_number = impl_->frame_number_; + ms.header.frame_id = "map"; + + 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.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.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.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); - rclcpp::spin_some(impl_->get_node_base_interface()); - } - - if (impl_->mocap_rigid_body_pub_->get_subscription_count() > 0) { - mocap4r2_msgs::msg::RigidBody rb1; - rb1.rigid_body_name = "rigid_body_" + link->GetName(); - 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(); - - mocap4r2_msgs::msg::Marker m1; - m1.id_type = mocap4r2_msgs::msg::Marker::USE_INDEX; - m1.marker_index = 1; - 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.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.translation.x = pos.X(); - m3.translation.y = pos.Y() + 0.015; - m3.translation.z = pos.Z() + 0.03; - - rb1.markers = {m1, m2, m3}; - - mocap4r2_msgs::msg::RigidBodies rbs; - rbs.header.stamp = impl_->now(); - rbs.header.frame_id = "map"; - rbs.rigidbodies = {rb1}; - - impl_->mocap_rigid_body_pub_->publish(rbs); - rclcpp::spin_some(impl_->get_node_base_interface()); - } + mocap4r2_msgs::msg::Marker m1; + m1.id_type = mocap4r2_msgs::msg::Marker::USE_INDEX; + 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 = 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 = index++; + m3.translation.x = pos.X(); + m3.translation.y = pos.Y() + 0.015; + m3.translation.z = pos.Z() + 0.03; + + 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); } for (const auto & link : impl_->marker_links_) { @@ -318,23 +293,22 @@ void GazeboRosMocap::OnUpdate() auto & pos = pose.Pos(); - 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.translation.x = pos.X(); - m1.translation.y = pos.Y(); - m1.translation.z = pos.Z(); - - mocap4r2_msgs::msg::Markers ms; - ms.header.stamp = impl_->now(); - ms.header.frame_id = "map"; - ms.frame_number = impl_->frame_number_++; - ms.markers = {m1}; - - impl_->mocap_markers_pub_->publish(ms); - rclcpp::spin_some(impl_->get_node_base_interface()); - } + mocap4r2_msgs::msg::Marker m1; + m1.id_type = mocap4r2_msgs::msg::Marker::USE_INDEX; + m1.marker_index = index++; + m1.translation.x = pos.X(); + m1.translation.y = pos.Y(); + m1.translation.z = pos.Z(); + + ms.markers.push_back(m1); + } + + 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); } } From 1334b1032f94efd652c10678abcd90aa4be79398 Mon Sep 17 00:00:00 2001 From: Juancams Date: Wed, 29 May 2024 12:35:41 +0200 Subject: [PATCH 3/4] Service to create new rigid_bodies with various markers Signed-off-by: Juancams --- src/gazebo_ros_mocap.cpp | 98 +++++++++++++++++++++++++++++++++++++--- 1 file changed, 91 insertions(+), 7 deletions(-) diff --git a/src/gazebo_ros_mocap.cpp b/src/gazebo_ros_mocap.cpp index f44a4cc..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,6 +53,11 @@ 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_; @@ -58,11 +65,15 @@ class GazeboRosMocapPrivate : public mocap4r2_control::ControlledLifecycleNode 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}; }; @@ -80,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); } @@ -118,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()) { @@ -208,15 +238,23 @@ void GazeboRosMocap::Load(physics::ModelPtr _parent, sdf::ElementPtr sdf) } 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()) { + 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()) { + 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()); } } @@ -239,10 +277,10 @@ void GazeboRosMocap::OnUpdate() 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(); @@ -281,11 +319,15 @@ void GazeboRosMocap::OnUpdate() 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; } for (const auto & link : impl_->marker_links_) { @@ -301,6 +343,48 @@ void GazeboRosMocap::OnUpdate() m1.translation.z = pos.Z(); ms.markers.push_back(m1); + impl_->markers_[m1.marker_index] = m1; + } + + 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();; + + rb.pose.position = centroid; + + 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) { From c321d9f8abea127196be03aa3f01354f2a4d9fb0 Mon Sep 17 00:00:00 2001 From: Juan Carlos Manzanares Serrano Date: Wed, 29 May 2024 14:56:40 +0200 Subject: [PATCH 4/4] Update README.md --- README.md | 23 +++++++++++++++++++---- 1 file changed, 19 insertions(+), 4 deletions(-) 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 ```