diff --git a/.ros1_unported/gazebo_plugins/include/gazebo_plugins/gazebo_ros_planar_move.h b/.ros1_unported/gazebo_plugins/include/gazebo_plugins/gazebo_ros_planar_move.h deleted file mode 100644 index d399443d1..000000000 --- a/.ros1_unported/gazebo_plugins/include/gazebo_plugins/gazebo_ros_planar_move.h +++ /dev/null @@ -1,99 +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: Simple model controller that uses a twist message to move a robot on - * the xy plane. - * Author: Piyush Khandelwal - * Date: 29 July 2013 - */ - -#ifndef GAZEBO_ROS_PLANAR_MOVE_HH -#define GAZEBO_ROS_PLANAR_MOVE_HH - -#include -#include -#include - -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -namespace gazebo { - - class GazeboRosPlanarMove : public ModelPlugin { - - public: - GazeboRosPlanarMove(); - ~GazeboRosPlanarMove(); - void Load(physics::ModelPtr parent, sdf::ElementPtr sdf); - - protected: - virtual void UpdateChild(); - virtual void FiniChild(); - - private: - void publishOdometry(double step_time); - - physics::ModelPtr parent_; - event::ConnectionPtr update_connection_; - - boost::shared_ptr rosnode_; - ros::Publisher odometry_pub_; - ros::Subscriber vel_sub_; - boost::shared_ptr transform_broadcaster_; - nav_msgs::Odometry odom_; - std::string tf_prefix_; - - boost::mutex lock; - - std::string robot_namespace_; - std::string command_topic_; - std::string odometry_topic_; - std::string odometry_frame_; - std::string robot_base_frame_; - double odometry_rate_; - - // Custom Callback Queue - ros::CallbackQueue queue_; - boost::thread callback_queue_thread_; - void QueueThread(); - - // command velocity callback - void cmdVelCallback(const geometry_msgs::Twist::ConstPtr& cmd_msg); - - double x_; - double y_; - double rot_; - bool alive_; - common::Time last_odom_publish_time_; - ignition::math::Pose3d last_odom_pose_; - - }; - -} - -#endif /* end of include guard: GAZEBO_ROS_PLANAR_MOVE_HH */ diff --git a/.ros1_unported/gazebo_plugins/src/gazebo_ros_planar_move.cpp b/.ros1_unported/gazebo_plugins/src/gazebo_ros_planar_move.cpp deleted file mode 100644 index fdad12bc9..000000000 --- a/.ros1_unported/gazebo_plugins/src/gazebo_ros_planar_move.cpp +++ /dev/null @@ -1,295 +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: Simple model controller that uses a twist message to move a robot on - * the xy plane. - * Author: Piyush Khandelwal - * Date: 29 July 2013 - */ - -#include - -namespace gazebo -{ - - GazeboRosPlanarMove::GazeboRosPlanarMove() {} - - GazeboRosPlanarMove::~GazeboRosPlanarMove() {} - - // Load the controller - void GazeboRosPlanarMove::Load(physics::ModelPtr parent, - sdf::ElementPtr sdf) - { - - parent_ = parent; - - /* Parse parameters */ - - robot_namespace_ = ""; - if (!sdf->HasElement("robotNamespace")) - { - ROS_INFO_NAMED("planar_move", "PlanarMovePlugin missing , " - "defaults to \"%s\"", robot_namespace_.c_str()); - } - else - { - robot_namespace_ = - sdf->GetElement("robotNamespace")->Get(); - } - - command_topic_ = "cmd_vel"; - if (!sdf->HasElement("commandTopic")) - { - ROS_WARN_NAMED("planar_move", "PlanarMovePlugin (ns = %s) missing , " - "defaults to \"%s\"", - robot_namespace_.c_str(), command_topic_.c_str()); - } - else - { - command_topic_ = sdf->GetElement("commandTopic")->Get(); - } - - odometry_topic_ = "odom"; - if (!sdf->HasElement("odometryTopic")) - { - ROS_WARN_NAMED("planar_move", "PlanarMovePlugin (ns = %s) missing , " - "defaults to \"%s\"", - robot_namespace_.c_str(), odometry_topic_.c_str()); - } - else - { - odometry_topic_ = sdf->GetElement("odometryTopic")->Get(); - } - - odometry_frame_ = "odom"; - if (!sdf->HasElement("odometryFrame")) - { - ROS_WARN_NAMED("planar_move", "PlanarMovePlugin (ns = %s) missing , " - "defaults to \"%s\"", - robot_namespace_.c_str(), odometry_frame_.c_str()); - } - else - { - odometry_frame_ = sdf->GetElement("odometryFrame")->Get(); - } - - robot_base_frame_ = "base_footprint"; - if (!sdf->HasElement("robotBaseFrame")) - { - ROS_WARN_NAMED("planar_move", "PlanarMovePlugin (ns = %s) missing , " - "defaults to \"%s\"", - robot_namespace_.c_str(), robot_base_frame_.c_str()); - } - else - { - robot_base_frame_ = sdf->GetElement("robotBaseFrame")->Get(); - } - - odometry_rate_ = 20.0; - if (!sdf->HasElement("odometryRate")) - { - ROS_WARN_NAMED("planar_move", "PlanarMovePlugin (ns = %s) missing , " - "defaults to %f", - robot_namespace_.c_str(), odometry_rate_); - } - else - { - odometry_rate_ = sdf->GetElement("odometryRate")->Get(); - } - -#if GAZEBO_MAJOR_VERSION >= 8 - last_odom_publish_time_ = parent_->GetWorld()->SimTime(); -#else - last_odom_publish_time_ = parent_->GetWorld()->GetSimTime(); -#endif -#if GAZEBO_MAJOR_VERSION >= 8 - last_odom_pose_ = parent_->WorldPose(); -#else - last_odom_pose_ = parent_->GetWorldPose().Ign(); -#endif - x_ = 0; - y_ = 0; - rot_ = 0; - alive_ = true; - - // Ensure that ROS has been initialized and subscribe to cmd_vel - if (!ros::isInitialized()) - { - ROS_FATAL_STREAM_NAMED("planar_move", "PlanarMovePlugin (ns = " << robot_namespace_ - << "). 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; - } - rosnode_.reset(new ros::NodeHandle(robot_namespace_)); - - ROS_DEBUG_NAMED("planar_move", "OCPlugin (%s) has started", - robot_namespace_.c_str()); - - tf_prefix_ = tf::getPrefixParam(*rosnode_); - transform_broadcaster_.reset(new tf::TransformBroadcaster()); - - // subscribe to the odometry topic - ros::SubscribeOptions so = - ros::SubscribeOptions::create(command_topic_, 1, - boost::bind(&GazeboRosPlanarMove::cmdVelCallback, this, _1), - ros::VoidPtr(), &queue_); - - vel_sub_ = rosnode_->subscribe(so); - odometry_pub_ = rosnode_->advertise(odometry_topic_, 1); - - // start custom queue for diff drive - callback_queue_thread_ = - boost::thread(boost::bind(&GazeboRosPlanarMove::QueueThread, this)); - - // listen to the update event (broadcast every simulation iteration) - update_connection_ = - event::Events::ConnectWorldUpdateBegin( - boost::bind(&GazeboRosPlanarMove::UpdateChild, this)); - - } - - // Update the controller - void GazeboRosPlanarMove::UpdateChild() - { - boost::mutex::scoped_lock scoped_lock(lock); -#if GAZEBO_MAJOR_VERSION >= 8 - ignition::math::Pose3d pose = parent_->WorldPose(); -#else - ignition::math::Pose3d pose = parent_->GetWorldPose().Ign(); -#endif - float yaw = pose.Rot().Yaw(); - parent_->SetLinearVel(ignition::math::Vector3d( - x_ * cosf(yaw) - y_ * sinf(yaw), - y_ * cosf(yaw) + x_ * sinf(yaw), - 0)); - parent_->SetAngularVel(ignition::math::Vector3d(0, 0, rot_)); - if (odometry_rate_ > 0.0) { -#if GAZEBO_MAJOR_VERSION >= 8 - common::Time current_time = parent_->GetWorld()->SimTime(); -#else - common::Time current_time = parent_->GetWorld()->GetSimTime(); -#endif - double seconds_since_last_update = - (current_time - last_odom_publish_time_).Double(); - if (seconds_since_last_update > (1.0 / odometry_rate_)) { - publishOdometry(seconds_since_last_update); - last_odom_publish_time_ = current_time; - } - } - } - - // Finalize the controller - void GazeboRosPlanarMove::FiniChild() { - alive_ = false; - queue_.clear(); - queue_.disable(); - rosnode_->shutdown(); - callback_queue_thread_.join(); - } - - void GazeboRosPlanarMove::cmdVelCallback( - const geometry_msgs::Twist::ConstPtr& cmd_msg) - { - boost::mutex::scoped_lock scoped_lock(lock); - x_ = cmd_msg->linear.x; - y_ = cmd_msg->linear.y; - rot_ = cmd_msg->angular.z; - } - - void GazeboRosPlanarMove::QueueThread() - { - static const double timeout = 0.01; - while (alive_ && rosnode_->ok()) - { - queue_.callAvailable(ros::WallDuration(timeout)); - } - } - - void GazeboRosPlanarMove::publishOdometry(double step_time) - { - - ros::Time current_time = ros::Time::now(); - std::string odom_frame = tf::resolve(tf_prefix_, odometry_frame_); - std::string base_footprint_frame = - tf::resolve(tf_prefix_, robot_base_frame_); - - // getting data for base_footprint to odom transform -#if GAZEBO_MAJOR_VERSION >= 8 - ignition::math::Pose3d pose = this->parent_->WorldPose(); -#else - ignition::math::Pose3d pose = this->parent_->GetWorldPose().Ign(); -#endif - - tf::Quaternion qt(pose.Rot().X(), pose.Rot().Y(), pose.Rot().Z(), pose.Rot().W()); - tf::Vector3 vt(pose.Pos().X(), pose.Pos().Y(), pose.Pos().Z()); - - tf::Transform base_footprint_to_odom(qt, vt); - transform_broadcaster_->sendTransform( - tf::StampedTransform(base_footprint_to_odom, current_time, odom_frame, - base_footprint_frame)); - - // publish odom topic - odom_.pose.pose.position.x = pose.Pos().X(); - odom_.pose.pose.position.y = pose.Pos().Y(); - - odom_.pose.pose.orientation.x = pose.Rot().X(); - odom_.pose.pose.orientation.y = pose.Rot().Y(); - odom_.pose.pose.orientation.z = pose.Rot().Z(); - odom_.pose.pose.orientation.w = pose.Rot().W(); - odom_.pose.covariance[0] = 0.00001; - odom_.pose.covariance[7] = 0.00001; - odom_.pose.covariance[14] = 1000000000000.0; - odom_.pose.covariance[21] = 1000000000000.0; - odom_.pose.covariance[28] = 1000000000000.0; - odom_.pose.covariance[35] = 0.001; - - // get velocity in /odom frame - ignition::math::Vector3d linear; - linear.X() = (pose.Pos().X() - last_odom_pose_.Pos().X()) / step_time; - linear.Y() = (pose.Pos().Y() - last_odom_pose_.Pos().Y()) / step_time; - if (rot_ > M_PI / step_time) - { - // we cannot calculate the angular velocity correctly - odom_.twist.twist.angular.z = rot_; - } - else - { - float last_yaw = last_odom_pose_.Rot().Yaw(); - float current_yaw = pose.Rot().Yaw(); - while (current_yaw < last_yaw - M_PI) current_yaw += 2 * M_PI; - while (current_yaw > last_yaw + M_PI) current_yaw -= 2 * M_PI; - float angular_diff = current_yaw - last_yaw; - odom_.twist.twist.angular.z = angular_diff / step_time; - } - last_odom_pose_ = pose; - - // convert velocity to child_frame_id (aka base_footprint) - float yaw = pose.Rot().Yaw(); - odom_.twist.twist.linear.x = cosf(yaw) * linear.X() + sinf(yaw) * linear.Y(); - odom_.twist.twist.linear.y = cosf(yaw) * linear.Y() - sinf(yaw) * linear.X(); - - odom_.header.stamp = current_time; - odom_.header.frame_id = odom_frame; - odom_.child_frame_id = base_footprint_frame; - - odometry_pub_.publish(odom_); - } - - GZ_REGISTER_MODEL_PLUGIN(GazeboRosPlanarMove) -} diff --git a/gazebo_plugins/CMakeLists.txt b/gazebo_plugins/CMakeLists.txt index 77fd50b7f..3f4749ca4 100644 --- a/gazebo_plugins/CMakeLists.txt +++ b/gazebo_plugins/CMakeLists.txt @@ -201,6 +201,21 @@ target_link_libraries(gazebo_ros_elevator ) ament_export_libraries(gazebo_ros_elevator) +# gazebo_ros_planar_move +add_library(gazebo_ros_planar_move SHARED + src/gazebo_ros_planar_move.cpp +) +ament_target_dependencies(gazebo_ros_planar_move + "gazebo_dev" + "gazebo_ros" + "nav_msgs" + "rclcpp" + "tf2" + "tf2_geometry_msgs" + "tf2_ros" +) +ament_export_libraries(gazebo_ros_planar_move) + # gazebo_ros_projector add_library(gazebo_ros_projector SHARED src/gazebo_ros_projector.cpp @@ -237,6 +252,7 @@ install(TARGETS gazebo_ros_ft_sensor gazebo_ros_imu_sensor gazebo_ros_joint_state_publisher + gazebo_ros_planar_move gazebo_ros_projector gazebo_ros_ray_sensor gazebo_ros_p3d diff --git a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_planar_move.hpp b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_planar_move.hpp new file mode 100644 index 000000000..b4db7836c --- /dev/null +++ b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_planar_move.hpp @@ -0,0 +1,89 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef GAZEBO_PLUGINS__GAZEBO_ROS_PLANAR_MOVE_HPP_ +#define GAZEBO_PLUGINS__GAZEBO_ROS_PLANAR_MOVE_HPP_ + +#include + +#include + +namespace gazebo_plugins +{ +class GazeboRosPlanarMovePrivate; + +/// Simple model controller that uses a twist message to move an entity on the xy plane. +/* + * \author Piyush Khandelwal (piyushk@gmail.com) + * + * \date 29 July 2013 + */ + +/** + Example Usage: + \code{.xml} + + + + + + /demo + + + cmd_vel:=custom_cmd_vel + odom:=custom_odom + + + + 100 + 10 + + + true + true + + odom_demo + link + + 0.0001 + 0.0001 + 0.01 + + + \endcode +*/ + +class GazeboRosPlanarMove : public gazebo::ModelPlugin +{ +public: + /// Constructor + GazeboRosPlanarMove(); + + /// Destructor + ~GazeboRosPlanarMove(); + +protected: + // Documentation inherited + void Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf) override; + + // Documentation inherited + void Reset() override; + +private: + /// Private data pointer + std::unique_ptr impl_; +}; +} // namespace gazebo_plugins + +#endif // GAZEBO_PLUGINS__GAZEBO_ROS_PLANAR_MOVE_HPP_ diff --git a/gazebo_plugins/src/gazebo_ros_planar_move.cpp b/gazebo_plugins/src/gazebo_ros_planar_move.cpp new file mode 100644 index 000000000..266cb57f8 --- /dev/null +++ b/gazebo_plugins/src/gazebo_ros_planar_move.cpp @@ -0,0 +1,294 @@ +// 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. + +/* + * \brief Simple model controller that uses a twist message to move an entity on the xy plane. + * + * \author Piyush Khandelwal (piyushk@gmail.com) + * + * \date 29 July 2013 + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +namespace gazebo_plugins +{ +class GazeboRosPlanarMovePrivate +{ +public: + /// Callback to be called at every simulation iteration. + /// \param[in] _info Updated simulation info. + void OnUpdate(const gazebo::common::UpdateInfo & _info); + + /// Callback when a velocity command is received. + /// \param[in] _msg Twist command message. + void OnCmdVel(const geometry_msgs::msg::Twist::SharedPtr _msg); + + /// Update odometry. + /// \param[in] _current_time Current simulation time + void UpdateOdometry(const gazebo::common::Time & _current_time); + + /// Publish odometry transforms + /// \param[in] _current_time Current simulation time + void PublishOdometryTf(const gazebo::common::Time & _current_time); + + /// A pointer to the GazeboROS node. + gazebo_ros::Node::SharedPtr ros_node_; + + /// Subscriber to command velocities + rclcpp::Subscription::SharedPtr cmd_vel_sub_; + + /// Odometry publisher + rclcpp::Publisher::SharedPtr odometry_pub_; + + /// To broadcast TF + std::shared_ptr transform_broadcaster_; + + /// Velocity received on command. + geometry_msgs::msg::Twist target_cmd_vel_; + + /// Keep latest odometry message + nav_msgs::msg::Odometry odom_; + + /// Pointer to world. + gazebo::physics::WorldPtr world_; + + /// Pointer to model. + gazebo::physics::ModelPtr model_; + + /// Connection to event called at every world iteration. + gazebo::event::ConnectionPtr update_connection_; + + /// Protect variables accessed on callbacks. + std::mutex lock_; + + /// Update period in seconds. + double update_period_; + + /// Publish period in seconds. + double publish_period_; + + /// Last update time. + gazebo::common::Time last_update_time_; + + /// Last publish time. + gazebo::common::Time last_publish_time_; + + /// Odometry frame ID + std::string odometry_frame_; + + /// Robot base frame ID + std::string robot_base_frame_; + + /// True to publish odometry messages. + bool publish_odom_; + + /// True to publish odom-to-world transforms. + bool publish_odom_tf_; +}; + +GazeboRosPlanarMove::GazeboRosPlanarMove() +: impl_(std::make_unique()) +{ +} + +GazeboRosPlanarMove::~GazeboRosPlanarMove() +{ +} + +void GazeboRosPlanarMove::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); + + // Odometry + impl_->odometry_frame_ = _sdf->Get("odometry_frame", "odom").first; + impl_->robot_base_frame_ = _sdf->Get("robot_base_frame", "base_footprint").first; + + // Update rate + auto update_rate = _sdf->Get("update_rate", 20.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(); + + // Update rate + auto publish_rate = _sdf->Get("publish_rate", 20.0).first; + if (update_rate > 0.0) { + impl_->publish_period_ = 1.0 / publish_rate; + } else { + impl_->publish_period_ = 0.0; + } + impl_->last_publish_time_ = impl_->world_->SimTime(); + + impl_->cmd_vel_sub_ = impl_->ros_node_->create_subscription( + "cmd_vel", rclcpp::QoS(rclcpp::KeepLast(1)), + std::bind(&GazeboRosPlanarMovePrivate::OnCmdVel, impl_.get(), std::placeholders::_1)); + + RCLCPP_INFO(impl_->ros_node_->get_logger(), "Subscribed to [%s]", + impl_->cmd_vel_sub_->get_topic_name()); + + // Advertise odometry topic + impl_->publish_odom_ = _sdf->Get("publish_odom", true).first; + if (impl_->publish_odom_) { + impl_->odometry_pub_ = impl_->ros_node_->create_publisher( + "odom", rclcpp::QoS(rclcpp::KeepLast(1))); + + RCLCPP_INFO(impl_->ros_node_->get_logger(), "Advertise odometry on [%s]", + impl_->odometry_pub_->get_topic_name()); + } + + // Broadcast TF + impl_->publish_odom_tf_ = _sdf->Get("publish_odom_tf", true).first; + if (impl_->publish_odom_tf_) { + impl_->transform_broadcaster_ = + std::make_shared(impl_->ros_node_); + + RCLCPP_INFO(impl_->ros_node_->get_logger(), + "Publishing odom transforms between [%s] and [%s]", impl_->odometry_frame_.c_str(), + impl_->robot_base_frame_.c_str()); + } + + auto covariance_x = _sdf->Get("covariance_x", 0.00001).first; + auto covariance_y = _sdf->Get("covariance_y", 0.00001).first; + auto covariance_yaw = _sdf->Get("covariance_yaw", 0.001).first; + + // Set covariance + impl_->odom_.pose.covariance[0] = covariance_x; + impl_->odom_.pose.covariance[7] = covariance_y; + impl_->odom_.pose.covariance[14] = 1000000000000.0; + impl_->odom_.pose.covariance[21] = 1000000000000.0; + impl_->odom_.pose.covariance[28] = 1000000000000.0; + impl_->odom_.pose.covariance[35] = covariance_yaw; + + impl_->odom_.twist.covariance[0] = covariance_x; + impl_->odom_.twist.covariance[7] = covariance_y; + impl_->odom_.twist.covariance[14] = 1000000000000.0; + impl_->odom_.twist.covariance[21] = 1000000000000.0; + impl_->odom_.twist.covariance[28] = 1000000000000.0; + impl_->odom_.twist.covariance[35] = covariance_yaw; + + // Set header + impl_->odom_.header.frame_id = impl_->odometry_frame_; + impl_->odom_.child_frame_id = impl_->robot_base_frame_; + + // Listen to the update event (broadcast every simulation iteration) + impl_->update_connection_ = gazebo::event::Events::ConnectWorldUpdateBegin( + std::bind(&GazeboRosPlanarMovePrivate::OnUpdate, impl_.get(), std::placeholders::_1)); +} + +void GazeboRosPlanarMove::Reset() +{ + impl_->last_update_time_ = impl_->world_->SimTime(); + impl_->target_cmd_vel_.linear.x = 0; + impl_->target_cmd_vel_.linear.y = 0; + impl_->target_cmd_vel_.angular.z = 0; +} + +void GazeboRosPlanarMovePrivate::OnUpdate(const gazebo::common::UpdateInfo & _info) +{ + double seconds_since_last_update = (_info.simTime - last_update_time_).Double(); + + std::lock_guard scoped_lock(lock_); + + if (seconds_since_last_update >= update_period_) { + ignition::math::Pose3d pose = model_->WorldPose(); + auto yaw = static_cast(pose.Rot().Yaw()); + model_->SetLinearVel(ignition::math::Vector3d( + target_cmd_vel_.linear.x * cosf(yaw) - target_cmd_vel_.linear.y * sinf(yaw), + target_cmd_vel_.linear.y * cosf(yaw) + target_cmd_vel_.linear.x * sinf(yaw), + 0)); + model_->SetAngularVel(ignition::math::Vector3d(0, 0, target_cmd_vel_.angular.z)); + + last_update_time_ = _info.simTime; + } + + if (publish_odom_ || publish_odom_tf_) { + double seconds_since_last_publish = (_info.simTime - last_publish_time_).Double(); + + if (seconds_since_last_publish < publish_period_) { + return; + } + + UpdateOdometry(_info.simTime); + + if (publish_odom_) { + odometry_pub_->publish(odom_); + } + + if (publish_odom_tf_) { + PublishOdometryTf(_info.simTime); + } + last_publish_time_ = _info.simTime; + } +} + +void GazeboRosPlanarMovePrivate::OnCmdVel(const geometry_msgs::msg::Twist::SharedPtr _msg) +{ + std::lock_guard scoped_lock(lock_); + target_cmd_vel_ = *_msg; +} + +void GazeboRosPlanarMovePrivate::UpdateOdometry(const gazebo::common::Time & _current_time) +{ + auto pose = model_->WorldPose(); + odom_.pose.pose = gazebo_ros::Convert(pose); + + // Get velocity in odom frame + odom_.twist.twist.angular.z = model_->WorldAngularVel().Z(); + + // Convert velocity to child_frame_id(aka base_footprint) + auto linear = model_->WorldLinearVel(); + auto yaw = static_cast(pose.Rot().Yaw()); + odom_.twist.twist.linear.x = cosf(yaw) * linear.X() + sinf(yaw) * linear.Y(); + odom_.twist.twist.linear.y = cosf(yaw) * linear.Y() - sinf(yaw) * linear.X(); + + // Set timestamp + odom_.header.stamp = gazebo_ros::Convert(_current_time); +} + +void GazeboRosPlanarMovePrivate::PublishOdometryTf(const gazebo::common::Time & _current_time) +{ + geometry_msgs::msg::TransformStamped msg; + msg.header.stamp = gazebo_ros::Convert(_current_time); + msg.header.frame_id = odometry_frame_; + msg.child_frame_id = robot_base_frame_; + msg.transform = gazebo_ros::Convert(odom_.pose.pose); + + transform_broadcaster_->sendTransform(msg); +} + +GZ_REGISTER_MODEL_PLUGIN(GazeboRosPlanarMove) +} // namespace gazebo_plugins diff --git a/gazebo_plugins/test/CMakeLists.txt b/gazebo_plugins/test/CMakeLists.txt index 7fedb9044..aa7e31cb6 100644 --- a/gazebo_plugins/test/CMakeLists.txt +++ b/gazebo_plugins/test/CMakeLists.txt @@ -19,6 +19,7 @@ set(tests test_gazebo_ros_imu_sensor test_gazebo_ros_joint_state_publisher test_gazebo_ros_p3d + test_gazebo_ros_planar_move test_gazebo_ros_ray_sensor test_gazebo_ros_tricycle_drive ) diff --git a/gazebo_plugins/test/test_gazebo_ros_planar_move.cpp b/gazebo_plugins/test/test_gazebo_ros_planar_move.cpp new file mode 100644 index 000000000..0165f3606 --- /dev/null +++ b/gazebo_plugins/test/test_gazebo_ros_planar_move.cpp @@ -0,0 +1,123 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include + +#include + +#define tol 10e-2 + +using namespace std::literals::chrono_literals; // NOLINT + +class GazeboRosPlanarMoveTest : public gazebo::ServerFixture +{ +}; + +TEST_F(GazeboRosPlanarMoveTest, Publishing) +{ + // Load test world and start paused + this->Load("worlds/gazebo_ros_planar_move.world", true); + + // World + auto world = gazebo::physics::get_world(); + ASSERT_NE(nullptr, world); + + // Box + auto box = world->ModelByName("box"); + ASSERT_NE(nullptr, box); + + // Create node and executor + auto node = std::make_shared("gazebo_ros_planar_move_test"); + ASSERT_NE(nullptr, node); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node); + + // Create subscriber + nav_msgs::msg::Odometry::SharedPtr latestMsg; + auto sub = node->create_subscription( + "test/odom_test", rclcpp::QoS(rclcpp::KeepLast(1)), + [&latestMsg](const nav_msgs::msg::Odometry::SharedPtr _msg) { + latestMsg = _msg; + }); + + // Step a bit for model to settle + world->Step(100); + executor.spin_once(100ms); + + // Check model state + EXPECT_NEAR(0.0, box->WorldPose().Pos().X(), tol); + EXPECT_NEAR(0.0, box->WorldPose().Pos().Y(), tol); + EXPECT_NEAR(0.0, box->WorldPose().Rot().Yaw(), tol); + EXPECT_NEAR(0.0, box->WorldLinearVel().X(), tol); + EXPECT_NEAR(0.0, box->WorldLinearVel().Y(), tol); + EXPECT_NEAR(0.0, box->WorldAngularVel().Z(), tol); + + // Send command + auto pub = node->create_publisher( + "test/cmd_test", rclcpp::QoS(rclcpp::KeepLast(1))); + + auto msg = geometry_msgs::msg::Twist(); + msg.linear.x = 1.0; + msg.angular.z = 0.1; + pub->publish(msg); + + // Wait for it to be processed + int sleep{0}; + int maxSleep{300}; + + auto yaw = static_cast(box->WorldPose().Rot().Yaw()); + auto linear_vel = box->WorldLinearVel(); + double linear_vel_x = cosf(yaw) * linear_vel.X() + sinf(yaw) * linear_vel.Y(); + + for (; sleep < maxSleep && (linear_vel_x < 0.9 || + box->WorldAngularVel().Z() < 0.09); ++sleep) + { + yaw = static_cast(box->WorldPose().Rot().Yaw()); + linear_vel = box->WorldLinearVel(); + linear_vel_x = cosf(yaw) * linear_vel.X() + sinf(yaw) * linear_vel.Y(); + world->Step(100); + executor.spin_once(100ms); + gazebo::common::Time::MSleep(100); + pub->publish(msg); + } + EXPECT_NE(sleep, maxSleep); + + // Check message + ASSERT_NE(nullptr, latestMsg); + EXPECT_EQ("odom_frame_test", latestMsg->header.frame_id); + EXPECT_LT(0.0, latestMsg->pose.pose.position.x); + EXPECT_LT(0.0, latestMsg->pose.pose.orientation.z); + + // Check movement + yaw = static_cast(box->WorldPose().Rot().Yaw()); + linear_vel = box->WorldLinearVel(); + linear_vel_x = cosf(yaw) * linear_vel.X() + sinf(yaw) * linear_vel.Y(); + EXPECT_LT(0.0, box->WorldPose().Pos().X()); + EXPECT_LT(0.0, yaw); + EXPECT_NEAR(1.0, linear_vel_x, tol); + EXPECT_NEAR(1.0, box->WorldLinearVel().X(), tol); + EXPECT_NEAR(0.1, box->WorldAngularVel().Z(), tol); +} + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/gazebo_plugins/test/worlds/gazebo_ros_planar_move.world b/gazebo_plugins/test/worlds/gazebo_ros_planar_move.world new file mode 100644 index 000000000..26a1024c2 --- /dev/null +++ b/gazebo_plugins/test/worlds/gazebo_ros_planar_move.world @@ -0,0 +1,46 @@ + + + + + + model://ground_plane + + + + 0 0 0.5 0 0 0 + + + + + 1 1 1 + + + + + + + 1 1 1 + + + + + + + + /test + cmd_vel:=cmd_test + odom:=odom_test + + + 100 + 100 + + true + true + odom_frame_test + link + + + + + diff --git a/gazebo_plugins/worlds/gazebo_ros_planar_move_demo.world b/gazebo_plugins/worlds/gazebo_ros_planar_move_demo.world new file mode 100644 index 000000000..a67822a3f --- /dev/null +++ b/gazebo_plugins/worlds/gazebo_ros_planar_move_demo.world @@ -0,0 +1,73 @@ + + + + + + + model://ground_plane + + + + model://sun + + + + 0 0 0.5 0 0 0 + + + + + 1 1 1 + + + + + + + 1 1 1 + + + + + + + + /demo + cmd_vel:=cmd_demo + odom:=odom_demo + + + 100 + 10 + + + true + true + + odom_demo + link + + 0.0001 + 0.0001 + 0.01 + + + + + diff --git a/gazebo_ros/include/gazebo_ros/conversions/generic.hpp b/gazebo_ros/include/gazebo_ros/conversions/generic.hpp index 7e866ca09..690c04743 100644 --- a/gazebo_ros/include/gazebo_ros/conversions/generic.hpp +++ b/gazebo_ros/include/gazebo_ros/conversions/generic.hpp @@ -19,6 +19,7 @@ #include #include +#include #include #include #include @@ -49,6 +50,16 @@ T Convert(const ignition::math::Quaterniond &) T::ConversionNotImplemented; } +/// Generic conversion from an Ignition Math pose3d to another type. +/// \param[in] in Input pose3d +/// \return Conversion result +/// \tparam T Output type +template +T Convert(const ignition::math::Pose3d &) +{ + T::ConversionNotImplemented; +} + /// Generic conversion from an Gazebo Time object to another type. /// \param[in] in Input time; /// \return Conversion result diff --git a/gazebo_ros/include/gazebo_ros/conversions/geometry_msgs.hpp b/gazebo_ros/include/gazebo_ros/conversions/geometry_msgs.hpp index 8af2a4d8a..0dbc343f5 100644 --- a/gazebo_ros/include/gazebo_ros/conversions/geometry_msgs.hpp +++ b/gazebo_ros/include/gazebo_ros/conversions/geometry_msgs.hpp @@ -17,7 +17,9 @@ #include #include +#include #include +#include #include #include #include @@ -110,6 +112,28 @@ ignition::math::Vector3d Convert(const geometry_msgs::msg::Point & in) return out; } +/// Generic conversion from a ROS geometry pose message to another type. +/// \param[in] in Input message. +/// \return Conversion result +/// \tparam T Output type +template +T Convert(const geometry_msgs::msg::Pose &) +{ + T::ConversionNotImplemented; +} + +/// \brief Specialized conversion from a ROS pose message to a ROS geometry transform message. +/// \param[in] in ROS pose message to convert. +/// \return A ROS geometry transform message. +template<> +geometry_msgs::msg::Transform Convert(const geometry_msgs::msg::Pose & in) +{ + geometry_msgs::msg::Transform msg; + msg.translation = Convert(in.position); + msg.rotation = in.orientation; + return msg; +} + /// \brief Specialized conversion from an Ignition Math vector to a ROS message. /// \param[in] vec Ignition vector to convert. /// \return ROS geometry vector message @@ -150,6 +174,18 @@ geometry_msgs::msg::Quaternion Convert(const ignition::math::Quaterniond & in) return msg; } +/// \brief Specialized conversion from an Ignition Math Pose3d to a ROS geometry pose message. +/// \param[in] in Ignition Pose3d to convert. +/// \return ROS geometry pose message +template<> +geometry_msgs::msg::Pose Convert(const ignition::math::Pose3d & in) +{ + geometry_msgs::msg::Pose msg; + msg.position = Convert(in.Pos()); + msg.orientation = Convert(in.Rot()); + return msg; +} + /// Generic conversion from a ROS Quaternion message to another type /// \param[in] in Input quaternion /// \return Conversion result diff --git a/gazebo_ros/test/test_conversions.cpp b/gazebo_ros/test/test_conversions.cpp index 6473c4cf4..c866782f1 100644 --- a/gazebo_ros/test/test_conversions.cpp +++ b/gazebo_ros/test/test_conversions.cpp @@ -49,6 +49,30 @@ TEST(TestConversions, Quaternion) EXPECT_EQ(1.0, ign_quat.W()); } +TEST(TestConversions, Pose) +{ + // Ign to ROS Pose + ignition::math::Pose3d pose(0.6, 0.5, 0.7, 0.9, 0.0, 0.3, -0.1); + auto pose_msg = gazebo_ros::Convert(pose); + EXPECT_EQ(0.6, pose_msg.position.x); + EXPECT_EQ(0.5, pose_msg.position.y); + EXPECT_EQ(0.7, pose_msg.position.z); + EXPECT_EQ(0.9, pose_msg.orientation.w); + EXPECT_EQ(0.0, pose_msg.orientation.x); + EXPECT_EQ(0.3, pose_msg.orientation.y); + EXPECT_EQ(-0.1, pose_msg.orientation.z); + + // ROS Pose to ROS Transform + auto transform_msg = gazebo_ros::Convert(pose_msg); + EXPECT_EQ(0.6, transform_msg.translation.x); + EXPECT_EQ(0.5, transform_msg.translation.y); + EXPECT_EQ(0.7, transform_msg.translation.z); + EXPECT_EQ(0.9, transform_msg.rotation.w); + EXPECT_EQ(0.0, transform_msg.rotation.x); + EXPECT_EQ(0.3, transform_msg.rotation.y); + EXPECT_EQ(-0.1, transform_msg.rotation.z); +} + TEST(TestConversions, Time) { // Gazebo time