45 if (!sdf->HasElement(
"robotNamespace"))
47 ROS_INFO(
"PlanarMovePlugin missing <robotNamespace>, " 53 sdf->GetElement(
"robotNamespace")->Get<std::string>();
57 if (!sdf->HasElement(
"commandTopic"))
59 ROS_WARN(
"PlanarMovePlugin (ns = %s) missing <commandTopic>, " 65 command_topic_ = sdf->GetElement(
"commandTopic")->Get<std::string>();
69 if (!sdf->HasElement(
"odometryTopic"))
71 ROS_WARN(
"PlanarMovePlugin (ns = %s) missing <odometryTopic>, " 77 odometry_topic_ = sdf->GetElement(
"odometryTopic")->Get<std::string>();
81 if (!sdf->HasElement(
"odometryFrame"))
83 ROS_WARN(
"PlanarMovePlugin (ns = %s) missing <odometryFrame>, " 89 odometry_frame_ = sdf->GetElement(
"odometryFrame")->Get<std::string>();
97 if (sdf->HasElement(
"yaw_velocity_p_gain"))
98 (sdf->GetElement(
"yaw_velocity_p_gain")->GetValue()->Get(torque_yaw_velocity_p_gain_));
100 if (sdf->HasElement(
"x_velocity_p_gain"))
103 if (sdf->HasElement(
"y_velocity_p_gain"))
106 ROS_INFO_STREAM(
"ForceBasedMove using gains: yaw: " << torque_yaw_velocity_p_gain_ <<
111 if (!sdf->HasElement(
"robotBaseFrame"))
113 ROS_WARN(
"PlanarMovePlugin (ns = %s) missing <robotBaseFrame>, " 114 "defaults to \"%s\"",
127 if (!sdf->HasElement(
"odometryRate"))
129 ROS_WARN(
"PlanarMovePlugin (ns = %s) missing <odometryRate>, " 139 if (!sdf->HasElement(
"publishOdometryTf")) {
140 ROS_WARN(
"PlanarMovePlugin Plugin (ns = %s) missing <publishOdometryTf>, defaults to %s",
141 this->
robot_namespace_.c_str(), this->publish_odometry_tf_ ?
"true" :
"false");
143 this->publish_odometry_tf_ = sdf->GetElement(
"publishOdometryTf")->Get<
bool>();
146 #if (GAZEBO_MAJOR_VERSION >= 8) 150 last_odom_publish_time_ =
parent_->GetWorld()->GetSimTime();
164 <<
"). A ROS node for Gazebo has not been initialized, " 165 <<
"unable to load plugin. Load the Gazebo system plugin " 166 <<
"'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
176 if (publish_odometry_tf_)
181 ros::SubscribeOptions::create<geometry_msgs::Twist>(
command_topic_, 1,
194 event::Events::ConnectWorldUpdateBegin(
202 boost::mutex::scoped_lock scoped_lock(
lock);
203 #if (GAZEBO_MAJOR_VERSION >= 8) 204 ignition::math::Pose3d pose =
parent_->WorldPose();
206 ignition::math::Vector3d angular_vel =
parent_->WorldAngularVel();
208 double error = angular_vel.Z() -
rot_;
212 float yaw = pose.Rot().Yaw();
214 ignition::math::Vector3d linear_vel =
parent_->RelativeLinearVel();
220 math::Pose pose =
parent_->GetWorldPose();
222 math::Vector3 angular_vel =
parent_->GetWorldAngularVel();
224 double error = angular_vel.z -
rot_;
226 link_->AddTorque(math::Vector3(0.0, 0.0, -error * torque_yaw_velocity_p_gain_));
228 float yaw = pose.rot.GetYaw();
230 math::Vector3 linear_vel =
parent_->GetRelativeLinearVel();
244 #if (GAZEBO_MAJOR_VERSION >= 8) 245 common::Time current_time =
parent_->GetWorld()->SimTime();
247 common::Time current_time =
parent_->GetWorld()->GetSimTime();
249 double seconds_since_last_update =
268 const geometry_msgs::Twist::ConstPtr& cmd_msg)
270 boost::mutex::scoped_lock scoped_lock(
lock);
271 x_ = cmd_msg->linear.x;
272 y_ = cmd_msg->linear.y;
273 rot_ = cmd_msg->angular.z;
278 static const double timeout = 0.01;
290 std::string base_footprint_frame =
293 #if (GAZEBO_MAJOR_VERSION >= 8) 294 ignition::math::Vector3d angular_vel =
parent_->RelativeAngularVel();
295 ignition::math::Vector3d linear_vel =
parent_->RelativeLinearVel();
300 odom_.twist.twist.angular.z = angular_vel.Z();
301 odom_.twist.twist.linear.x = linear_vel.X();
302 odom_.twist.twist.linear.y = linear_vel.Y();
304 math::Vector3 angular_vel =
parent_->GetRelativeAngularVel();
305 math::Vector3 linear_vel =
parent_->GetRelativeLinearVel();
310 odom_.twist.twist.angular.z = angular_vel.z;
311 odom_.twist.twist.linear.x = linear_vel.x;
312 odom_.twist.twist.linear.y = linear_vel.y;
315 odom_.header.stamp = current_time;
316 odom_.header.frame_id = odom_frame;
317 odom_.child_frame_id = base_footprint_frame;
322 base_footprint_frame));
325 odom_.pose.covariance[0] = 0.001;
326 odom_.pose.covariance[7] = 0.001;
327 odom_.pose.covariance[14] = 1000000000000.0;
328 odom_.pose.covariance[21] = 1000000000000.0;
329 odom_.pose.covariance[28] = 1000000000000.0;
331 #if (GAZEBO_MAJOR_VERSION >= 8) 332 if (std::abs(angular_vel.Z()) < 0.0001) {
334 if (std::abs(angular_vel.z) < 0.0001) {
336 odom_.pose.covariance[35] = 0.01;
338 odom_.pose.covariance[35] = 100.0;
341 odom_.twist.covariance[0] = 0.001;
342 odom_.twist.covariance[7] = 0.001;
343 odom_.twist.covariance[14] = 0.001;
344 odom_.twist.covariance[21] = 1000000000000.0;
345 odom_.twist.covariance[28] = 1000000000000.0;
347 #if (GAZEBO_MAJOR_VERSION >= 8) 348 if (std::abs(angular_vel.Z()) < 0.0001) {
350 if (std::abs(angular_vel.z) < 0.0001) {
352 odom_.twist.covariance[35] = 0.01;
354 odom_.twist.covariance[35] = 100.0;
368 if (std::abs(angular_vel) < 0.0001) {
370 tmp.
setOrigin(
tf::Vector3(static_cast<double>(linear_vel_x*timeSeconds), static_cast<double>(linear_vel_y*timeSeconds), 0.0));
372 const double distX = linear_vel_x * timeSeconds;
373 const double distY = linear_vel_y * timeSeconds;
375 if (std::abs(distX) < 0.0001 && std::abs(distY) < 0.0001) {
377 const double angleChange = angular_vel * timeSeconds;
381 const double distChange = std::sqrt(distX * distX + distY * distY);
382 const double angleDriveDirection = std::atan2(distY, distX);
383 const double angleChange = angular_vel * timeSeconds;
385 const double arcRadius = distChange / angleChange;
388 arcRadius - std::cos(angleChange) * arcRadius,
virtual void UpdateChild()
boost::shared_ptr< ros::NodeHandle > rosnode_
std::string command_topic_
void publish(const boost::shared_ptr< M > &message) const
double force_y_velocity_p_gain_
std::string getPrefixParam(ros::NodeHandle &nh)
physics::LinkPtr link_
A pointer to the Link, where force is applied.
ROSCPP_DECL bool isInitialized()
ros::Publisher odometry_pub_
physics::ModelPtr parent_
void publishOdometry(double step_time)
std::string resolve(const std::string &prefix, const std::string &frame_name)
ros::CallbackQueue queue_
std::string robot_namespace_
math::Pose last_odom_pose_
TFSIMD_FORCE_INLINE Vector3 rotate(const Vector3 &wAxis, const tfScalar angle) const
static Quaternion createQuaternionFromYaw(double yaw)
#define ROS_FATAL_STREAM(args)
std::string odometry_topic_
tf::Transform odom_transform_
std::string odometry_frame_
std::string robot_base_frame_
void Load(physics::ModelPtr parent, sdf::ElementPtr sdf)
boost::shared_ptr< tf::TransformBroadcaster > transform_broadcaster_
tf::Transform getTransformForMotion(double linear_vel_x, double linear_vel_y, double angular_vel, double timeSeconds) const
common::Time last_odom_publish_time_
event::ConnectionPtr update_connection_
bool publish_odometry_tf_
static void poseTFToMsg(const Pose &bt, geometry_msgs::Pose &msg)
#define ROS_INFO_STREAM(args)
~GazeboRosForceBasedMove()
double force_x_velocity_p_gain_
void cmdVelCallback(const geometry_msgs::Twist::ConstPtr &cmd_msg)
boost::thread callback_queue_thread_
boost::shared_ptr< void > VoidPtr
double torque_yaw_velocity_p_gain_
GazeboRosForceBasedMove()