From 555e3ac0c87bd1240db7904cb733b2be483539f0 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Mon, 6 Aug 2018 18:27:30 -0700 Subject: [PATCH 1/6] Port joint_state_publisher, copyright failing checker, still need to add a test --- .../gazebo_ros_joint_state_publisher.h | 91 --------- .../src/gazebo_ros_joint_state_publisher.cpp | 158 ---------------- gazebo_plugins/CMakeLists.txt | 19 +- .../gazebo_ros_joint_state_publisher.hpp | 91 +++++++++ .../src/gazebo_ros_joint_state_publisher.cpp | 172 ++++++++++++++++++ ...azebo_ros_joint_state_publisher_demo.world | 34 ++++ gazebo_ros/include/gazebo_ros/conversions.hpp | 9 +- 7 files changed, 318 insertions(+), 256 deletions(-) delete mode 100644 .ros1_unported/gazebo_plugins/include/gazebo_plugins/gazebo_ros_joint_state_publisher.h delete mode 100644 .ros1_unported/gazebo_plugins/src/gazebo_ros_joint_state_publisher.cpp create mode 100644 gazebo_plugins/include/gazebo_plugins/gazebo_ros_joint_state_publisher.hpp create mode 100644 gazebo_plugins/src/gazebo_ros_joint_state_publisher.cpp create mode 100644 gazebo_plugins/worlds/gazebo_ros_joint_state_publisher_demo.world diff --git a/.ros1_unported/gazebo_plugins/include/gazebo_plugins/gazebo_ros_joint_state_publisher.h b/.ros1_unported/gazebo_plugins/include/gazebo_plugins/gazebo_ros_joint_state_publisher.h deleted file mode 100644 index 27999107b..000000000 --- a/.ros1_unported/gazebo_plugins/include/gazebo_plugins/gazebo_ros_joint_state_publisher.h +++ /dev/null @@ -1,91 +0,0 @@ -/* - * Copyright (c) 2013, Markus Bader - * 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 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 Antons Rebguns ''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 Antons Rebguns 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. - * - **/ - - -#ifndef JOINT_STATE_PUBLISHER_PLUGIN_HH -#define JOINT_STATE_PUBLISHER_PLUGIN_HH - -#include -#include -#include -#include -#include - -// ROS -#include -#include -#include - -// Usage in URDF: -// -// -// /pioneer2dx -// chassis_swivel_joint, swivel_wheel_joint, left_hub_joint, right_hub_joint -// 100.0 -// true -// -// - - - -namespace gazebo { -class GazeboRosJointStatePublisher : public ModelPlugin { -public: - GazeboRosJointStatePublisher(); - ~GazeboRosJointStatePublisher(); - void Load ( physics::ModelPtr _parent, sdf::ElementPtr _sdf ); - void OnUpdate ( const common::UpdateInfo & _info ); - void publishJointStates(); - // Pointer to the model -private: - event::ConnectionPtr updateConnection; - physics::WorldPtr world_; - physics::ModelPtr parent_; - std::vector joints_; - - // ROS STUFF - boost::shared_ptr rosnode_; - sensor_msgs::JointState joint_state_; - ros::Publisher joint_state_publisher_; - std::string tf_prefix_; - std::string robot_namespace_; - std::vector joint_names_; - - // Update Rate - double update_rate_; - double update_period_; - common::Time last_update_time_; - -}; - -// Register this plugin with the simulator -GZ_REGISTER_MODEL_PLUGIN ( GazeboRosJointStatePublisher ) -} - -#endif //JOINT_STATE_PUBLISHER_PLUGIN_HH - diff --git a/.ros1_unported/gazebo_plugins/src/gazebo_ros_joint_state_publisher.cpp b/.ros1_unported/gazebo_plugins/src/gazebo_ros_joint_state_publisher.cpp deleted file mode 100644 index c21843823..000000000 --- a/.ros1_unported/gazebo_plugins/src/gazebo_ros_joint_state_publisher.cpp +++ /dev/null @@ -1,158 +0,0 @@ -/* - * Copyright (c) 2013, Markus Bader - * 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 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 Antons Rebguns ''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 Antons Rebguns 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. - * - **/ -#include -#include -#include -#include - -using namespace gazebo; - -GazeboRosJointStatePublisher::GazeboRosJointStatePublisher() {} - -// Destructor -GazeboRosJointStatePublisher::~GazeboRosJointStatePublisher() { - rosnode_->shutdown(); -} - -void GazeboRosJointStatePublisher::Load ( physics::ModelPtr _parent, sdf::ElementPtr _sdf ) { - // Store the pointer to the model - this->parent_ = _parent; - this->world_ = _parent->GetWorld(); - - this->robot_namespace_ = parent_->GetName (); - if ( !_sdf->HasElement ( "robotNamespace" ) ) { - ROS_INFO_NAMED("joint_state_publisher", "GazeboRosJointStatePublisher Plugin missing , defaults to \"%s\"", - this->robot_namespace_.c_str() ); - } else { - this->robot_namespace_ = _sdf->GetElement ( "robotNamespace" )->Get(); - if ( this->robot_namespace_.empty() ) this->robot_namespace_ = parent_->GetName (); - } - if ( !robot_namespace_.empty() ) this->robot_namespace_ += "/"; - rosnode_ = boost::shared_ptr ( new ros::NodeHandle ( this->robot_namespace_ ) ); - - if ( !_sdf->HasElement ( "jointName" ) ) { - ROS_ASSERT ( "GazeboRosJointStatePublisher Plugin missing jointNames" ); - } else { - sdf::ElementPtr element = _sdf->GetElement ( "jointName" ) ; - std::string joint_names = element->Get(); - boost::erase_all ( joint_names, " " ); - boost::split ( joint_names_, joint_names, boost::is_any_of ( "," ) ); - } - - this->update_rate_ = 100.0; - if ( !_sdf->HasElement ( "updateRate" ) ) { - ROS_WARN_NAMED("joint_state_publisher", "GazeboRosJointStatePublisher Plugin (ns = %s) missing , defaults to %f", - this->robot_namespace_.c_str(), this->update_rate_ ); - } else { - this->update_rate_ = _sdf->GetElement ( "updateRate" )->Get(); - } - - // Initialize update rate stuff - if ( this->update_rate_ > 0.0 ) { - this->update_period_ = 1.0 / this->update_rate_; - } else { - this->update_period_ = 0.0; - } -#if GAZEBO_MAJOR_VERSION >= 8 - last_update_time_ = this->world_->SimTime(); -#else - last_update_time_ = this->world_->GetSimTime(); -#endif - - for ( unsigned int i = 0; i< joint_names_.size(); i++ ) { - physics::JointPtr joint = this->parent_->GetJoint(joint_names_[i]); - if (!joint) { - ROS_FATAL_NAMED("joint_state_publisher", "Joint %s does not exist!", joint_names_[i].c_str()); - } - joints_.push_back ( joint ); - ROS_INFO_NAMED("joint_state_publisher", "GazeboRosJointStatePublisher is going to publish joint: %s", joint_names_[i].c_str() ); - } - - ROS_INFO_NAMED("joint_state_publisher", "Starting GazeboRosJointStatePublisher Plugin (ns = %s)!, parent name: %s", this->robot_namespace_.c_str(), parent_->GetName ().c_str() ); - - tf_prefix_ = tf::getPrefixParam ( *rosnode_ ); - joint_state_publisher_ = rosnode_->advertise ( "joint_states",1000 ); - -#if GAZEBO_MAJOR_VERSION >= 8 - last_update_time_ = this->world_->SimTime(); -#else - last_update_time_ = this->world_->GetSimTime(); -#endif - // Listen to the update event. This event is broadcast every - // simulation iteration. - this->updateConnection = event::Events::ConnectWorldUpdateBegin ( - boost::bind ( &GazeboRosJointStatePublisher::OnUpdate, this, _1 ) ); -} - -void GazeboRosJointStatePublisher::OnUpdate ( const common::UpdateInfo & _info ) { - // Apply a small linear velocity to the model. -#if GAZEBO_MAJOR_VERSION >= 8 - common::Time current_time = this->world_->SimTime(); -#else - common::Time current_time = this->world_->GetSimTime(); -#endif - if (current_time < last_update_time_) - { - ROS_WARN_NAMED("joint_state_publisher", "Negative joint state update time difference detected."); - last_update_time_ = current_time; - } - - double seconds_since_last_update = ( current_time - last_update_time_ ).Double(); - - if ( seconds_since_last_update > update_period_ ) { - - publishJointStates(); - - last_update_time_+= common::Time ( update_period_ ); - - } - -} - -void GazeboRosJointStatePublisher::publishJointStates() { - ros::Time current_time = ros::Time::now(); - - joint_state_.header.stamp = current_time; - joint_state_.name.resize ( joints_.size() ); - joint_state_.position.resize ( joints_.size() ); - joint_state_.velocity.resize ( joints_.size() ); - - for ( int i = 0; i < joints_.size(); i++ ) { - physics::JointPtr joint = joints_[i]; - double velocity = joint->GetVelocity( 0 ); -#if GAZEBO_MAJOR_VERSION >= 8 - double position = joint->Position ( 0 ); -#else - double position = joint->GetAngle ( 0 ).Radian(); -#endif - joint_state_.name[i] = joint->GetName(); - joint_state_.position[i] = position; - joint_state_.velocity[i] = velocity; - } - joint_state_publisher_.publish ( joint_state_ ); -} diff --git a/gazebo_plugins/CMakeLists.txt b/gazebo_plugins/CMakeLists.txt index a1fc730cf..16d97bdca 100644 --- a/gazebo_plugins/CMakeLists.txt +++ b/gazebo_plugins/CMakeLists.txt @@ -16,21 +16,35 @@ find_package(gazebo_dev REQUIRED) find_package(gazebo_ros REQUIRED) find_package(geometry_msgs REQUIRED) find_package(rclcpp REQUIRED) +find_package(sensor_msgs REQUIRED) include_directories(include ${gazebo_ros_INCLUDE_DIRS} ${geometry_msgs_INCLUDE_DIRS} + ${sensor_msgs_INCLUDE_DIRS} ) +# gazebo_ros_joint_state_publisher +add_library(gazebo_ros_joint_state_publisher SHARED + src/gazebo_ros_joint_state_publisher.cpp +) +ament_target_dependencies(gazebo_ros_joint_state_publisher + "gazebo_dev" + "gazebo_ros" + "rclcpp" + "sensor_msgs" +) +ament_export_libraries(gazebo_ros_force) + # gazebo_ros_force add_library(gazebo_ros_force SHARED src/gazebo_ros_force.cpp ) ament_target_dependencies(gazebo_ros_force - "rclcpp" "gazebo_dev" "gazebo_ros" "geometry_msgs" + "rclcpp" ) ament_export_libraries(gazebo_ros_force) @@ -39,9 +53,9 @@ add_library(gazebo_ros_template SHARED src/gazebo_ros_template.cpp ) ament_target_dependencies(gazebo_ros_template - "rclcpp" "gazebo_dev" "gazebo_ros" + "rclcpp" ) ament_export_libraries(gazebo_ros_template) @@ -64,6 +78,7 @@ install(DIRECTORY include/ install( TARGETS gazebo_ros_force + gazebo_ros_joint_state_publisher gazebo_ros_template ARCHIVE DESTINATION lib LIBRARY DESTINATION lib diff --git a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_joint_state_publisher.hpp b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_joint_state_publisher.hpp new file mode 100644 index 000000000..11a904e0d --- /dev/null +++ b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_joint_state_publisher.hpp @@ -0,0 +1,91 @@ +// Copyright (c) 2013, Markus Bader +// 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 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 Antons Rebguns ''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 Antons Rebguns 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. + +#ifndef GAZEBO_PLUGINS__GAZEBO_ROS_JOINT_STATE_PUBLISHER_HPP_ +#define GAZEBO_PLUGINS__GAZEBO_ROS_JOINT_STATE_PUBLISHER_HPP_ + +#include + +#include + +namespace gazebo_plugins +{ +class GazeboRosJointStatePublisherPrivate; + +/// Publish the state of joints in simulation to a given ROS topic. +/** + Example Usage: + \code{.xml} + + + + + + my_joint_state_publisher + + + /ny_namespace + + + joint_states:=my_joint_states + + + + + 2 + + + left_wheel + right_wheel + elbow + shoulder + + + \endcode +*/ +class GazeboRosJointStatePublisher : public gazebo::ModelPlugin +{ +public: + /// Constructor + GazeboRosJointStatePublisher(); + + /// Destructor + ~GazeboRosJointStatePublisher(); + +protected: + // Documentation inherited + void Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf); + +private: + /// Callback to be called at every simulation iteration. + /// \param[in] info Updated simulation info. + void OnUpdate(const gazebo::common::UpdateInfo & info); + + /// Private data pointer + std::unique_ptr impl_; +}; +} // namespace gazebo_plugins +#endif // GAZEBO_PLUGINS__GAZEBO_ROS_JOINT_STATE_PUBLISHER_HPP_ diff --git a/gazebo_plugins/src/gazebo_ros_joint_state_publisher.cpp b/gazebo_plugins/src/gazebo_ros_joint_state_publisher.cpp new file mode 100644 index 000000000..4a53be352 --- /dev/null +++ b/gazebo_plugins/src/gazebo_ros_joint_state_publisher.cpp @@ -0,0 +1,172 @@ +// Copyright (c) 2013, Markus Bader +// 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 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 Antons Rebguns ''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 Antons Rebguns 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. + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace gazebo_plugins +{ +class GazeboRosJointStatePublisherPrivate +{ +public: + /// A pointer to the GazeboROS node. + gazebo_ros::Node::SharedPtr ros_node_; + + /// Joint state publisher. + rclcpp::Publisher::SharedPtr joint_state_pub_; + + /// Joints being tracked. + std::vector joints_; + + /// Period in seconds + double update_period_; + + /// Keep last time an update was published + gazebo::common::Time last_update_time_; + + /// Pointer to the update event connection. + gazebo::event::ConnectionPtr update_connection_; +}; + +GazeboRosJointStatePublisher::GazeboRosJointStatePublisher() +: impl_(std::make_unique()) +{ +} + +GazeboRosJointStatePublisher::~GazeboRosJointStatePublisher() +{ +} + +void GazeboRosJointStatePublisher::Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf) +{ + auto logger = rclcpp::get_logger("gazebo_ros_force"); + + // Joints + if (!sdf->HasElement("joint_name")) { + RCLCPP_ERROR(logger, "Plugin missing s"); + return; + } + + sdf::ElementPtr joint_elem = sdf->GetElement("joint_name"); + while (joint_elem) { + auto joint_name = joint_elem->Get(); + + auto joint = model->GetJoint(joint_name); + if (!joint) { + RCLCPP_ERROR(logger, "Joint %s does not exist!", joint_name.c_str()); + } else { + impl_->joints_.push_back(joint); + RCLCPP_INFO(logger, "Going to publish joint [%s]", joint_name.c_str() ); + } + + joint_elem = joint_elem->GetNextElement("joint_name"); + } + + if (impl_->joints_.empty()) { + RCLCPP_ERROR(logger, "No joints found."); + return; + } + + // Update rate + double update_rate = 100.0; + if (!sdf->HasElement("update_rate")) { + RCLCPP_INFO(logger, "Missing , defaults to %f", update_rate); + } else { + update_rate = sdf->GetElement("update_rate")->Get(); + } + + if (update_rate > 0.0) { + impl_->update_period_ = 1.0 / update_rate; + } else { + impl_->update_period_ = 0.0; + } + + impl_->last_update_time_ = model->GetWorld()->SimTime(); + + // Joint state publisher + impl_->ros_node_ = gazebo_ros::Node::Create("gazebo_ros_joint_state_publisher", sdf); + impl_->joint_state_pub_ = impl_->ros_node_->create_publisher( + "joint_states", 1000); + + // Callback on every iteration + impl_->update_connection_ = gazebo::event::Events::ConnectWorldUpdateBegin( + std::bind(&GazeboRosJointStatePublisher::OnUpdate, this, std::placeholders::_1)); +} + +void GazeboRosJointStatePublisher::OnUpdate(const gazebo::common::UpdateInfo & info) +{ + gazebo::common::Time current_time = info.simTime; + + // If the world is reset, for example + if (current_time < impl_->last_update_time_) { + RCLCPP_INFO(impl_->ros_node_->get_logger(), "Negative sim time difference detected."); + impl_->last_update_time_ = current_time; + } + + // Check period + double seconds_since_last_update = (current_time - impl_->last_update_time_).Double(); + + if (seconds_since_last_update < impl_->update_period_) { + return; + } + + // Pupolate message + sensor_msgs::msg::JointState joint_state; + + joint_state.header.stamp = gazebo_ros::Convert(current_time); + joint_state.name.resize(impl_->joints_.size()); + joint_state.position.resize(impl_->joints_.size()); + joint_state.velocity.resize(impl_->joints_.size()); + + for (unsigned int i = 0; i < impl_->joints_.size(); ++i) { + auto joint = impl_->joints_[i]; + double velocity = joint->GetVelocity(0); + double position = joint->Position(0); + joint_state.name[i] = joint->GetName(); + joint_state.position[i] = position; + joint_state.velocity[i] = velocity; + } + + // Publish + impl_->joint_state_pub_->publish(joint_state); + + // Update time + impl_->last_update_time_ = current_time; +} + +GZ_REGISTER_MODEL_PLUGIN(GazeboRosJointStatePublisher) +} // namespace gazebo_plugins diff --git a/gazebo_plugins/worlds/gazebo_ros_joint_state_publisher_demo.world b/gazebo_plugins/worlds/gazebo_ros_joint_state_publisher_demo.world new file mode 100644 index 000000000..a0b94c226 --- /dev/null +++ b/gazebo_plugins/worlds/gazebo_ros_joint_state_publisher_demo.world @@ -0,0 +1,34 @@ + + + + + + model://ground_plane + + + model://sun + + + model://double_pendulum_with_base + + + + joint_state_publisher_demo_node + /demo + joint_states:=joint_states_demo + + 2 + upper_joint + lower_joint + + + + + diff --git a/gazebo_ros/include/gazebo_ros/conversions.hpp b/gazebo_ros/include/gazebo_ros/conversions.hpp index c834d46b1..cc15bec1d 100644 --- a/gazebo_ros/include/gazebo_ros/conversions.hpp +++ b/gazebo_ros/include/gazebo_ros/conversions.hpp @@ -15,15 +15,14 @@ #ifndef GAZEBO_ROS__CONVERSIONS_HPP_ #define GAZEBO_ROS__CONVERSIONS_HPP_ -#include -#include #include +#include +#include +#include +#include #include -#include #include -#include - namespace gazebo_ros { /// Generic conversion from a ROS geometry vector message to another type. From 378c99d9a78667b0b620223c4e0715385dc9bb21 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Tue, 7 Aug 2018 11:27:55 -0700 Subject: [PATCH 2/6] Fix copyright --- .../gazebo_ros_joint_state_publisher.hpp | 49 +++++++++++-------- .../src/gazebo_ros_joint_state_publisher.cpp | 49 +++++++++++-------- 2 files changed, 56 insertions(+), 42 deletions(-) diff --git a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_joint_state_publisher.hpp b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_joint_state_publisher.hpp index 11a904e0d..5b8e658f3 100644 --- a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_joint_state_publisher.hpp +++ b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_joint_state_publisher.hpp @@ -1,27 +1,34 @@ // Copyright (c) 2013, Markus Bader -// All rights reserved. +// 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 nor the -// names of its contributors may be used to endorse or promote products -// derived from this software without specific prior written permission. +// Software License Agreement (BSD License 2.0) // -// THIS SOFTWARE IS PROVIDED BY Antons Rebguns ''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 Antons Rebguns 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. +// 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. #ifndef GAZEBO_PLUGINS__GAZEBO_ROS_JOINT_STATE_PUBLISHER_HPP_ #define GAZEBO_PLUGINS__GAZEBO_ROS_JOINT_STATE_PUBLISHER_HPP_ diff --git a/gazebo_plugins/src/gazebo_ros_joint_state_publisher.cpp b/gazebo_plugins/src/gazebo_ros_joint_state_publisher.cpp index 4a53be352..3f92920e0 100644 --- a/gazebo_plugins/src/gazebo_ros_joint_state_publisher.cpp +++ b/gazebo_plugins/src/gazebo_ros_joint_state_publisher.cpp @@ -1,27 +1,34 @@ // Copyright (c) 2013, Markus Bader -// All rights reserved. +// 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 nor the -// names of its contributors may be used to endorse or promote products -// derived from this software without specific prior written permission. +// Software License Agreement (BSD License 2.0) // -// THIS SOFTWARE IS PROVIDED BY Antons Rebguns ''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 Antons Rebguns 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. +// 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. #include #include From 83cfeabb9e1dd7034a373c0fb698ce179048b118 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Tue, 7 Aug 2018 16:31:50 -0700 Subject: [PATCH 3/6] Tests for joint state publisher --- .../gazebo_ros_joint_state_publisher.hpp | 2 + gazebo_plugins/package.xml | 1 + .../src/gazebo_ros_joint_state_publisher.cpp | 2 +- gazebo_plugins/test/CMakeLists.txt | 4 +- .../test_gazebo_ros_joint_state_publisher.cpp | 89 +++++++++++++++++++ .../gazebo_ros_joint_state_publisher.world | 81 +++++++++++++++++ gazebo_ros/package.xml | 1 + 7 files changed, 178 insertions(+), 2 deletions(-) create mode 100644 gazebo_plugins/test/test_gazebo_ros_joint_state_publisher.cpp create mode 100644 gazebo_plugins/test/worlds/gazebo_ros_joint_state_publisher.world diff --git a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_joint_state_publisher.hpp b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_joint_state_publisher.hpp index 5b8e658f3..6a4f975d9 100644 --- a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_joint_state_publisher.hpp +++ b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_joint_state_publisher.hpp @@ -43,6 +43,8 @@ class GazeboRosJointStatePublisherPrivate; /// Publish the state of joints in simulation to a given ROS topic. /** + \details If the joint contains more than one axis, only the state of the first axis is reported. + Example Usage: \code{.xml} geometry_msgs rclcpp + ament_cmake_gtest ament_lint_auto ament_lint_common diff --git a/gazebo_plugins/src/gazebo_ros_joint_state_publisher.cpp b/gazebo_plugins/src/gazebo_ros_joint_state_publisher.cpp index 3f92920e0..768442a85 100644 --- a/gazebo_plugins/src/gazebo_ros_joint_state_publisher.cpp +++ b/gazebo_plugins/src/gazebo_ros_joint_state_publisher.cpp @@ -151,7 +151,7 @@ void GazeboRosJointStatePublisher::OnUpdate(const gazebo::common::UpdateInfo & i return; } - // Pupolate message + // Populate message sensor_msgs::msg::JointState joint_state; joint_state.header.stamp = gazebo_ros::Convert(current_time); diff --git a/gazebo_plugins/test/CMakeLists.txt b/gazebo_plugins/test/CMakeLists.txt index 2baebd282..c5cfe3231 100644 --- a/gazebo_plugins/test/CMakeLists.txt +++ b/gazebo_plugins/test/CMakeLists.txt @@ -6,6 +6,7 @@ file(COPY worlds DESTINATION .) # Tests set(tests test_gazebo_ros_force + test_gazebo_ros_joint_state_publisher ) foreach(test ${tests}) @@ -21,10 +22,11 @@ foreach(test ${tests}) gazebo_test_fixture ) ament_target_dependencies(${test} - "rclcpp" "gazebo_dev" "gazebo_ros" "geometry_msgs" + "rclcpp" + "sensor_msgs" ) endforeach() diff --git a/gazebo_plugins/test/test_gazebo_ros_joint_state_publisher.cpp b/gazebo_plugins/test/test_gazebo_ros_joint_state_publisher.cpp new file mode 100644 index 000000000..c75dc9abf --- /dev/null +++ b/gazebo_plugins/test/test_gazebo_ros_joint_state_publisher.cpp @@ -0,0 +1,89 @@ +// Copyright 2018 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-10 + +using namespace std::literals::chrono_literals; // NOLINT + +class GazeboRosJointStatePublisherTest : public gazebo::ServerFixture +{ +}; + +TEST_F(GazeboRosJointStatePublisherTest, Publishing) +{ + // Load test world and start paused + this->Load("worlds/gazebo_ros_joint_state_publisher.world", true); + + // World + auto world = gazebo::physics::get_world(); + ASSERT_NE(nullptr, world); + + // Model + auto revoluter = world->ModelByName("revoluter"); + ASSERT_NE(nullptr, revoluter); + + // Joint + auto hinge = revoluter->GetJoint("hinge"); + ASSERT_NE(nullptr, hinge); + + // Check joint state + EXPECT_NEAR(0.0, hinge->Position(), tol); + EXPECT_NEAR(0.0, hinge->GetVelocity(0), tol); + + // Create node and executor + auto node = std::make_shared("gazebo_ros_joint_state_publisher_test"); + ASSERT_NE(nullptr, node); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node); + + // Create subscriber + sensor_msgs::msg::JointState::SharedPtr latestMsg; + auto sub = node->create_subscription( + "test/joint_states_test", + [&latestMsg](const sensor_msgs::msg::JointState::SharedPtr msg) { + latestMsg = msg; + }); + + // Check that we receive the latest joint state + for (unsigned int i = 0; i < 10; ++i) { + world->Step(100); + executor.spin_once(100ms); + gazebo::common::Time::MSleep(100); + + ASSERT_NE(nullptr, latestMsg); + + ASSERT_EQ(1u, latestMsg->name.size()); + ASSERT_EQ(1u, latestMsg->position.size()); + ASSERT_EQ(1u, latestMsg->velocity.size()); + + EXPECT_EQ("hinge", latestMsg->name[0]); + EXPECT_NEAR(hinge->Position(), latestMsg->position[0], tol); + EXPECT_NEAR(hinge->GetVelocity(0), latestMsg->velocity[0], 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_joint_state_publisher.world b/gazebo_plugins/test/worlds/gazebo_ros_joint_state_publisher.world new file mode 100644 index 000000000..2fc075660 --- /dev/null +++ b/gazebo_plugins/test/worlds/gazebo_ros_joint_state_publisher.world @@ -0,0 +1,81 @@ + + + + + model://ground_plane + + + model://sun + + + + 0.5 0.5 0.5 0 0 0 + + + 0 0 0 0 -0 0 + + + + 0.5 1 1 + + + + + + + 0.5 1 1 + + + + + + + 0.25 0 -0.2 0 -0 0 + + + + 0.05 + 0.5 + + + + + + + + 0.05 + 0.5 + + + + + + + base + needle + + 0 0 0 0 0 0 + + 1 0 0 + + + + + world + base + + + + + joint_state_publisher_test_node + /test + joint_states:=joint_states_test + + 100 + hinge + + + + + diff --git a/gazebo_ros/package.xml b/gazebo_ros/package.xml index 610681423..b3509a0c2 100644 --- a/gazebo_ros/package.xml +++ b/gazebo_ros/package.xml @@ -27,6 +27,7 @@ rclcpp gazebo_dev + ament_cmake_gtest ament_lint_auto ament_lint_common std_msgs From 683a0d78f5819df117f158b5c97d18767456d7e2 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Tue, 7 Aug 2018 16:44:19 -0700 Subject: [PATCH 4/6] cleanup --- .../gazebo_ros_joint_state_publisher.hpp | 5 +-- .../src/gazebo_ros_joint_state_publisher.cpp | 32 +++++++++++-------- 2 files changed, 19 insertions(+), 18 deletions(-) diff --git a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_joint_state_publisher.hpp b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_joint_state_publisher.hpp index 6a4f975d9..2db081cbb 100644 --- a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_joint_state_publisher.hpp +++ b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_joint_state_publisher.hpp @@ -86,13 +86,10 @@ class GazeboRosJointStatePublisher : public gazebo::ModelPlugin protected: // Documentation inherited - void Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf); + void Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf) override; private: /// Callback to be called at every simulation iteration. - /// \param[in] info Updated simulation info. - void OnUpdate(const gazebo::common::UpdateInfo & info); - /// Private data pointer std::unique_ptr impl_; }; diff --git a/gazebo_plugins/src/gazebo_ros_joint_state_publisher.cpp b/gazebo_plugins/src/gazebo_ros_joint_state_publisher.cpp index 768442a85..48a83bab3 100644 --- a/gazebo_plugins/src/gazebo_ros_joint_state_publisher.cpp +++ b/gazebo_plugins/src/gazebo_ros_joint_state_publisher.cpp @@ -50,6 +50,10 @@ namespace gazebo_plugins class GazeboRosJointStatePublisherPrivate { public: + /// Callback to be called at every simulation iteration. + /// \param[in] info Updated simulation info. + void OnUpdate(const gazebo::common::UpdateInfo & info); + /// A pointer to the GazeboROS node. gazebo_ros::Node::SharedPtr ros_node_; @@ -131,23 +135,23 @@ void GazeboRosJointStatePublisher::Load(gazebo::physics::ModelPtr model, sdf::El // Callback on every iteration impl_->update_connection_ = gazebo::event::Events::ConnectWorldUpdateBegin( - std::bind(&GazeboRosJointStatePublisher::OnUpdate, this, std::placeholders::_1)); + std::bind(&GazeboRosJointStatePublisherPrivate::OnUpdate, impl_.get(), std::placeholders::_1)); } -void GazeboRosJointStatePublisher::OnUpdate(const gazebo::common::UpdateInfo & info) +void GazeboRosJointStatePublisherPrivate::OnUpdate(const gazebo::common::UpdateInfo & info) { gazebo::common::Time current_time = info.simTime; // If the world is reset, for example - if (current_time < impl_->last_update_time_) { - RCLCPP_INFO(impl_->ros_node_->get_logger(), "Negative sim time difference detected."); - impl_->last_update_time_ = current_time; + if (current_time < last_update_time_) { + RCLCPP_INFO(ros_node_->get_logger(), "Negative sim time difference detected."); + last_update_time_ = current_time; } // Check period - double seconds_since_last_update = (current_time - impl_->last_update_time_).Double(); + double seconds_since_last_update = (current_time - last_update_time_).Double(); - if (seconds_since_last_update < impl_->update_period_) { + if (seconds_since_last_update < update_period_) { return; } @@ -155,12 +159,12 @@ void GazeboRosJointStatePublisher::OnUpdate(const gazebo::common::UpdateInfo & i sensor_msgs::msg::JointState joint_state; joint_state.header.stamp = gazebo_ros::Convert(current_time); - joint_state.name.resize(impl_->joints_.size()); - joint_state.position.resize(impl_->joints_.size()); - joint_state.velocity.resize(impl_->joints_.size()); + joint_state.name.resize(joints_.size()); + joint_state.position.resize(joints_.size()); + joint_state.velocity.resize(joints_.size()); - for (unsigned int i = 0; i < impl_->joints_.size(); ++i) { - auto joint = impl_->joints_[i]; + for (unsigned int i = 0; i < joints_.size(); ++i) { + auto joint = joints_[i]; double velocity = joint->GetVelocity(0); double position = joint->Position(0); joint_state.name[i] = joint->GetName(); @@ -169,10 +173,10 @@ void GazeboRosJointStatePublisher::OnUpdate(const gazebo::common::UpdateInfo & i } // Publish - impl_->joint_state_pub_->publish(joint_state); + joint_state_pub_->publish(joint_state); // Update time - impl_->last_update_time_ = current_time; + last_update_time_ = current_time; } GZ_REGISTER_MODEL_PLUGIN(GazeboRosJointStatePublisher) From 604ac5260a44362408324bdc2a675428039e6cf4 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Tue, 7 Aug 2018 17:01:28 -0700 Subject: [PATCH 5/6] depend on sensor_msgs --- gazebo_plugins/package.xml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/gazebo_plugins/package.xml b/gazebo_plugins/package.xml index b30c60d97..fef87e50c 100644 --- a/gazebo_plugins/package.xml +++ b/gazebo_plugins/package.xml @@ -23,11 +23,13 @@ gazebo_ros geometry_msgs rclcpp + sensor_msgs gazebo_dev gazebo_ros geometry_msgs rclcpp + sensor_msgs ament_cmake_gtest ament_lint_auto From 4fe1967e3e6c1ee1cc176dfd253f7dd0d9ae51f9 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Wed, 8 Aug 2018 13:30:52 -0700 Subject: [PATCH 6/6] Use node's logger --- .../src/gazebo_ros_joint_state_publisher.cpp | 18 +++++++++++------- 1 file changed, 11 insertions(+), 7 deletions(-) diff --git a/gazebo_plugins/src/gazebo_ros_joint_state_publisher.cpp b/gazebo_plugins/src/gazebo_ros_joint_state_publisher.cpp index 48a83bab3..f4890ddd5 100644 --- a/gazebo_plugins/src/gazebo_ros_joint_state_publisher.cpp +++ b/gazebo_plugins/src/gazebo_ros_joint_state_publisher.cpp @@ -84,11 +84,13 @@ GazeboRosJointStatePublisher::~GazeboRosJointStatePublisher() void GazeboRosJointStatePublisher::Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf) { - auto logger = rclcpp::get_logger("gazebo_ros_force"); + // ROS node + impl_->ros_node_ = gazebo_ros::Node::Create("gazebo_ros_joint_state_publisher", sdf); // Joints if (!sdf->HasElement("joint_name")) { - RCLCPP_ERROR(logger, "Plugin missing s"); + RCLCPP_ERROR(impl_->ros_node_->get_logger(), "Plugin missing s"); + impl_->ros_node_.reset(); return; } @@ -98,24 +100,27 @@ void GazeboRosJointStatePublisher::Load(gazebo::physics::ModelPtr model, sdf::El auto joint = model->GetJoint(joint_name); if (!joint) { - RCLCPP_ERROR(logger, "Joint %s does not exist!", joint_name.c_str()); + RCLCPP_ERROR(impl_->ros_node_->get_logger(), "Joint %s does not exist!", joint_name.c_str()); } else { impl_->joints_.push_back(joint); - RCLCPP_INFO(logger, "Going to publish joint [%s]", joint_name.c_str() ); + RCLCPP_INFO(impl_->ros_node_->get_logger(), "Going to publish joint [%s]", + joint_name.c_str() ); } joint_elem = joint_elem->GetNextElement("joint_name"); } if (impl_->joints_.empty()) { - RCLCPP_ERROR(logger, "No joints found."); + RCLCPP_ERROR(impl_->ros_node_->get_logger(), "No joints found."); + impl_->ros_node_.reset(); return; } // Update rate double update_rate = 100.0; if (!sdf->HasElement("update_rate")) { - RCLCPP_INFO(logger, "Missing , defaults to %f", update_rate); + RCLCPP_INFO(impl_->ros_node_->get_logger(), "Missing , defaults to %f", + update_rate); } else { update_rate = sdf->GetElement("update_rate")->Get(); } @@ -129,7 +134,6 @@ void GazeboRosJointStatePublisher::Load(gazebo::physics::ModelPtr model, sdf::El impl_->last_update_time_ = model->GetWorld()->SimTime(); // Joint state publisher - impl_->ros_node_ = gazebo_ros::Node::Create("gazebo_ros_joint_state_publisher", sdf); impl_->joint_state_pub_ = impl_->ros_node_->create_publisher( "joint_states", 1000);