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..2db081cbb --- /dev/null +++ b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_joint_state_publisher.hpp @@ -0,0 +1,97 @@ +// Copyright (c) 2013, Markus Bader +// 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. + +#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. +/** + \details If the joint contains more than one axis, only the state of the first axis is reported. + + 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) override; + +private: + /// Callback to be called at every simulation iteration. + /// Private data pointer + std::unique_ptr impl_; +}; +} // namespace gazebo_plugins +#endif // GAZEBO_PLUGINS__GAZEBO_ROS_JOINT_STATE_PUBLISHER_HPP_ diff --git a/gazebo_plugins/package.xml b/gazebo_plugins/package.xml index fecc0a143..fef87e50c 100644 --- a/gazebo_plugins/package.xml +++ b/gazebo_plugins/package.xml @@ -23,12 +23,15 @@ gazebo_ros geometry_msgs rclcpp + sensor_msgs gazebo_dev gazebo_ros geometry_msgs rclcpp + sensor_msgs + 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 new file mode 100644 index 000000000..f4890ddd5 --- /dev/null +++ b/gazebo_plugins/src/gazebo_ros_joint_state_publisher.cpp @@ -0,0 +1,187 @@ +// Copyright (c) 2013, Markus Bader +// 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. + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +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_; + + /// 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) +{ + // ROS node + impl_->ros_node_ = gazebo_ros::Node::Create("gazebo_ros_joint_state_publisher", sdf); + + // Joints + if (!sdf->HasElement("joint_name")) { + RCLCPP_ERROR(impl_->ros_node_->get_logger(), "Plugin missing s"); + impl_->ros_node_.reset(); + 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(impl_->ros_node_->get_logger(), "Joint %s does not exist!", joint_name.c_str()); + } else { + impl_->joints_.push_back(joint); + 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(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(impl_->ros_node_->get_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_->joint_state_pub_ = impl_->ros_node_->create_publisher( + "joint_states", 1000); + + // Callback on every iteration + impl_->update_connection_ = gazebo::event::Events::ConnectWorldUpdateBegin( + std::bind(&GazeboRosJointStatePublisherPrivate::OnUpdate, impl_.get(), std::placeholders::_1)); +} + +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 < 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 - last_update_time_).Double(); + + if (seconds_since_last_update < update_period_) { + return; + } + + // Populate message + sensor_msgs::msg::JointState joint_state; + + joint_state.header.stamp = gazebo_ros::Convert(current_time); + joint_state.name.resize(joints_.size()); + joint_state.position.resize(joints_.size()); + joint_state.velocity.resize(joints_.size()); + + 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(); + joint_state.position[i] = position; + joint_state.velocity[i] = velocity; + } + + // Publish + joint_state_pub_->publish(joint_state); + + // Update time + last_update_time_ = current_time; +} + +GZ_REGISTER_MODEL_PLUGIN(GazeboRosJointStatePublisher) +} // namespace gazebo_plugins 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_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.