From 0b7918e17c481bc113a0f4193a0a8ac9c08e00a2 Mon Sep 17 00:00:00 2001 From: Shivesh Khaitan Date: Tue, 16 Jul 2019 20:35:54 +0530 Subject: [PATCH] [ros2] Port bumper sensor to ROS2 (#943) * [ros2] Port bumper sensor to ROS2 * Add author name * Minor fixes and add contact msg conversion * Remove unused header includes --- .../gazebo_plugins/gazebo_ros_bumper.h | 93 ----- .../gazebo_plugins/src/gazebo_ros_bumper.cpp | 330 ------------------ gazebo_plugins/CMakeLists.txt | 14 + .../gazebo_plugins/gazebo_ros_bumper.hpp | 63 ++++ gazebo_plugins/package.xml | 2 + gazebo_plugins/src/gazebo_ros_bumper.cpp | 110 ++++++ gazebo_plugins/test/CMakeLists.txt | 2 + .../test/test_gazebo_ros_bumper.cpp | 86 +++++ .../test/worlds/gazebo_ros_bumper.world | 54 +++ .../worlds/gazebo_ros_bumper_demo.world | 123 +++++++ .../gazebo_ros/conversions/gazebo_msgs.hpp | 131 +++++++ gazebo_ros/test/test_conversions.cpp | 1 - 12 files changed, 585 insertions(+), 424 deletions(-) delete mode 100644 .ros1_unported/gazebo_plugins/include/gazebo_plugins/gazebo_ros_bumper.h delete mode 100644 .ros1_unported/gazebo_plugins/src/gazebo_ros_bumper.cpp create mode 100644 gazebo_plugins/include/gazebo_plugins/gazebo_ros_bumper.hpp create mode 100644 gazebo_plugins/src/gazebo_ros_bumper.cpp create mode 100644 gazebo_plugins/test/test_gazebo_ros_bumper.cpp create mode 100644 gazebo_plugins/test/worlds/gazebo_ros_bumper.world create mode 100644 gazebo_plugins/worlds/gazebo_ros_bumper_demo.world create mode 100644 gazebo_ros/include/gazebo_ros/conversions/gazebo_msgs.hpp diff --git a/.ros1_unported/gazebo_plugins/include/gazebo_plugins/gazebo_ros_bumper.h b/.ros1_unported/gazebo_plugins/include/gazebo_plugins/gazebo_ros_bumper.h deleted file mode 100644 index 35a2b267b..000000000 --- a/.ros1_unported/gazebo_plugins/include/gazebo_plugins/gazebo_ros_bumper.h +++ /dev/null @@ -1,93 +0,0 @@ -/* - * Copyright 2012 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_ROS_BUMPER_HH -#define GAZEBO_ROS_BUMPER_HH - -#include - -#include -#include -#include - -#include - -#include -#include - -#include - -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace gazebo -{ - /// \brief A Bumper controller - class GazeboRosBumper : public SensorPlugin - { - /// Constructor - public: GazeboRosBumper(); - - /// Destructor - public: ~GazeboRosBumper(); - - /// \brief Load the plugin - /// \param take in SDF root element - public: void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf); - - /// Update the controller - private: void OnContact(); - - /// \brief pointer to ros node - private: ros::NodeHandle* rosnode_; - private: ros::Publisher contact_pub_; - - private: sensors::ContactSensorPtr parentSensor; - - /// \brief set topic name of broadcast - private: std::string bumper_topic_name_; - - private: std::string frame_name_; - - /// \brief broadcast some string for now. - private: gazebo_msgs::ContactsState contact_state_msg_; - - /// \brief for setting ROS name space - private: std::string robot_namespace_; - - private: ros::CallbackQueue contact_queue_; - private: void ContactQueueThread(); - private: boost::thread callback_queue_thread_; - - // Pointer to the update event connection - private: event::ConnectionPtr update_connection_; - }; -} - -#endif - diff --git a/.ros1_unported/gazebo_plugins/src/gazebo_ros_bumper.cpp b/.ros1_unported/gazebo_plugins/src/gazebo_ros_bumper.cpp deleted file mode 100644 index 2c5ba04ba..000000000 --- a/.ros1_unported/gazebo_plugins/src/gazebo_ros_bumper.cpp +++ /dev/null @@ -1,330 +0,0 @@ -/* - * Copyright 2013 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: Bumper controller - * Author: Nate Koenig - * Date: 09 Sept. 2008 - */ - -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include -#include - -namespace gazebo -{ -// Register this plugin with the simulator -GZ_REGISTER_SENSOR_PLUGIN(GazeboRosBumper) - -//////////////////////////////////////////////////////////////////////////////// -// Constructor -GazeboRosBumper::GazeboRosBumper() : SensorPlugin() -{ -} - -//////////////////////////////////////////////////////////////////////////////// -// Destructor -GazeboRosBumper::~GazeboRosBumper() -{ - this->rosnode_->shutdown(); - this->callback_queue_thread_.join(); - - delete this->rosnode_; -} - -//////////////////////////////////////////////////////////////////////////////// -// Load the controller -void GazeboRosBumper::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf) -{ - GAZEBO_SENSORS_USING_DYNAMIC_POINTER_CAST; - this->parentSensor = dynamic_pointer_cast(_parent); - if (!this->parentSensor) - { - ROS_ERROR_NAMED("bumper", "Contact sensor parent is not of type ContactSensor"); - return; - } - - this->robot_namespace_ = ""; - if (_sdf->HasElement("robotNamespace")) - this->robot_namespace_ = - _sdf->GetElement("robotNamespace")->Get() + "/"; - - // "publishing contact/collisions to this topic name: " - // << this->bumper_topic_name_ << std::endl; - this->bumper_topic_name_ = "bumper_states"; - if (_sdf->HasElement("bumperTopicName")) - this->bumper_topic_name_ = - _sdf->GetElement("bumperTopicName")->Get(); - - // "transform contact/collisions pose, forces to this body (link) name: " - // << this->frame_name_ << std::endl; - if (!_sdf->HasElement("frameName")) - { - ROS_INFO_NAMED("bumper", "bumper plugin missing , defaults to world"); - this->frame_name_ = "world"; - } - else - this->frame_name_ = _sdf->GetElement("frameName")->Get(); - - // Make sure the ROS node for Gazebo has already been initialized - if (!ros::isInitialized()) - { - ROS_FATAL_STREAM_NAMED("bumper", "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; - } - - this->rosnode_ = new ros::NodeHandle(this->robot_namespace_); - - // resolve tf prefix - std::string prefix; - this->rosnode_->getParam(std::string("tf_prefix"), prefix); - this->frame_name_ = tf::resolve(prefix, this->frame_name_); - - this->contact_pub_ = this->rosnode_->advertise( - std::string(this->bumper_topic_name_), 1); - - // Initialize - // start custom queue for contact bumper - this->callback_queue_thread_ = boost::thread( - boost::bind(&GazeboRosBumper::ContactQueueThread, this)); - - // Listen to the update event. This event is broadcast every - // simulation iteration. - this->update_connection_ = this->parentSensor->ConnectUpdated( - boost::bind(&GazeboRosBumper::OnContact, this)); - - // Make sure the parent sensor is active. - this->parentSensor->SetActive(true); -} - -//////////////////////////////////////////////////////////////////////////////// -// Update the controller -void GazeboRosBumper::OnContact() -{ - if (this->contact_pub_.getNumSubscribers() <= 0) - return; - - msgs::Contacts contacts; - contacts = this->parentSensor->Contacts(); - /// \TODO: need a time for each Contact in i-loop, they may differ - this->contact_state_msg_.header.frame_id = this->frame_name_; - this->contact_state_msg_.header.stamp = ros::Time(contacts.time().sec(), - contacts.time().nsec()); - -/* - /// \TODO: get frame_name_ transforms from tf or gazebo - /// and rotate results to local frame. for now, results are reported in world frame. - - // if frameName specified is "world", "/map" or "map" report back - // inertial values in the gazebo world - physics::LinkPtr myFrame; - if (myFrame == NULL && this->frame_name_ != "world" && - this->frame_name_ != "/map" && this->frame_name_ != "map") - { - // lock in case a model is being spawned - boost::recursive_mutex::scoped_lock lock( - *Simulator::Instance()->GetMRMutex()); - // look through all models in the world, search for body - // name that matches frameName - physics::Model_V all_models = World::Instance()->Models(); - for (physics::Model_V::iterator iter = all_models.begin(); - iter != all_models.end(); iter++) - { - if (*iter) myFrame = - boost::dynamic_pointer_cast((*iter)->GetLink(this->frame_name_)); - if (myFrame) break; - } - - // not found - if (!myFrame) - { - ROS_INFO_NAMED("bumper", "gazebo_ros_bumper plugin: frameName: %s does not exist" - " yet, will not publish\n",this->frame_name_.c_str()); - return; - } - } -*/ - // get reference frame (body(link)) pose and subtract from it to get - // relative force, torque, position and normal vectors - ignition::math::Pose3d pose, frame_pose; - ignition::math::Quaterniond rot, frame_rot; - ignition::math::Vector3d pos, frame_pos; - /* - if (myFrame) - { - frame_pose = myFrame->WorldPose(); //-this->myBody->GetCoMPose(); - frame_pos = frame_pose.Pos(); - frame_rot = frame_pose.Rot(); - } - else - */ - { - // no specific frames specified, use identity pose, keeping - // relative frame at inertial origin - frame_pos = ignition::math::Vector3d(0, 0, 0); - frame_rot = ignition::math::Quaterniond(1, 0, 0, 0); // gazebo u,x,y,z == identity - frame_pose = ignition::math::Pose3d(frame_pos, frame_rot); - } - - - - // set contact states size - this->contact_state_msg_.states.clear(); - - // GetContacts returns all contacts on the collision body - unsigned int contactsPacketSize = contacts.contact_size(); - for (unsigned int i = 0; i < contactsPacketSize; ++i) - { - - // For each collision contact - // Create a ContactState - gazebo_msgs::ContactState state; - /// \TODO: - gazebo::msgs::Contact contact = contacts.contact(i); - - state.collision1_name = contact.collision1(); - state.collision2_name = contact.collision2(); - std::ostringstream stream; - stream << "Debug: i:(" << i << "/" << contactsPacketSize - << ") my geom:" << state.collision1_name - << " other geom:" << state.collision2_name - << " time:" << ros::Time(contact.time().sec(), contact.time().nsec()) - << std::endl; - state.info = stream.str(); - - state.wrenches.clear(); - state.contact_positions.clear(); - state.contact_normals.clear(); - state.depths.clear(); - - // sum up all wrenches for each DOF - geometry_msgs::Wrench total_wrench; - total_wrench.force.x = 0; - total_wrench.force.y = 0; - total_wrench.force.z = 0; - total_wrench.torque.x = 0; - total_wrench.torque.y = 0; - total_wrench.torque.z = 0; - - unsigned int contactGroupSize = contact.position_size(); - for (unsigned int j = 0; j < contactGroupSize; ++j) - { - // loop through individual contacts between collision1 and collision2 - // gzerr << j << " Position:" - // << contact.position(j).x() << " " - // << contact.position(j).y() << " " - // << contact.position(j).z() << "\n"; - // gzerr << " Normal:" - // << contact.normal(j).x() << " " - // << contact.normal(j).y() << " " - // << contact.normal(j).z() << "\n"; - // gzerr << " Depth:" << contact.depth(j) << "\n"; - - // Get force, torque and rotate into user specified frame. - // frame_rot is identity if world is used (default for now) - ignition::math::Vector3d force = frame_rot.RotateVectorReverse(ignition::math::Vector3d( - contact.wrench(j).body_1_wrench().force().x(), - contact.wrench(j).body_1_wrench().force().y(), - contact.wrench(j).body_1_wrench().force().z())); - ignition::math::Vector3d torque = frame_rot.RotateVectorReverse(ignition::math::Vector3d( - contact.wrench(j).body_1_wrench().torque().x(), - contact.wrench(j).body_1_wrench().torque().y(), - contact.wrench(j).body_1_wrench().torque().z())); - - // set wrenches - geometry_msgs::Wrench wrench; - wrench.force.x = force.X(); - wrench.force.y = force.Y(); - wrench.force.z = force.Z(); - wrench.torque.x = torque.X(); - wrench.torque.y = torque.Y(); - wrench.torque.z = torque.Z(); - state.wrenches.push_back(wrench); - - total_wrench.force.x += wrench.force.x; - total_wrench.force.y += wrench.force.y; - total_wrench.force.z += wrench.force.z; - total_wrench.torque.x += wrench.torque.x; - total_wrench.torque.y += wrench.torque.y; - total_wrench.torque.z += wrench.torque.z; - - // transform contact positions into relative frame - // set contact positions - ignition::math::Vector3d position = frame_rot.RotateVectorReverse( - ignition::math::Vector3d(contact.position(j).x(), - contact.position(j).y(), - contact.position(j).z()) - frame_pos); - geometry_msgs::Vector3 contact_position; - contact_position.x = position.X(); - contact_position.y = position.Y(); - contact_position.z = position.Z(); - state.contact_positions.push_back(contact_position); - - // rotate normal into user specified frame. - // frame_rot is identity if world is used. - ignition::math::Vector3d normal = frame_rot.RotateVectorReverse( - ignition::math::Vector3d(contact.normal(j).x(), - contact.normal(j).y(), - contact.normal(j).z())); - // set contact normals - geometry_msgs::Vector3 contact_normal; - contact_normal.x = normal.X(); - contact_normal.y = normal.Y(); - contact_normal.z = normal.Z(); - state.contact_normals.push_back(contact_normal); - - // set contact depth, interpenetration - state.depths.push_back(contact.depth(j)); - } - - state.total_wrench = total_wrench; - this->contact_state_msg_.states.push_back(state); - } - - this->contact_pub_.publish(this->contact_state_msg_); -} - - -//////////////////////////////////////////////////////////////////////////////// -// Put laser data to the interface -void GazeboRosBumper::ContactQueueThread() -{ - static const double timeout = 0.01; - - while (this->rosnode_->ok()) - { - this->contact_queue_.callAvailable(ros::WallDuration(timeout)); - } -} -} diff --git a/gazebo_plugins/CMakeLists.txt b/gazebo_plugins/CMakeLists.txt index 7048b3e96..dd32a4d2c 100644 --- a/gazebo_plugins/CMakeLists.txt +++ b/gazebo_plugins/CMakeLists.txt @@ -14,6 +14,7 @@ endif() find_package(ament_cmake REQUIRED) find_package(camera_info_manager REQUIRED) find_package(gazebo_dev REQUIRED) +find_package(gazebo_msgs REQUIRED) find_package(gazebo_ros REQUIRED) find_package(geometry_msgs REQUIRED) find_package(image_transport REQUIRED) @@ -187,9 +188,21 @@ ament_target_dependencies(gazebo_ros_ft_sensor ) ament_export_libraries(gazebo_ros_ft_sensor) +# gazebo_ros_bumper +add_library(gazebo_ros_bumper SHARED + src/gazebo_ros_bumper.cpp +) +ament_target_dependencies(gazebo_ros_bumper + "gazebo_ros" + "gazebo_msgs" + "geometry_msgs" +) +ament_export_libraries(gazebo_ros_bumper) + ament_export_include_directories(include) ament_export_dependencies(rclcpp) ament_export_dependencies(gazebo_dev) +ament_export_dependencies(gazebo_msgs) ament_export_dependencies(gazebo_ros) if(BUILD_TESTING) @@ -204,6 +217,7 @@ install(DIRECTORY include/ DESTINATION include) install(TARGETS + gazebo_ros_bumper gazebo_ros_camera gazebo_ros_diff_drive gazebo_ros_force diff --git a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_bumper.hpp b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_bumper.hpp new file mode 100644 index 000000000..e5f6f6222 --- /dev/null +++ b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_bumper.hpp @@ -0,0 +1,63 @@ +// 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. + +#ifndef GAZEBO_PLUGINS__GAZEBO_ROS_BUMPER_HPP_ +#define GAZEBO_PLUGINS__GAZEBO_ROS_BUMPER_HPP_ + +#include + +#include + +namespace gazebo_plugins +{ +class GazeboRosBumperPrivate; + +/// A plugin that publishes contact states of a body using contact sensor. +/** + Example Usage: + \code{.xml} + + + + custom_ns + bumper_states:=custom_topic + + + + custom_frame + + \endcode +*/ +class GazeboRosBumper : public gazebo::SensorPlugin +{ +public: + /// Constructor + GazeboRosBumper(); + + /// Destructor + virtual ~GazeboRosBumper(); + + // Documentation Inherited + void Load(gazebo::sensors::SensorPtr _parent, sdf::ElementPtr _sdf); + +private: + /// Private data pointer + std::unique_ptr impl_; +}; + +} // namespace gazebo_plugins + +#endif // GAZEBO_PLUGINS__GAZEBO_ROS_BUMPER_HPP_ diff --git a/gazebo_plugins/package.xml b/gazebo_plugins/package.xml index 63f313961..829fe7654 100644 --- a/gazebo_plugins/package.xml +++ b/gazebo_plugins/package.xml @@ -30,10 +30,12 @@ tf2_ros gazebo_dev + gazebo_msgs gazebo_ros rclcpp gazebo_dev + gazebo_msgs gazebo_ros rclcpp diff --git a/gazebo_plugins/src/gazebo_ros_bumper.cpp b/gazebo_plugins/src/gazebo_ros_bumper.cpp new file mode 100644 index 000000000..50ef37a91 --- /dev/null +++ b/gazebo_plugins/src/gazebo_ros_bumper.cpp @@ -0,0 +1,110 @@ +// 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. + +/* + * \desc Bumper controller + * \author Nate Koenig + * \date 09 Sept. 2008 + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +namespace gazebo_plugins +{ +class GazeboRosBumperPrivate +{ +public: + /// Callback to be called when sensor updates. + void OnUpdate(); + + /// A pointer to the GazeboROS node. + gazebo_ros::Node::SharedPtr ros_node_{nullptr}; + + /// Contact mesage publisher. + rclcpp::Publisher::SharedPtr pub_{nullptr}; + + /// Pointer to sensor + gazebo::sensors::ContactSensorPtr parent_sensor_; + + /// Frame name, to be used by TF. + std::string frame_name_; + + /// Connects to pre-render events. + gazebo::event::ConnectionPtr update_connection_; +}; + +GazeboRosBumper::GazeboRosBumper() +: impl_(std::make_unique()) +{ +} + +GazeboRosBumper::~GazeboRosBumper() +{ + impl_->ros_node_.reset(); +} + +void GazeboRosBumper::Load(gazebo::sensors::SensorPtr _sensor, sdf::ElementPtr _sdf) +{ + // Initialize ROS node + impl_->ros_node_ = gazebo_ros::Node::Get(_sdf); + + impl_->parent_sensor_ = std::dynamic_pointer_cast(_sensor); + if (!impl_->parent_sensor_) { + RCLCPP_ERROR(impl_->ros_node_->get_logger(), + "Contact sensor parent is not of type ContactSensor. Aborting"); + impl_->ros_node_.reset(); + return; + } + + // Contact state publisher + impl_->pub_ = impl_->ros_node_->create_publisher( + "bumper_states", rclcpp::SensorDataQoS()); + + RCLCPP_INFO(impl_->ros_node_->get_logger(), "Publishing contact states to [%s]", + impl_->pub_->get_topic_name()); + + // Get tf frame for output + impl_->frame_name_ = _sdf->Get("frame_name", "world").first; + + impl_->update_connection_ = impl_->parent_sensor_->ConnectUpdated( + std::bind(&GazeboRosBumperPrivate::OnUpdate, impl_.get())); + + impl_->parent_sensor_->SetActive(true); +} + +void GazeboRosBumperPrivate::OnUpdate() +{ + gazebo::msgs::Contacts contacts; + contacts = parent_sensor_->Contacts(); + + auto contact_state_msg = gazebo_ros::Convert(contacts); + contact_state_msg.header.frame_id = frame_name_; + + pub_->publish(contact_state_msg); +} + +GZ_REGISTER_SENSOR_PLUGIN(GazeboRosBumper) +} // namespace gazebo_plugins diff --git a/gazebo_plugins/test/CMakeLists.txt b/gazebo_plugins/test/CMakeLists.txt index 37cc5553b..af71fc692 100644 --- a/gazebo_plugins/test/CMakeLists.txt +++ b/gazebo_plugins/test/CMakeLists.txt @@ -12,6 +12,7 @@ endforeach() # Tests set(tests + test_gazebo_ros_bumper test_gazebo_ros_diff_drive test_gazebo_ros_force test_gazebo_ros_ft_sensor @@ -48,6 +49,7 @@ foreach(test ${tests}) ament_target_dependencies(${test} "cv_bridge" "gazebo_dev" + "gazebo_msgs" "gazebo_ros" "geometry_msgs" "image_transport" diff --git a/gazebo_plugins/test/test_gazebo_ros_bumper.cpp b/gazebo_plugins/test/test_gazebo_ros_bumper.cpp new file mode 100644 index 000000000..938cb9432 --- /dev/null +++ b/gazebo_plugins/test/test_gazebo_ros_bumper.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 + +#include + +#define tol 10e-2 + +/// Tests the gazebo_ros_bumper plugin +class GazeboRosBumperTest : public gazebo::ServerFixture +{ +}; + +TEST_F(GazeboRosBumperTest, BumperMessageCorrect) +{ + // Load test world and start paused + this->Load("worlds/gazebo_ros_bumper.world", true); + + // World + auto world = gazebo::physics::get_world(); + ASSERT_NE(nullptr, world); + + // Model + auto ball = world->ModelByName("ball"); + ASSERT_NE(nullptr, ball); + + // Make sure sensor is loaded + auto link = ball->GetLink("link"); + ASSERT_EQ(link->GetSensorCount(), 1u); + + // Create node / executor for receiving bumper contact message + auto node = std::make_shared("test_gazebo_ros_bumper"); + ASSERT_NE(nullptr, node); + + gazebo_msgs::msg::ContactsState::SharedPtr msg = nullptr; + auto sub = + node->create_subscription( + "/test/bumper_test", rclcpp::SensorDataQoS(), + [&msg](gazebo_msgs::msg::ContactsState::SharedPtr _msg) { + msg = _msg; + }); + + // Step until a contact message will have been published + int sleep{0}; + int max_sleep{30}; + while (sleep < max_sleep && nullptr == msg) { + world->Step(100); + rclcpp::spin_some(node); + gazebo::common::Time::MSleep(100); + sleep++; + } + EXPECT_LT(0u, sub->get_publisher_count()); + EXPECT_LT(sleep, max_sleep); + ASSERT_NE(nullptr, msg); + + EXPECT_NEAR(0.0, msg->states[0].total_wrench.force.x, tol); + EXPECT_NEAR(0.0, msg->states[0].total_wrench.force.y, tol); + // force z = body mass * gravity_acceleration + EXPECT_NEAR(0.25, msg->states[0].total_wrench.force.z, tol); + EXPECT_NEAR(0.0, msg->states[0].total_wrench.torque.x, tol); + EXPECT_NEAR(0.0, msg->states[0].total_wrench.torque.y, tol); + EXPECT_NEAR(0.0, msg->states[0].total_wrench.torque.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_bumper.world b/gazebo_plugins/test/worlds/gazebo_ros_bumper.world new file mode 100644 index 000000000..81c97fc50 --- /dev/null +++ b/gazebo_plugins/test/worlds/gazebo_ros_bumper.world @@ -0,0 +1,54 @@ + + + + + model://sun + + + model://ground_plane + + + + + 0.026 + + + 1.664e-5 + 1.664e-5 + 1.664e-5 + 0 + 0 + 0 + + + + + + 0.04 + + + + + + + 0.04 + + + + + -1 + + collision + + + + test + bumper_states:=bumper_test + + world + + + + + + diff --git a/gazebo_plugins/worlds/gazebo_ros_bumper_demo.world b/gazebo_plugins/worlds/gazebo_ros_bumper_demo.world new file mode 100644 index 000000000..2a5dcf56f --- /dev/null +++ b/gazebo_plugins/worlds/gazebo_ros_bumper_demo.world @@ -0,0 +1,123 @@ + + + + + + + model://sun + + + + true + + + + + 0 0 1 + 100 100 + + + + + + 100 + 50 + + + + 1.0 + 0 + + + + 10000 + 0.001 + + + + + + false + + + 0 0 1 + 100 100 + + + + + + + + + + + false + -1 0 1 0 0 0 + false + + + 0.026 + + + 1.664e-5 + 1.664e-5 + 1.664e-5 + 0 + 0 + 0 + + + + + + 0.04 + + + + + 1.0 + 0 + + + + 10000 + 0.001 + + + + + + + + 0.04 + + + + + 60 + + collision + + + + demo + bumper_states:=bumper_demo + + world + + + + + + diff --git a/gazebo_ros/include/gazebo_ros/conversions/gazebo_msgs.hpp b/gazebo_ros/include/gazebo_ros/conversions/gazebo_msgs.hpp new file mode 100644 index 000000000..f47b15913 --- /dev/null +++ b/gazebo_ros/include/gazebo_ros/conversions/gazebo_msgs.hpp @@ -0,0 +1,131 @@ +// 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. + +#ifndef GAZEBO_ROS__CONVERSIONS__GAZEBO_MSGS_HPP_ +#define GAZEBO_ROS__CONVERSIONS__GAZEBO_MSGS_HPP_ + +#include +#include +#include + +#include + +#include "gazebo_ros/conversions/builtin_interfaces.hpp" +#include "gazebo_ros/conversions/generic.hpp" +#include "gazebo_ros/conversions/geometry_msgs.hpp" + +namespace gazebo_ros +{ +/// Generic conversion from an Gazebo Contact message to another type. +/// \param[in] in Input message; +/// \return Conversion result +/// \tparam T Output type +template +T Convert(const gazebo::msgs::Contacts &) +{ + T::ConversionNotImplemented; +} + +/// \brief Specialized conversion from an Gazebo message to a ROS Contacts State. +/// \param[in] in Input message; +/// \return A ROS Contacts state message with the same data as the input message +template<> +gazebo_msgs::msg::ContactsState Convert(const gazebo::msgs::Contacts & in) +{ + gazebo_msgs::msg::ContactsState contact_state_msg; + contact_state_msg.header.stamp = Convert(in.time()); + + ignition::math::Quaterniond frame_rot; + ignition::math::Vector3d frame_pos; + + int contacts_packet_size = in.contact_size(); + for (int i = 0; i < contacts_packet_size; ++i) { + // For each collision contact + // Create a ContactState + gazebo_msgs::msg::ContactState state; + const gazebo::msgs::Contact & contact = in.contact(i); + + state.collision1_name = contact.collision1(); + state.collision2_name = contact.collision2(); + std::ostringstream stream; + stream << "Debug: i: (" << i + 1 << "/" << contacts_packet_size << + ") my_geom: [" << state.collision1_name << + "] other_geom: [" << state.collision2_name << + "] time: [" << contact.time().sec() << "." << contact.time().nsec() << "]\n"; + + state.info = stream.str(); + + // sum up all wrenches for each DOF + geometry_msgs::msg::Wrench total_wrench; + + int contact_group_size = contact.position_size(); + for (int j = 0; j < contact_group_size; ++j) { + // loop through individual contacts between collision1 and collision2 + + // Get force, torque and rotate into user specified frame. + // frame_rot is identity if world is used (default for now) + ignition::math::Vector3d force = frame_rot.RotateVectorReverse(ignition::math::Vector3d( + contact.wrench(j).body_1_wrench().force().x(), + contact.wrench(j).body_1_wrench().force().y(), + contact.wrench(j).body_1_wrench().force().z())); + ignition::math::Vector3d torque = frame_rot.RotateVectorReverse(ignition::math::Vector3d( + contact.wrench(j).body_1_wrench().torque().x(), + contact.wrench(j).body_1_wrench().torque().y(), + contact.wrench(j).body_1_wrench().torque().z())); + + // set wrenches + geometry_msgs::msg::Wrench wrench; + wrench.force = Convert(force); + wrench.torque = Convert(torque); + state.wrenches.push_back(wrench); + + total_wrench.force.x += wrench.force.x; + total_wrench.force.y += wrench.force.y; + total_wrench.force.z += wrench.force.z; + total_wrench.torque.x += wrench.torque.x; + total_wrench.torque.y += wrench.torque.y; + total_wrench.torque.z += wrench.torque.z; + + // transform contact positions into relative frame + // set contact positions + ignition::math::Vector3d position = + frame_rot.RotateVectorReverse(ignition::math::Vector3d( + contact.position(j).x(), contact.position(j).y(), contact.position(j).z()) - frame_pos); + + auto contact_position = Convert(position); + + state.contact_positions.push_back(contact_position); + + // rotate normal into user specified frame. + // frame_rot is identity if world is used. + ignition::math::Vector3d normal = + frame_rot.RotateVectorReverse(ignition::math::Vector3d( + contact.normal(j).x(), contact.normal(j).y(), contact.normal(j).z())); + // set contact normals + auto contact_normal = Convert(normal); + state.contact_normals.push_back(contact_normal); + + // set contact depth, interpenetration + state.depths.push_back(contact.depth(j)); + } + + state.total_wrench = total_wrench; + contact_state_msg.states.push_back(state); + } + + return contact_state_msg; +} + +} // namespace gazebo_ros +#endif // GAZEBO_ROS__CONVERSIONS__GAZEBO_MSGS_HPP_ diff --git a/gazebo_ros/test/test_conversions.cpp b/gazebo_ros/test/test_conversions.cpp index 6473c4cf4..821b5dfec 100644 --- a/gazebo_ros/test/test_conversions.cpp +++ b/gazebo_ros/test/test_conversions.cpp @@ -13,7 +13,6 @@ // limitations under the License. #include -#include #include #include