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();
303 math::Vector3 angular_vel =
parent_->GetRelativeAngularVel();
304 math::Vector3 linear_vel =
parent_->GetRelativeLinearVel();
309 odom_.twist.twist.angular.z = angular_vel.z;
310 odom_.twist.twist.linear.x = linear_vel.x;
313 odom_.header.stamp = current_time;
314 odom_.header.frame_id = odom_frame;
315 odom_.child_frame_id = base_footprint_frame;
320 base_footprint_frame));
323 odom_.pose.covariance[0] = 0.001;
324 odom_.pose.covariance[7] = 0.001;
325 odom_.pose.covariance[14] = 1000000000000.0;
326 odom_.pose.covariance[21] = 1000000000000.0;
327 odom_.pose.covariance[28] = 1000000000000.0;
329 #if (GAZEBO_MAJOR_VERSION >= 8) 330 if (std::abs(angular_vel.Z()) < 0.0001) {
332 if (std::abs(angular_vel.z) < 0.0001) {
334 odom_.pose.covariance[35] = 0.01;
336 odom_.pose.covariance[35] = 100.0;
339 odom_.twist.covariance[0] = 0.001;
340 odom_.twist.covariance[7] = 0.001;
341 odom_.twist.covariance[14] = 0.001;
342 odom_.twist.covariance[21] = 1000000000000.0;
343 odom_.twist.covariance[28] = 1000000000000.0;
345 #if (GAZEBO_MAJOR_VERSION >= 8) 346 if (std::abs(angular_vel.Z()) < 0.0001) {
348 if (std::abs(angular_vel.z) < 0.0001) {
350 odom_.twist.covariance[35] = 0.01;
352 odom_.twist.covariance[35] = 100.0;
367 if (std::abs(angular_vel) < 0.0001) {
372 double distChange = linear_vel_x * timeSeconds;
373 double angleChange = angular_vel * timeSeconds;
375 double arcRadius = distChange / angleChange;
378 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_
tf::Transform getTransformForMotion(double linear_vel_x, double angular_vel, double timeSeconds) const
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_
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_
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()