diff --git a/.ros1_unported/gazebo_plugins/include/gazebo_plugins/gazebo_ros_joint_pose_trajectory.h b/.ros1_unported/gazebo_plugins/include/gazebo_plugins/gazebo_ros_joint_pose_trajectory.h deleted file mode 100644 index bc9651fc6..000000000 --- a/.ros1_unported/gazebo_plugins/include/gazebo_plugins/gazebo_ros_joint_pose_trajectory.h +++ /dev/null @@ -1,133 +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. - * -*/ - -// ************************************************************* -// DEPRECATED -// This class has been renamed to gazebo_ros_joint_pose_trajectory -// ************************************************************* - -#ifndef GAZEBO_ROS_JOINT_TRAJECTORY_PLUGIN_HH -#define GAZEBO_ROS_JOINT_TRAJECTORY_PLUGIN_HH - -#include -#include - -#include -#include - -#include -#include -#include -#include - -#include -#include - -#undef ENABLE_SERVICE -#ifdef ENABLE_SERVICE -#include -#endif - -#include -#include -#include -#include -#include - -namespace gazebo -{ - class GazeboRosJointPoseTrajectory : public ModelPlugin // replaced with GazeboROSJointPoseTrajectory - { - /// \brief Constructor - public: GazeboRosJointPoseTrajectory(); - - /// \brief Destructor - public: virtual ~GazeboRosJointPoseTrajectory(); - - /// \brief Load the controller - public: void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf); - - /// \brief Update the controller - private: void SetTrajectory( - const trajectory_msgs::JointTrajectory::ConstPtr& trajectory); -#ifdef ENABLE_SERVICE - private: bool SetTrajectory( - const gazebo_msgs::SetJointTrajectory::Request& req, - const gazebo_msgs::SetJointTrajectory::Response& res); -#endif - private: void UpdateStates(); - - private: physics::WorldPtr world_; - private: physics::ModelPtr model_; - - /// \brief pose should be set relative to this link (default to "world") - private: physics::LinkPtr reference_link_; - private: std::string reference_link_name_; - - /// \brief pointer to ros node - private: ros::NodeHandle* rosnode_; - private: ros::Subscriber sub_; - private: ros::ServiceServer srv_; - private: bool has_trajectory_; - - /// \brief ros message - private: trajectory_msgs::JointTrajectory trajectory_msg_; - private: bool set_model_pose_; - private: geometry_msgs::Pose model_pose_; - - /// \brief topic name - private: std::string topic_name_; - private: std::string service_name_; - - /// \brief A mutex to lock access to fields that are - /// used in message callbacks - private: boost::mutex update_mutex; - - /// \brief save last_time - private: common::Time last_time_; - - // trajectory time control - private: common::Time trajectory_start; - private: unsigned int trajectory_index; - - // rate control - private: double update_rate_; - private: bool disable_physics_updates_; - private: bool physics_engine_enabled_; - - /// \brief for setting ROS name space - private: std::string robot_namespace_; - - private: ros::CallbackQueue queue_; - private: void QueueThread(); - private: boost::thread callback_queue_thread_; - - private: std::vector joints_; - private: std::vector points_; - - // Pointer to the update event connection - private: event::ConnectionPtr update_connection_; - - private: trajectory_msgs::JointTrajectory joint_trajectory_; - - // deferred load in case ros is blocking - private: sdf::ElementPtr sdf; - private: void LoadThread(); - private: boost::thread deferred_load_thread_; - }; -} -#endif diff --git a/.ros1_unported/gazebo_plugins/src/gazebo_ros_joint_pose_trajectory.cpp b/.ros1_unported/gazebo_plugins/src/gazebo_ros_joint_pose_trajectory.cpp deleted file mode 100644 index e94b1e1d2..000000000 --- a/.ros1_unported/gazebo_plugins/src/gazebo_ros_joint_pose_trajectory.cpp +++ /dev/null @@ -1,452 +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: 3D position interface for ground truth. - * Author: Sachin Chitta and John Hsu - * Date: 1 June 2008 - */ - -#include -#include -#include - -#include - -namespace gazebo -{ -GZ_REGISTER_MODEL_PLUGIN(GazeboRosJointPoseTrajectory); - -//////////////////////////////////////////////////////////////////////////////// -// Constructor -ROS_DEPRECATED GazeboRosJointPoseTrajectory::GazeboRosJointPoseTrajectory() // replaced with GazeboROSJointPoseTrajectory -{ - - this->has_trajectory_ = false; - this->trajectory_index = 0; - this->joint_trajectory_.points.clear(); - this->physics_engine_enabled_ = true; - this->disable_physics_updates_ = true; -} - -//////////////////////////////////////////////////////////////////////////////// -// Destructor -GazeboRosJointPoseTrajectory::~GazeboRosJointPoseTrajectory() -{ - this->update_connection_.reset(); - // Finalize the controller - this->rosnode_->shutdown(); - this->queue_.clear(); - this->queue_.disable(); - this->callback_queue_thread_.join(); - delete this->rosnode_; -} - -//////////////////////////////////////////////////////////////////////////////// -// Load the controller -void GazeboRosJointPoseTrajectory::Load(physics::ModelPtr _model, - sdf::ElementPtr _sdf) -{ - // save pointers - this->model_ = _model; - this->sdf = _sdf; - this->world_ = this->model_->GetWorld(); - - // this->world_->SetGravity(ignition::math::Vector3d(0, 0, 0)); - - // load parameters - this->robot_namespace_ = ""; - if (this->sdf->HasElement("robotNamespace")) - this->robot_namespace_ = this->sdf->Get("robotNamespace") + "/"; - - if (!this->sdf->HasElement("serviceName")) - { - // default - this->service_name_ = "set_joint_trajectory"; - } - else - this->service_name_ = this->sdf->Get("serviceName"); - - if (!this->sdf->HasElement("topicName")) - { - // default - this->topic_name_ = "set_joint_trajectory"; - } - else - this->topic_name_ = this->sdf->Get("topicName"); - - if (!this->sdf->HasElement("updateRate")) - { - ROS_INFO_NAMED("joint_pose_trajectory", "joint trajectory plugin missing , defaults" - " to 0.0 (as fast as possible)"); - this->update_rate_ = 0; - } - else - this->update_rate_ = this->sdf->Get("updateRate"); - - // Make sure the ROS node for Gazebo has already been initialized - if (!ros::isInitialized()) - { - ROS_FATAL_STREAM_NAMED("joint_pose_trajectory", "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->deferred_load_thread_ = boost::thread( - boost::bind(&GazeboRosJointPoseTrajectory::LoadThread, this)); - -} - -//////////////////////////////////////////////////////////////////////////////// -// Load the controller -void GazeboRosJointPoseTrajectory::LoadThread() -{ - this->rosnode_ = new ros::NodeHandle(this->robot_namespace_); - - // resolve tf prefix - std::string prefix; - this->rosnode_->getParam(std::string("tf_prefix"), prefix); - - if (this->topic_name_ != "") - { - ros::SubscribeOptions trajectory_so = - ros::SubscribeOptions::create( - this->topic_name_, 100, boost::bind( - &GazeboRosJointPoseTrajectory::SetTrajectory, this, _1), - ros::VoidPtr(), &this->queue_); - this->sub_ = this->rosnode_->subscribe(trajectory_so); - } - -#ifdef ENABLE_SERVICE - if (this->service_name_ != "") - { - ros::AdvertiseServiceOptions srv_aso = - ros::AdvertiseServiceOptions::create( - this->service_name_, - boost::bind(&GazeboRosJointPoseTrajectory::SetTrajectory, this, _1, _2), - ros::VoidPtr(), &this->queue_); - this->srv_ = this->rosnode_->advertiseService(srv_aso); - } -#endif - -#if GAZEBO_MAJOR_VERSION >= 8 - this->last_time_ = this->world_->SimTime(); -#else - this->last_time_ = this->world_->GetSimTime(); -#endif - - // start custom queue for joint trajectory plugin ros topics - this->callback_queue_thread_ = - boost::thread(boost::bind(&GazeboRosJointPoseTrajectory::QueueThread, this)); - - // New Mechanism for Updating every World Cycle - // Listen to the update event. This event is broadcast every - // simulation iteration. - this->update_connection_ = event::Events::ConnectWorldUpdateBegin( - boost::bind(&GazeboRosJointPoseTrajectory::UpdateStates, this)); -} - -//////////////////////////////////////////////////////////////////////////////// -// set joint trajectory -void GazeboRosJointPoseTrajectory::SetTrajectory( - const trajectory_msgs::JointTrajectory::ConstPtr& trajectory) -{ - boost::mutex::scoped_lock lock(this->update_mutex); - - this->reference_link_name_ = trajectory->header.frame_id; - // do this every time a new joint trajectory is supplied, - // use header.frame_id as the reference_link_name_ - if (this->reference_link_name_ != "world" && - this->reference_link_name_ != "/map" && - this->reference_link_name_ != "map") - { - physics::EntityPtr ent = -#if GAZEBO_MAJOR_VERSION >= 8 - this->world_->EntityByName(this->reference_link_name_); -#else - this->world_->GetEntity(this->reference_link_name_); -#endif - if (ent) - this->reference_link_ = boost::dynamic_pointer_cast(ent); - if (!this->reference_link_) - { - ROS_ERROR_NAMED("joint_pose_trajectory", "ros_joint_trajectory plugin needs a reference link [%s] as" - " frame_id, aborting.\n", this->reference_link_name_.c_str()); - return; - } - else - { - this->model_ = this->reference_link_->GetParentModel(); - ROS_DEBUG_NAMED("joint_pose_trajectory", "test: update model pose by keeping link [%s] stationary" - " inertially", this->reference_link_->GetName().c_str()); - } - } - - // copy joint configuration into a map - unsigned int chain_size = trajectory->joint_names.size(); - this->joints_.resize(chain_size); - for (unsigned int i = 0; i < chain_size; ++i) - { - this->joints_[i] = this->model_->GetJoint(trajectory->joint_names[i]); - } - - unsigned int points_size = trajectory->points.size(); - this->points_.resize(points_size); - for (unsigned int i = 0; i < points_size; ++i) - { - this->points_[i].positions.resize(chain_size); - this->points_[i].time_from_start = trajectory->points[i].time_from_start; - for (unsigned int j = 0; j < chain_size; ++j) - { - this->points_[i].positions[j] = trajectory->points[i].positions[j]; - } - } - - // trajectory start time - this->trajectory_start = gazebo::common::Time(trajectory->header.stamp.sec, - trajectory->header.stamp.nsec); -#if GAZEBO_MAJOR_VERSION >= 8 - common::Time cur_time = this->world_->SimTime(); -#else - common::Time cur_time = this->world_->GetSimTime(); -#endif - if (this->trajectory_start < cur_time) - this->trajectory_start = cur_time; - - // update the joint trajectory to play - this->has_trajectory_ = true; - // reset trajectory_index to beginning of new trajectory - this->trajectory_index = 0; - - if (this->disable_physics_updates_) - { -#if GAZEBO_MAJOR_VERSION >= 8 - this->physics_engine_enabled_ = this->world_->PhysicsEnabled(); - this->world_->SetPhysicsEnabled(false); -#else - this->physics_engine_enabled_ = this->world_->GetEnablePhysicsEngine(); - this->world_->EnablePhysicsEngine(false); -#endif - } -} - -#ifdef ENABLE_SERVICE -bool GazeboRosJointPoseTrajectory::SetTrajectory( - const gazebo_msgs::SetJointTrajectory::Request& req, - const gazebo_msgs::SetJointTrajectory::Response& res) -{ - boost::mutex::scoped_lock lock(this->update_mutex); - - this->model_pose_ = req.model_pose; - this->set_model_pose_ = req.set_model_pose; - - this->reference_link_name_ = req.joint_trajectory.header.frame_id; - // do this every time a new joint_trajectory is supplied, - // use header.frame_id as the reference_link_name_ - if (this->reference_link_name_ != "world" && - this->reference_link_name_ != "/map" && - this->reference_link_name_ != "map") - { - physics::EntityPtr ent = -#if GAZEBO_MAJOR_VERSION >= 8 - this->world_->EntityByName(this->reference_link_name_); -#else - this->world_->GetEntity(this->reference_link_name_); -#endif - if (ent) - this->reference_link_ = boost::dynamic_pointer_cast(ent); - if (!this->reference_link_) - { - ROS_ERROR_NAMED("joint_pose_trajectory", "ros_joint_trajectory plugin specified a reference link [%s]" - " that does not exist, aborting.\n", - this->reference_link_name_.c_str()); - ROS_DEBUG_NAMED("joint_pose_trajectory", "will set model [%s] configuration, keeping model root link" - " stationary.", this->model_->GetName().c_str()); - return false; - } - else - ROS_DEBUG_NAMED("joint_pose_trajectory", "test: update model pose by keeping link [%s] stationary" - " inertially", this->reference_link_->GetName().c_str()); - } - -#if GAZEBO_MAJOR_VERSION >= 8 - this->model_ = this->world_->ModelByName(req.model_name); -#else - this->model_ = this->world_->GetModel(req.model_name); -#endif - if (!this->model_) // look for it by frame_id name - { - this->model_ = this->reference_link_->GetParentModel(); - if (this->model_) - { - ROS_INFO_NAMED("joint_pose_trajectory", "found model[%s] by link name specified in frame_id[%s]", - this->model_->GetName().c_str(), - req.joint_trajectory.header.frame_id.c_str()); - } - else - { - ROS_WARN_NAMED("joint_pose_trajectory", "no model found by link name specified in frame_id[%s]," - " aborting.", req.joint_trajectory.header.frame_id.c_str()); - return false; - } - } - - // copy joint configuration into a map - this->joint_trajectory_ = req.joint_trajectory; - - // trajectory start time - this->trajectory_start = gazebo::common::Time( - req.joint_trajectory.header.stamp.sec, - req.joint_trajectory.header.stamp.nsec); - - // update the joint_trajectory to play - this->has_trajectory_ = true; - // reset trajectory_index to beginning of new trajectory - this->trajectory_index = 0; - this->disable_physics_updates_ = req.disable_physics_updates; - if (this->disable_physics_updates_) - { -#if GAZEBO_MAJOR_VERSION >= 8 - this->physics_engine_enabled_ = this->world_->PhysicsEnabled(); - this->world_->SetPhysicsEnabled(false); -#else - this->physics_engine_enabled_ = this->world_->GetEnablePhysicsEngine(); - this->world_->EnablePhysicsEngine(false); -#endif - } - - return true; -} -#endif - -//////////////////////////////////////////////////////////////////////////////// -// Play the trajectory, update states -void GazeboRosJointPoseTrajectory::UpdateStates() -{ - boost::mutex::scoped_lock lock(this->update_mutex); - if (this->has_trajectory_) - { -#if GAZEBO_MAJOR_VERSION >= 8 - common::Time cur_time = this->world_->SimTime(); -#else - common::Time cur_time = this->world_->GetSimTime(); -#endif - // roll out trajectory via set model configuration - // gzerr << "i[" << trajectory_index << "] time " - // << trajectory_start << " now: " << cur_time << " : "<< "\n"; - if (cur_time >= this->trajectory_start) - { - // @todo: consider a while loop until the trajectory - // catches up to the current time - // gzerr << trajectory_index << " : " << this->points_.size() << "\n"; - if (this->trajectory_index < this->points_.size()) - { - ROS_INFO_NAMED("joint_pose_trajectory", "time [%f] updating configuration [%d/%lu]", - cur_time.Double(), this->trajectory_index, this->points_.size()); - - // get reference link pose before updates -#if GAZEBO_MAJOR_VERSION >= 8 - ignition::math::Pose3d reference_pose = this->model_->WorldPose(); -#else - ignition::math::Pose3d reference_pose = this->model_->GetWorldPose().Ign(); -#endif - if (this->reference_link_) - { -#if GAZEBO_MAJOR_VERSION >= 8 - reference_pose = this->reference_link_->WorldPose(); -#else - reference_pose = this->reference_link_->GetWorldPose().Ign(); -#endif - } - - // trajectory roll-out based on time: - // set model configuration from trajectory message - unsigned int chain_size = this->joints_.size(); - if (chain_size == - this->points_[this->trajectory_index].positions.size()) - { - for (unsigned int i = 0; i < chain_size; ++i) - { - // this is not the most efficient way to set things - if (this->joints_[i]) - { -#if GAZEBO_MAJOR_VERSION >= 9 - this->joints_[i]->SetPosition(0, - this->points_[this->trajectory_index].positions[i], true); -#else - this->joints_[i]->SetPosition(0, - this->points_[this->trajectory_index].positions[i]); -#endif - } - } - - // set model pose - if (this->reference_link_) - this->model_->SetLinkWorldPose(reference_pose, - this->reference_link_); - else - this->model_->SetWorldPose(reference_pose); - } - else - { - ROS_ERROR_NAMED("joint_pose_trajectory", "point[%u] in JointTrajectory has different number of" - " joint names[%u] and positions[%lu].", - this->trajectory_index, chain_size, - this->points_[this->trajectory_index].positions.size()); - } - - // this->world_->SetPaused(is_paused); // resume original pause-state - gazebo::common::Time duration( - this->points_[this->trajectory_index].time_from_start.sec, - this->points_[this->trajectory_index].time_from_start.nsec); - - // reset start time for next trajectory point - this->trajectory_start += duration; - this->trajectory_index++; // increment to next trajectory point - - // save last update time stamp - this->last_time_ = cur_time; - } - else // no more trajectory points - { - // trajectory finished - this->reference_link_.reset(); - this->has_trajectory_ = false; - if (this->disable_physics_updates_) - { -#if GAZEBO_MAJOR_VERSION >= 8 - this->world_->SetPhysicsEnabled(this->physics_engine_enabled_); -#else - this->world_->EnablePhysicsEngine(this->physics_engine_enabled_); -#endif - } - } - } - } -} - -//////////////////////////////////////////////////////////////////////////////// -// Put laser data to the interface -void GazeboRosJointPoseTrajectory::QueueThread() -{ - static const double timeout = 0.01; - while (this->rosnode_->ok()) - { - this->queue_.callAvailable(ros::WallDuration(timeout)); - } -} -} diff --git a/gazebo_msgs/srv/SetJointTrajectory.srv b/gazebo_msgs/srv/SetJointTrajectory.srv index 62169e6e9..ac9d38aee 100644 --- a/gazebo_msgs/srv/SetJointTrajectory.srv +++ b/gazebo_msgs/srv/SetJointTrajectory.srv @@ -1,3 +1,4 @@ +# Deprecated, kept for ROS 1 bridge. string model_name trajectory_msgs/JointTrajectory joint_trajectory geometry_msgs/Pose model_pose diff --git a/gazebo_plugins/CMakeLists.txt b/gazebo_plugins/CMakeLists.txt index 507d001ab..2f0b14349 100644 --- a/gazebo_plugins/CMakeLists.txt +++ b/gazebo_plugins/CMakeLists.txt @@ -26,6 +26,7 @@ find_package(std_msgs REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_geometry_msgs REQUIRED) find_package(tf2_ros REQUIRED) +find_package(trajectory_msgs REQUIRED) include_directories(include ${gazebo_dev_INCLUDE_DIRS} @@ -258,6 +259,18 @@ target_link_libraries(gazebo_ros_elevator ) ament_export_libraries(gazebo_ros_elevator) +# gazebo_ros_joint_pose_trajectory +add_library(gazebo_ros_joint_pose_trajectory SHARED + src/gazebo_ros_joint_pose_trajectory.cpp +) +ament_target_dependencies(gazebo_ros_joint_pose_trajectory + "gazebo_dev" + "gazebo_ros" + "rclcpp" + "trajectory_msgs" +) +ament_export_libraries(gazebo_ros_joint_pose_trajectory) + # gazebo_ros_planar_move add_library(gazebo_ros_planar_move SHARED src/gazebo_ros_planar_move.cpp @@ -315,6 +328,7 @@ install(TARGETS gazebo_ros_hand_of_god gazebo_ros_harness gazebo_ros_imu_sensor + gazebo_ros_joint_pose_trajectory gazebo_ros_joint_state_publisher gazebo_ros_planar_move gazebo_ros_projector diff --git a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_joint_pose_trajectory.hpp b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_joint_pose_trajectory.hpp new file mode 100644 index 000000000..0baae5469 --- /dev/null +++ b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_joint_pose_trajectory.hpp @@ -0,0 +1,68 @@ +// Copyright 2019 Open Source Robotics Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef GAZEBO_PLUGINS__GAZEBO_ROS_JOINT_POSE_TRAJECTORY_HPP_ +#define GAZEBO_PLUGINS__GAZEBO_ROS_JOINT_POSE_TRAJECTORY_HPP_ + +#include + +#include + +namespace gazebo_plugins +{ +class GazeboRosJointPoseTrajectoryPrivate; + +/// Set the trajectory of points to be followed by joints in simulation. +/// Currently only positions specified in the trajectory_msgs are handled. +/** + Example Usage: + \code{.xml} + + + + + + /my_namespace + + + set_joint_trajectory:=my_trajectory + + + + + 2 + + + \endcode +*/ +class GazeboRosJointPoseTrajectory : public gazebo::ModelPlugin +{ +public: + /// Constructor + GazeboRosJointPoseTrajectory(); + + /// Destructor + ~GazeboRosJointPoseTrajectory(); + +protected: + // Documentation inherited + void Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf) override; + +private: + /// Private data pointer + std::unique_ptr impl_; +}; +} // namespace gazebo_plugins +#endif // GAZEBO_PLUGINS__GAZEBO_ROS_JOINT_POSE_TRAJECTORY_HPP_ diff --git a/gazebo_plugins/package.xml b/gazebo_plugins/package.xml index 31bd191f4..10ec91122 100644 --- a/gazebo_plugins/package.xml +++ b/gazebo_plugins/package.xml @@ -28,6 +28,7 @@ std_msgs tf2_geometry_msgs tf2_ros + trajectory_msgs gazebo_dev gazebo_msgs diff --git a/gazebo_plugins/src/gazebo_ros_joint_pose_trajectory.cpp b/gazebo_plugins/src/gazebo_ros_joint_pose_trajectory.cpp new file mode 100644 index 000000000..e0fb1e0f1 --- /dev/null +++ b/gazebo_plugins/src/gazebo_ros_joint_pose_trajectory.cpp @@ -0,0 +1,262 @@ +// 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. + +/* + * \brief Set the trajectory of points to be followed by joints in simulation. + * + * \author Sachin Chitta and John Hsu + * + * \date 1 June 2008 + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace gazebo_plugins +{ +class GazeboRosJointPoseTrajectoryPrivate +{ +public: + /// Callback to be called at every simulation iteration. + /// \param[in] info Updated simulation info. + void OnUpdate(const gazebo::common::UpdateInfo & info); + + /// \brief Callback for set joint trajectory topic. + /// \param[in] msg Trajectory msg + void SetJointTrajectory(trajectory_msgs::msg::JointTrajectory::SharedPtr _msg); + + /// A pointer to the GazeboROS node. + gazebo_ros::Node::SharedPtr ros_node_; + + /// Subscriber to Trajectory messages. + rclcpp::Subscription::SharedPtr sub_; + + /// Pointer to model. + gazebo::physics::ModelPtr model_; + + /// Pointer to world. + gazebo::physics::WorldPtr world_; + + /// Pointer to link wrt which the trajectory is set. + gazebo::physics::LinkPtr reference_link_; + + /// Joints for setting the trajectory. + std::vector joints_; + + /// Command trajectory points + std::vector points_; + + /// Period in seconds + double update_period_; + + /// Keep last time an update was published + gazebo::common::Time last_update_time_; + + /// Desired trajectory start time + gazebo::common::Time trajectory_start_time_; + + /// Protect variables accessed on callbacks. + std::mutex lock_; + + /// Index number of trajectory to be executed + unsigned int trajectory_index_; + + /// True if trajectory is available + bool has_trajectory_; + + /// Pointer to the update event connection. + gazebo::event::ConnectionPtr update_connection_; +}; + +GazeboRosJointPoseTrajectory::GazeboRosJointPoseTrajectory() +: impl_(std::make_unique()) +{ +} + +GazeboRosJointPoseTrajectory::~GazeboRosJointPoseTrajectory() +{ +} + +void GazeboRosJointPoseTrajectory::Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf) +{ + impl_->model_ = model; + + impl_->world_ = model->GetWorld(); + + // Initialize ROS node + impl_->ros_node_ = gazebo_ros::Node::Get(sdf); + + // Update rate + auto update_rate = sdf->Get("update_rate", 100.0).first; + if (update_rate > 0.0) { + impl_->update_period_ = 1.0 / update_rate; + } else { + impl_->update_period_ = 0.0; + } + impl_->last_update_time_ = impl_->world_->SimTime(); + + // Set Joint Trajectory Callback + impl_->sub_ = impl_->ros_node_->create_subscription( + "set_joint_trajectory", rclcpp::QoS(rclcpp::KeepLast(1)), + std::bind(&GazeboRosJointPoseTrajectoryPrivate::SetJointTrajectory, + impl_.get(), std::placeholders::_1)); + + // Callback on every iteration + impl_->update_connection_ = gazebo::event::Events::ConnectWorldUpdateBegin( + std::bind(&GazeboRosJointPoseTrajectoryPrivate::OnUpdate, impl_.get(), std::placeholders::_1)); +} + +void GazeboRosJointPoseTrajectoryPrivate::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; + } + + std::lock_guard scoped_lock(lock_); + if (has_trajectory_ && current_time >= trajectory_start_time_) { + if (trajectory_index_ < points_.size()) { + RCLCPP_INFO(ros_node_->get_logger(), "time [%f] updating configuration [%d/%lu]", + current_time.Double(), trajectory_index_ + 1, points_.size()); + + // get reference link pose before updates + auto reference_pose = model_->WorldPose(); + + if (reference_link_) { + reference_pose = reference_link_->WorldPose(); + } + + // trajectory roll-out based on time: + // set model configuration from trajectory message + auto chain_size = static_cast(joints_.size()); + if (chain_size == points_[trajectory_index_].positions.size()) { + for (unsigned int i = 0; i < chain_size; ++i) { + // this is not the most efficient way to set things + if (joints_[i]) { + joints_[i]->SetPosition(0, points_[trajectory_index_].positions[i], true); + } + } + // set model pose + if (reference_link_) { + model_->SetLinkWorldPose(reference_pose, reference_link_); + } else { + model_->SetWorldPose(reference_pose); + } + } else { + RCLCPP_ERROR(ros_node_->get_logger(), + "point[%u] has different number of joint names[%u] and positions[%lu].", + trajectory_index_ + 1, chain_size, points_[trajectory_index_].positions.size()); + } + + auto duration = + gazebo_ros::Convert(points_[trajectory_index_].time_from_start); + + // reset start time for next trajectory point + trajectory_start_time_ += duration; + trajectory_index_++; // increment to next trajectory point + + // Update time + last_update_time_ = current_time; + } else { + // trajectory finished + reference_link_.reset(); + // No more trajectory points + has_trajectory_ = false; + } + } +} + +void GazeboRosJointPoseTrajectoryPrivate::SetJointTrajectory( + trajectory_msgs::msg::JointTrajectory::SharedPtr msg) +{ + std::lock_guard scoped_lock(lock_); + + std::string reference_link_name = msg->header.frame_id; + // do this every time a new joint trajectory is supplied, + // use header.frame_id as the reference_link_name_ + if (!(reference_link_name == "world" || reference_link_name == "map")) { + gazebo::physics::EntityPtr entity = world_->EntityByName(reference_link_name); + if (entity) { + reference_link_ = boost::dynamic_pointer_cast(entity); + } + if (!reference_link_) { + RCLCPP_ERROR(ros_node_->get_logger(), + "Plugin needs a reference link [%s] as frame_id, aborting.", reference_link_name.c_str()); + return; + } + model_ = reference_link_->GetParentModel(); + RCLCPP_DEBUG(ros_node_->get_logger(), + "Update model pose by keeping link [%s] stationary inertially", + reference_link_->GetName().c_str()); + } + + // copy joint configuration into a map + auto chain_size = static_cast(msg->joint_names.size()); + joints_.resize(chain_size); + for (unsigned int i = 0; i < chain_size; ++i) { + joints_[i] = model_->GetJoint(msg->joint_names[i]); + if (!joints_[i]) { + RCLCPP_ERROR(ros_node_->get_logger(), "Joint [%s] not found. Trajectory not set.", + msg->joint_names[i].c_str()); + return; + } + } + + auto points_size = static_cast(msg->points.size()); + points_.resize(points_size); + for (unsigned int i = 0; i < points_size; ++i) { + points_[i].positions.resize(chain_size); + points_[i].time_from_start = msg->points[i].time_from_start; + for (unsigned int j = 0; j < chain_size; ++j) { + points_[i].positions[j] = msg->points[i].positions[j]; + } + } + + // trajectory start time + trajectory_start_time_ = gazebo_ros::Convert(msg->header.stamp); + + gazebo::common::Time cur_time = world_->SimTime(); + if (trajectory_start_time_ < cur_time) { + trajectory_start_time_ = cur_time; + } + // update the joint trajectory to play + has_trajectory_ = true; + // reset trajectory_index to beginning of new trajectory + trajectory_index_ = 0; +} + +GZ_REGISTER_MODEL_PLUGIN(GazeboRosJointPoseTrajectory) +} // namespace gazebo_plugins diff --git a/gazebo_plugins/test/CMakeLists.txt b/gazebo_plugins/test/CMakeLists.txt index dd2bd2f02..da5a69b45 100644 --- a/gazebo_plugins/test/CMakeLists.txt +++ b/gazebo_plugins/test/CMakeLists.txt @@ -21,6 +21,7 @@ set(tests test_gazebo_ros_hand_of_god test_gazebo_ros_harness test_gazebo_ros_imu_sensor + test_gazebo_ros_joint_pose_trajectory test_gazebo_ros_joint_state_publisher test_gazebo_ros_p3d test_gazebo_ros_planar_move @@ -64,6 +65,7 @@ foreach(test ${tests}) "tf2" "tf2_geometry_msgs" "tf2_ros" + "trajectory_msgs" ) endforeach() diff --git a/gazebo_plugins/test/test_gazebo_ros_joint_pose_trajectory.cpp b/gazebo_plugins/test/test_gazebo_ros_joint_pose_trajectory.cpp new file mode 100644 index 000000000..b36104b1b --- /dev/null +++ b/gazebo_plugins/test/test_gazebo_ros_joint_pose_trajectory.cpp @@ -0,0 +1,84 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include + +#define tol 10e-2 + +using namespace std::literals::chrono_literals; // NOLINT + +class GazeboRosJointPoseTrajectoryTest : public gazebo::ServerFixture +{ +}; + +TEST_F(GazeboRosJointPoseTrajectoryTest, Publishing) +{ + // Load test world and start paused + this->Load("worlds/gazebo_ros_joint_pose_trajectory.world", true); + + // World + auto world = gazebo::physics::get_world(); + ASSERT_NE(nullptr, world); + + // Model + auto model = world->ModelByName("double_pendulum_with_base"); + ASSERT_NE(nullptr, model); + + // Joint + auto joint = model->GetJoint("upper_joint"); + ASSERT_NE(nullptr, joint); + + // Create node and executor + auto node = std::make_shared("gazebo_ros_joint_pose_trajectory_test"); + ASSERT_NE(nullptr, node); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node); + + // Create publisher + auto pub = node->create_publisher( + "test/set_trajectory_test", rclcpp::QoS(rclcpp::KeepLast(1))); + + auto joint_cmd = trajectory_msgs::msg::JointTrajectory(); + joint_cmd.header.frame_id = "world"; + joint_cmd.joint_names.push_back("upper_joint"); + joint_cmd.points.resize(1); + joint_cmd.points[0].positions.push_back(-1.57); + + pub->publish(joint_cmd); + + unsigned int sleep = 0; + unsigned int max_sleep = 30; + while (sleep < max_sleep) { + world->Step(100); + gazebo::common::Time::MSleep(100); + executor.spin_once(100ms); + pub->publish(joint_cmd); + sleep++; + } + + ASSERT_NEAR(joint->Position(0), -1.57, 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_pose_trajectory.world b/gazebo_plugins/test/worlds/gazebo_ros_joint_pose_trajectory.world new file mode 100644 index 000000000..a258b1cfc --- /dev/null +++ b/gazebo_plugins/test/worlds/gazebo_ros_joint_pose_trajectory.world @@ -0,0 +1,26 @@ + + + + 0 0 0 + + model://ground_plane + + + model://sun + + + + model://double_pendulum_with_base + + + + /test + set_joint_trajectory:=set_trajectory_test + + 2 + + + + + diff --git a/gazebo_plugins/worlds/gazebo_ros_joint_pose_trajectory_demo.world b/gazebo_plugins/worlds/gazebo_ros_joint_pose_trajectory_demo.world new file mode 100644 index 000000000..99f7ed793 --- /dev/null +++ b/gazebo_plugins/worlds/gazebo_ros_joint_pose_trajectory_demo.world @@ -0,0 +1,31 @@ + + + + + + model://ground_plane + + + model://sun + + + model://double_pendulum_with_base + + + + /demo + set_joint_trajectory:=set_trajectory_demo + + 2 + + + + + diff --git a/gazebo_ros/include/gazebo_ros/conversions/builtin_interfaces.hpp b/gazebo_ros/include/gazebo_ros/conversions/builtin_interfaces.hpp index 25d6cc5b7..8d3c4cd1d 100644 --- a/gazebo_ros/include/gazebo_ros/conversions/builtin_interfaces.hpp +++ b/gazebo_ros/include/gazebo_ros/conversions/builtin_interfaces.hpp @@ -48,5 +48,49 @@ builtin_interfaces::msg::Time Convert(const gazebo::msgs::Time & in) return time; } +/// Generic conversion from a ROS builtin interface time message to another type. +/// \param[in] in Input message. +/// \return Conversion result +/// \tparam T Output type +template +T Convert(const builtin_interfaces::msg::Time &) +{ + T::ConversionNotImplemented; +} + +/// \brief Specialized conversion from a ROS Time message to a Gazebo Time. +/// \param[in] in ROS Time message to convert. +/// \return A Gazebo Time with the same value as in +template<> +gazebo::common::Time Convert(const builtin_interfaces::msg::Time & in) +{ + gazebo::common::Time time; + time.sec = in.sec; + time.nsec = in.nanosec; + return time; +} + +/// Generic conversion from a ROS builtin interface duration message to another type. +/// \param[in] in Input message. +/// \return Conversion result +/// \tparam T Output type +template +T Convert(const builtin_interfaces::msg::Duration &) +{ + T::ConversionNotImplemented; +} + +/// \brief Specialized conversion from a ROS duration message to a Gazebo Time. +/// \param[in] in ROS Time message to convert. +/// \return A Gazebo Time with the same value as in +template<> +gazebo::common::Time Convert(const builtin_interfaces::msg::Duration & in) +{ + gazebo::common::Time time; + time.sec = in.sec; + time.nsec = in.nanosec; + return time; +} + } // namespace gazebo_ros #endif // GAZEBO_ROS__CONVERSIONS__BUILTIN_INTERFACES_HPP_ diff --git a/gazebo_ros/test/test_conversions.cpp b/gazebo_ros/test/test_conversions.cpp index 3bce3d180..2f4766d06 100644 --- a/gazebo_ros/test/test_conversions.cpp +++ b/gazebo_ros/test/test_conversions.cpp @@ -86,6 +86,11 @@ TEST(TestConversions, Time) auto time_msg = gazebo_ros::Convert(time); EXPECT_EQ(200, time_msg.sec); EXPECT_EQ(100u, time_msg.nanosec); + + // to Gazebo time + auto gazebo_time = gazebo_ros::Convert(time_msg); + EXPECT_EQ(200, gazebo_time.sec); + EXPECT_EQ(100, gazebo_time.nsec); } // Gazebo msg @@ -99,6 +104,18 @@ TEST(TestConversions, Time) EXPECT_EQ(200, time_msg.sec); EXPECT_EQ(100u, time_msg.nanosec); } + + // ROS Duration + { + auto duration = builtin_interfaces::msg::Duration(); + duration.sec = 200; + duration.nanosec = 100u; + + // to Gazebo time + auto gazebo_time = gazebo_ros::Convert(duration); + EXPECT_EQ(200, gazebo_time.sec); + EXPECT_EQ(100, gazebo_time.nsec); + } } int main(int argc, char ** argv)