diff --git a/.ros1_unported/gazebo_plugins/include/gazebo_plugins/gazebo_ros_hand_of_god.h b/.ros1_unported/gazebo_plugins/include/gazebo_plugins/gazebo_ros_hand_of_god.h deleted file mode 100644 index 4773b8029..000000000 --- a/.ros1_unported/gazebo_plugins/include/gazebo_plugins/gazebo_ros_hand_of_god.h +++ /dev/null @@ -1,71 +0,0 @@ -/* - * Copyright (C) 2012-2014 Open Source Robotics Foundation - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * -*/ -/* - * Desc: 3D position interface. - * Author: Sachin Chitta and John Hsu - * Date: 10 June 2008 - */ - -#ifndef GAZEBO_ROS_TEMPLATE_HH -#define GAZEBO_ROS_TEMPLATE_HH - -#include - -#include -#include -#include -#include -#include - -#include -#include - -namespace gazebo -{ - - class GazeboRosHandOfGod : public ModelPlugin - { - /// \brief Constructor - public: GazeboRosHandOfGod(); - - /// \brief Destructor - public: virtual ~GazeboRosHandOfGod(); - - /// \brief Load the controller - public: void Load( physics::ModelPtr _parent, sdf::ElementPtr _sdf ); - - /// \brief Update the controller - protected: virtual void GazeboUpdate(); - - /// Pointer to the update event connection - private: event::ConnectionPtr update_connection_; - boost::shared_ptr tf_buffer_; - boost::shared_ptr tf_listener_; - boost::shared_ptr tf_broadcaster_; - physics::ModelPtr model_; - physics::LinkPtr floating_link_; - std::string link_name_; - std::string robot_namespace_; - std::string frame_id_; - double kl_, ka_; - double cl_, ca_; - }; - -} - -#endif - diff --git a/.ros1_unported/gazebo_plugins/src/gazebo_ros_hand_of_god.cpp b/.ros1_unported/gazebo_plugins/src/gazebo_ros_hand_of_god.cpp deleted file mode 100644 index 2725febde..000000000 --- a/.ros1_unported/gazebo_plugins/src/gazebo_ros_hand_of_god.cpp +++ /dev/null @@ -1,204 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2013, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Open Source Robotics Foundation - * nor the names of its contributors may be - * used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/** - * \author Jonathan Bohren - * \desc A "hand-of-god" plugin which drives a floating object around based - * on the location of a TF frame. This plugin is useful for connecting human input - * devices to "god-like" objects in a Gazebo simulation. - */ - -#include -#include - -namespace gazebo -{ - // Register this plugin with the simulator - GZ_REGISTER_MODEL_PLUGIN(GazeboRosHandOfGod); - - //////////////////////////////////////////////////////////////////////////////// - // Constructor - GazeboRosHandOfGod::GazeboRosHandOfGod() : - ModelPlugin(), - robot_namespace_(""), - frame_id_("hog"), - kl_(200), - ka_(200) - { - } - - //////////////////////////////////////////////////////////////////////////////// - // Destructor - GazeboRosHandOfGod::~GazeboRosHandOfGod() - { - } - - //////////////////////////////////////////////////////////////////////////////// - // Load the controller - void GazeboRosHandOfGod::Load( physics::ModelPtr _parent, sdf::ElementPtr _sdf ) - { - // Make sure the ROS node for Gazebo has already been initalized - if (!ros::isInitialized()) { - ROS_FATAL_STREAM_NAMED("hand_of_god", "A ROS node for Gazebo has not been initialized, unable to load plugin. " - << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)"); - return; - } - - // Get sdf parameters - if(_sdf->HasElement("robotNamespace")) { - this->robot_namespace_ = _sdf->Get("robotNamespace") + "/"; - } - - if(_sdf->HasElement("frameId")) { - this->frame_id_ = _sdf->Get("frameId"); - } - - if(_sdf->HasElement("kl")) { - this->kl_ = _sdf->Get("kl"); - } - if(_sdf->HasElement("ka")) { - this->ka_ = _sdf->Get("ka"); - } - - if(_sdf->HasElement("linkName")) { - this->link_name_ = _sdf->Get("linkName"); - } else { - ROS_FATAL_STREAM_NAMED("hand_of_god", "The hand-of-god plugin requires a `linkName` parameter tag"); - return; - } - - // Store the model - model_ = _parent; - - // Get the floating link - floating_link_ = model_->GetLink(link_name_); - // Disable gravity for the hog - floating_link_->SetGravityMode(false); - if(!floating_link_) { - ROS_ERROR_NAMED("hand_of_god", "Floating link not found"); - const std::vector &links = model_->GetLinks(); - for(unsigned i=0; i < links.size(); i++) { - ROS_ERROR_STREAM_NAMED("hand_of_god", " -- Link "<GetName()); - } - return; - } - -#if GAZEBO_MAJOR_VERSION >= 8 - cl_ = 2.0 * sqrt(kl_*floating_link_->GetInertial()->Mass()); - ca_ = 2.0 * sqrt(ka_*floating_link_->GetInertial()->IXX()); -#else - cl_ = 2.0 * sqrt(kl_*floating_link_->GetInertial()->GetMass()); - ca_ = 2.0 * sqrt(ka_*floating_link_->GetInertial()->GetIXX()); -#endif - - // Create the TF listener for the desired position of the hog - tf_buffer_.reset(new tf2_ros::Buffer()); - tf_listener_.reset(new tf2_ros::TransformListener(*tf_buffer_)); - tf_broadcaster_.reset(new tf2_ros::TransformBroadcaster()); - - // Register update event handler - this->update_connection_ = event::Events::ConnectWorldUpdateBegin( - boost::bind(&GazeboRosHandOfGod::GazeboUpdate, this)); - } - - //////////////////////////////////////////////////////////////////////////////// - // Update the controller - void GazeboRosHandOfGod::GazeboUpdate() - { - // Get TF transform relative to the /world link - geometry_msgs::TransformStamped hog_desired_tform; - static bool errored = false; - try{ - hog_desired_tform = tf_buffer_->lookupTransform("world", frame_id_+"_desired", ros::Time(0)); - errored = false; - } catch (tf2::TransformException ex){ - if(!errored) { - ROS_ERROR_NAMED("hand_of_god", "%s",ex.what()); - errored = true; - } - return; - } - - // Convert TF transform to Gazebo Pose - const geometry_msgs::Vector3 &p = hog_desired_tform.transform.translation; - const geometry_msgs::Quaternion &q = hog_desired_tform.transform.rotation; - ignition::math::Pose3d hog_desired( - ignition::math::Vector3d(p.x, p.y, p.z), - ignition::math::Quaterniond(q.w, q.x, q.y, q.z)); - - // Relative transform from actual to desired pose -#if GAZEBO_MAJOR_VERSION >= 8 - ignition::math::Pose3d world_pose = floating_link_->DirtyPose(); - ignition::math::Vector3d worldLinearVel = floating_link_->WorldLinearVel(); - ignition::math::Vector3d relativeAngularVel = floating_link_->RelativeAngularVel(); -#else - ignition::math::Pose3d world_pose = floating_link_->GetDirtyPose().Ign(); - ignition::math::Vector3d worldLinearVel = floating_link_->GetWorldLinearVel().Ign(); - ignition::math::Vector3d relativeAngularVel = floating_link_->GetRelativeAngularVel().Ign(); -#endif - ignition::math::Vector3d err_pos = hog_desired.Pos() - world_pose.Pos(); - // Get exponential coordinates for rotation - ignition::math::Quaterniond err_rot = (ignition::math::Matrix4d(world_pose.Rot()).Inverse() - * ignition::math::Matrix4d(hog_desired.Rot())).Rotation(); - ignition::math::Quaterniond not_a_quaternion = err_rot.Log(); - - floating_link_->AddForce( - kl_ * err_pos - cl_ * worldLinearVel); - - floating_link_->AddRelativeTorque( - ka_ * ignition::math::Vector3d(not_a_quaternion.X(), not_a_quaternion.Y(), not_a_quaternion.Z()) - - ca_ * relativeAngularVel); - - // Convert actual pose to TransformStamped message - geometry_msgs::TransformStamped hog_actual_tform; - - hog_actual_tform.header.frame_id = "world"; - hog_actual_tform.header.stamp = ros::Time::now(); - - hog_actual_tform.child_frame_id = frame_id_ + "_actual"; - - hog_actual_tform.transform.translation.x = world_pose.Pos().X(); - hog_actual_tform.transform.translation.y = world_pose.Pos().Y(); - hog_actual_tform.transform.translation.z = world_pose.Pos().Z(); - - hog_actual_tform.transform.rotation.w = world_pose.Rot().W(); - hog_actual_tform.transform.rotation.x = world_pose.Rot().X(); - hog_actual_tform.transform.rotation.y = world_pose.Rot().Y(); - hog_actual_tform.transform.rotation.z = world_pose.Rot().Z(); - - tf_broadcaster_->sendTransform(hog_actual_tform); - } - -} diff --git a/gazebo_plugins/CMakeLists.txt b/gazebo_plugins/CMakeLists.txt index 2f745ed16..1721e0380 100644 --- a/gazebo_plugins/CMakeLists.txt +++ b/gazebo_plugins/CMakeLists.txt @@ -201,6 +201,20 @@ target_link_libraries(gazebo_ros_harness ) ament_export_libraries(gazebo_ros_harness) +# gazebo_ros_hand_of_god +add_library(gazebo_ros_hand_of_god SHARED + src/gazebo_ros_hand_of_god.cpp +) +ament_target_dependencies(gazebo_ros_hand_of_god + "gazebo_ros" + "gazebo_dev" + "rclcpp" + "tf2" + "tf2_geometry_msgs" + "tf2_ros" +) +ament_export_libraries(gazebo_ros_hand_of_god) + # gazebo_ros_ackermann_drive add_library(gazebo_ros_ackermann_drive SHARED src/gazebo_ros_ackermann_drive.cpp @@ -282,6 +296,7 @@ install(TARGETS gazebo_ros_elevator gazebo_ros_force gazebo_ros_ft_sensor + gazebo_ros_hand_of_god gazebo_ros_harness gazebo_ros_imu_sensor gazebo_ros_joint_state_publisher diff --git a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_hand_of_god.hpp b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_hand_of_god.hpp new file mode 100644 index 000000000..653871744 --- /dev/null +++ b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_hand_of_god.hpp @@ -0,0 +1,73 @@ +// Copyright 2019 Open Source Robotics Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef GAZEBO_PLUGINS__GAZEBO_ROS_HAND_OF_GOD_HPP_ +#define GAZEBO_PLUGINS__GAZEBO_ROS_HAND_OF_GOD_HPP_ + +#include + +#include + +namespace gazebo_plugins +{ +class GazeboRosHandOfGodPrivate; + +/// Drives a floating object around based on the location of a TF frame. +/// It is useful for connecting human input +/** + Example Usage: + \code{.xml} + + + + + + /demo + + + + + link + + + + link + + + 200 + 200 + + + \endcode +*/ +class GazeboRosHandOfGod : public gazebo::ModelPlugin +{ +public: + /// Constructor + GazeboRosHandOfGod(); + + /// Destructor + ~GazeboRosHandOfGod(); + +protected: + // Documentation inherited + void Load(gazebo::physics::ModelPtr _model, sdf::ElementPtr _sdf) override; + +private: + /// Private data pointer + std::unique_ptr impl_; +}; +} // namespace gazebo_plugins + +#endif // GAZEBO_PLUGINS__GAZEBO_ROS_HAND_OF_GOD_HPP_ diff --git a/gazebo_plugins/src/gazebo_ros_hand_of_god.cpp b/gazebo_plugins/src/gazebo_ros_hand_of_god.cpp new file mode 100644 index 000000000..f568772a1 --- /dev/null +++ b/gazebo_plugins/src/gazebo_ros_hand_of_god.cpp @@ -0,0 +1,182 @@ +// Copyright (c) 2013, Open Source Robotics Foundation +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of the company nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +/* + * \file gazebo_ros_hand_of_god.cpp + * + * \brief A "hand-of-god" plugin which drives a floating object around based + * on the location of a TF frame. This plugin is useful for connecting human input + * + * \author Jonathan Bohren + * + */ + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +namespace gazebo_plugins +{ +class GazeboRosHandOfGodPrivate +{ +public: + /// Callback to be called at every simulation iteration. + void OnUpdate(); + + /// A pointer to the GazeboROS node. + gazebo_ros::Node::SharedPtr ros_node_; + + /// Pointer to link. + gazebo::physics::LinkPtr link_; + + /// TF buffer + std::shared_ptr buffer_; + + /// TF listener + std::shared_ptr transform_listener_; + + /// To broadcast TFs + std::shared_ptr transform_broadcaster_; + + /// frame ID + std::string frame_; + + /// Applied force and torque gains + double kl_, ka_, cl_, ca_; + + /// Connection to event called at every world iteration. + gazebo::event::ConnectionPtr update_connection_; +}; + +GazeboRosHandOfGod::GazeboRosHandOfGod() +: impl_(std::make_unique()) +{ +} + +GazeboRosHandOfGod::~GazeboRosHandOfGod() +{ +} + +void GazeboRosHandOfGod::Load(gazebo::physics::ModelPtr _model, sdf::ElementPtr _sdf) +{ + // Initialize ROS node + impl_->ros_node_ = gazebo_ros::Node::Get(_sdf); + + impl_->frame_ = _sdf->Get("frame_id", "world").first; + + impl_->kl_ = _sdf->Get("kl", 200).first; + impl_->ka_ = _sdf->Get("ka", 200).first; + + if (_sdf->HasElement("link_name")) { + auto link_name = _sdf->Get("link_name"); + impl_->link_ = _model->GetLink(link_name); + if (!impl_->link_) { + RCLCPP_ERROR(impl_->ros_node_->get_logger(), + "Link [%s] not found. Aborting", link_name.c_str()); + impl_->ros_node_.reset(); + return; + } + } else { + RCLCPP_ERROR(impl_->ros_node_->get_logger(), "Please specify . Aborting"); + impl_->ros_node_.reset(); + return; + } + + impl_->link_->SetGravityMode(false); + + impl_->cl_ = 2.0 * sqrt(impl_->kl_ * impl_->link_->GetInertial()->Mass()); + impl_->ca_ = 2.0 * sqrt(impl_->ka_ * impl_->link_->GetInertial()->IXX()); + + impl_->buffer_ = std::make_shared(impl_->ros_node_->get_clock()); + impl_->transform_listener_ = std::make_shared(*impl_->buffer_); + impl_->transform_broadcaster_ = std::make_shared(impl_->ros_node_); + + // Listen to the update event (broadcast every simulation iteration) + impl_->update_connection_ = gazebo::event::Events::ConnectWorldUpdateBegin( + std::bind(&GazeboRosHandOfGodPrivate::OnUpdate, impl_.get())); +} + +void GazeboRosHandOfGodPrivate::OnUpdate() +{ + // Get TF transform relative to the /world frame + geometry_msgs::msg::TransformStamped hog_desired_tform; + try { + hog_desired_tform = buffer_->lookupTransform("world", frame_ + "_desired", tf2::TimePointZero); + } catch (const tf2::TransformException & ex) { + RCLCPP_WARN_ONCE(ros_node_->get_logger(), "%s", ex.what()); + return; + } + + // Convert TF transform to Gazebo Pose + auto hog_desired = gazebo_ros::Convert(hog_desired_tform.transform); + + // Relative transform from actual to desired pose + ignition::math::Pose3d world_pose = link_->DirtyPose(); + ignition::math::Vector3d world_linear_vel = link_->WorldLinearVel(); + ignition::math::Vector3d relative_angular_vel = link_->RelativeAngularVel(); + + ignition::math::Vector3d err_pos = hog_desired.Pos() - world_pose.Pos(); + // Get exponential coordinates for rotation + ignition::math::Quaterniond err_rot = (ignition::math::Matrix4d(world_pose.Rot()).Inverse() * + ignition::math::Matrix4d(hog_desired.Rot())).Rotation(); + + ignition::math::Vector3d err_vec(err_rot.Log().X(), err_rot.Log().Y(), err_rot.Log().Z()); + + link_->AddForce(kl_ * err_pos - cl_ * world_linear_vel); + + link_->AddRelativeTorque(ka_ * err_vec - ca_ * relative_angular_vel); + + // Convert actual pose to TransformStamped message + geometry_msgs::msg::TransformStamped hog_actual_tform; + + hog_actual_tform.header.frame_id = "world"; + hog_actual_tform.header.stamp = ros_node_->now(); + + hog_actual_tform.child_frame_id = frame_ + "_actual"; + + hog_actual_tform.transform = gazebo_ros::Convert(world_pose); + + transform_broadcaster_->sendTransform(hog_actual_tform); +} + +GZ_REGISTER_MODEL_PLUGIN(GazeboRosHandOfGod) +} // namespace gazebo_plugins diff --git a/gazebo_plugins/test/CMakeLists.txt b/gazebo_plugins/test/CMakeLists.txt index d83d19897..884643cd0 100644 --- a/gazebo_plugins/test/CMakeLists.txt +++ b/gazebo_plugins/test/CMakeLists.txt @@ -17,6 +17,7 @@ set(tests test_gazebo_ros_elevator test_gazebo_ros_force test_gazebo_ros_ft_sensor + test_gazebo_ros_hand_of_god test_gazebo_ros_harness test_gazebo_ros_imu_sensor test_gazebo_ros_joint_state_publisher @@ -59,6 +60,9 @@ foreach(test ${tests}) "rclcpp" "sensor_msgs" "std_msgs" + "tf2" + "tf2_geometry_msgs" + "tf2_ros" ) endforeach() diff --git a/gazebo_plugins/test/test_gazebo_ros_hand_of_god.cpp b/gazebo_plugins/test/test_gazebo_ros_hand_of_god.cpp new file mode 100644 index 000000000..a21712f21 --- /dev/null +++ b/gazebo_plugins/test/test_gazebo_ros_hand_of_god.cpp @@ -0,0 +1,86 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include + +#define tol 10e-2 + +class GazeboRosHandOfGodTest : public gazebo::ServerFixture +{ +}; + +TEST_F(GazeboRosHandOfGodTest, HandOfGodTransform) +{ + // Load test world and start paused + this->Load("worlds/gazebo_ros_hand_of_god.world", true); + + // World + auto world = gazebo::physics::get_world(); + ASSERT_NE(nullptr, world); + + // Box + auto box = world->ModelByName("box"); + ASSERT_NE(nullptr, box); + + // Link + auto link = box->GetLink("link"); + ASSERT_NE(nullptr, link); + + world->Step(100); + + // Check box is at world origin + EXPECT_NEAR(0.0, box->WorldPose().Pos().X(), tol); + EXPECT_NEAR(0.0, box->WorldPose().Pos().Y(), tol); + // Height of box's center + EXPECT_NEAR(0.5, box->WorldPose().Pos().Z(), tol); + + // Create ROS node + auto node = std::make_shared("gazebo_ros_hand_of_god_test"); + ASSERT_NE(nullptr, node); + + auto tf_broadcaster = std::make_shared(node); + + geometry_msgs::msg::TransformStamped msg; + msg.header.frame_id = "world"; + msg.child_frame_id = "link_desired"; + msg.header.stamp = node->now(); + msg.transform.translation.z = 10; + msg.transform.rotation.w = 1; + + unsigned int sleep = 0; + unsigned int max_sleep = 30; + while (sleep < max_sleep) { + tf_broadcaster->sendTransform(msg); + gazebo::common::Time::MSleep(100); + world->Step(100); + sleep++; + } + + // Check box moved + EXPECT_NEAR(0, box->WorldPose().Pos().X(), tol); + EXPECT_NEAR(0, box->WorldPose().Pos().Y(), tol); + EXPECT_NEAR(10, box->WorldPose().Pos().Z(), tol); +} + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/gazebo_plugins/test/worlds/gazebo_ros_hand_of_god.world b/gazebo_plugins/test/worlds/gazebo_ros_hand_of_god.world new file mode 100644 index 000000000..6451e76f4 --- /dev/null +++ b/gazebo_plugins/test/worlds/gazebo_ros_hand_of_god.world @@ -0,0 +1,34 @@ + + + + + model://ground_plane + + + 0 0 0.5 0 0 0 + + + + + 1 1 1 + + + + + + + 1 1 1 + + + + + + + /test + + link + link + + + + diff --git a/gazebo_plugins/worlds/gazebo_ros_hand_of_god_demo.world b/gazebo_plugins/worlds/gazebo_ros_hand_of_god_demo.world new file mode 100644 index 000000000..14148bef8 --- /dev/null +++ b/gazebo_plugins/worlds/gazebo_ros_hand_of_god_demo.world @@ -0,0 +1,48 @@ + + + + + + model://ground_plane + + + model://sun + + + 0 0 0.5 0 0 0 + + + + + 1 1 1 + + + + + + + 1 1 1 + + + + + + + /demo + + link + link + + + + diff --git a/gazebo_ros/include/gazebo_ros/conversions/geometry_msgs.hpp b/gazebo_ros/include/gazebo_ros/conversions/geometry_msgs.hpp index 0dbc343f5..310a92216 100644 --- a/gazebo_ros/include/gazebo_ros/conversions/geometry_msgs.hpp +++ b/gazebo_ros/include/gazebo_ros/conversions/geometry_msgs.hpp @@ -21,6 +21,7 @@ #include #include #include +#include #include #include @@ -174,6 +175,18 @@ geometry_msgs::msg::Quaternion Convert(const ignition::math::Quaterniond & in) return msg; } +/// \brief Specialized conversion from an Ignition Math Pose3d to a ROS geometry transform message. +/// \param[in] in Ignition Pose3d to convert. +/// \return ROS geometry transform message +template<> +geometry_msgs::msg::Transform Convert(const ignition::math::Pose3d & in) +{ + geometry_msgs::msg::Transform msg; + msg.translation = Convert(in.Pos()); + msg.rotation = Convert(in.Rot()); + return msg; +} + /// \brief Specialized conversion from an Ignition Math Pose3d to a ROS geometry pose message. /// \param[in] in Ignition Pose3d to convert. /// \return ROS geometry pose message @@ -205,5 +218,27 @@ ignition::math::Quaterniond Convert(const geometry_msgs::msg::Quaternion & in) return ignition::math::Quaterniond(in.w, in.x, in.y, in.z); } +/// Generic conversion from a ROS geometry transform message to another type. +/// \param[in] in Input message. +/// \return Conversion result +/// \tparam T Output type +template +T Convert(const geometry_msgs::msg::Transform &) +{ + T::ConversionNotImplemented; +} + +/// \brief Specialized conversion from a ROS geometry transform message to an Ignition math pose3d. +/// \param[in] in ROS message to convert. +/// \return A Ignition Math pose3d. +template<> +ignition::math::Pose3d Convert(const geometry_msgs::msg::Transform & in) +{ + ignition::math::Pose3d msg; + msg.Pos() = Convert(in.translation); + msg.Rot() = Convert(in.rotation); + return msg; +} + } // namespace gazebo_ros #endif // GAZEBO_ROS__CONVERSIONS__GEOMETRY_MSGS_HPP_