00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026 #include <hector_gazebo_plugins/gazebo_ros_force_based_move.h>
00027
00028 namespace gazebo
00029 {
00030
00031 GazeboRosForceBasedMove::GazeboRosForceBasedMove() {}
00032
00033 GazeboRosForceBasedMove::~GazeboRosForceBasedMove() {}
00034
00035
00036 void GazeboRosForceBasedMove::Load(physics::ModelPtr parent,
00037 sdf::ElementPtr sdf)
00038 {
00039
00040 parent_ = parent;
00041
00042
00043
00044 robot_namespace_ = "";
00045 if (!sdf->HasElement("robotNamespace"))
00046 {
00047 ROS_INFO("PlanarMovePlugin missing <robotNamespace>, "
00048 "defaults to \"%s\"", robot_namespace_.c_str());
00049 }
00050 else
00051 {
00052 robot_namespace_ =
00053 sdf->GetElement("robotNamespace")->Get<std::string>();
00054 }
00055
00056 command_topic_ = "cmd_vel";
00057 if (!sdf->HasElement("commandTopic"))
00058 {
00059 ROS_WARN("PlanarMovePlugin (ns = %s) missing <commandTopic>, "
00060 "defaults to \"%s\"",
00061 robot_namespace_.c_str(), command_topic_.c_str());
00062 }
00063 else
00064 {
00065 command_topic_ = sdf->GetElement("commandTopic")->Get<std::string>();
00066 }
00067
00068 odometry_topic_ = "odom";
00069 if (!sdf->HasElement("odometryTopic"))
00070 {
00071 ROS_WARN("PlanarMovePlugin (ns = %s) missing <odometryTopic>, "
00072 "defaults to \"%s\"",
00073 robot_namespace_.c_str(), odometry_topic_.c_str());
00074 }
00075 else
00076 {
00077 odometry_topic_ = sdf->GetElement("odometryTopic")->Get<std::string>();
00078 }
00079
00080 odometry_frame_ = "odom";
00081 if (!sdf->HasElement("odometryFrame"))
00082 {
00083 ROS_WARN("PlanarMovePlugin (ns = %s) missing <odometryFrame>, "
00084 "defaults to \"%s\"",
00085 robot_namespace_.c_str(), odometry_frame_.c_str());
00086 }
00087 else
00088 {
00089 odometry_frame_ = sdf->GetElement("odometryFrame")->Get<std::string>();
00090 }
00091
00092
00093 torque_yaw_velocity_p_gain_ = 100.0;
00094 force_x_velocity_p_gain_ = 10000.0;
00095 force_y_velocity_p_gain_ = 10000.0;
00096
00097 if (sdf->HasElement("yaw_velocity_p_gain"))
00098 (sdf->GetElement("yaw_velocity_p_gain")->GetValue()->Get(torque_yaw_velocity_p_gain_));
00099
00100 if (sdf->HasElement("x_velocity_p_gain"))
00101 (sdf->GetElement("x_velocity_p_gain")->GetValue()->Get(force_x_velocity_p_gain_));
00102
00103 if (sdf->HasElement("y_velocity_p_gain"))
00104 (sdf->GetElement("y_velocity_p_gain")->GetValue()->Get(force_y_velocity_p_gain_));
00105
00106 ROS_INFO_STREAM("ForceBasedMove using gains: yaw: " << torque_yaw_velocity_p_gain_ <<
00107 " x: " << force_x_velocity_p_gain_ <<
00108 " y: " << force_y_velocity_p_gain_ << "\n");
00109
00110 robot_base_frame_ = "base_footprint";
00111 if (!sdf->HasElement("robotBaseFrame"))
00112 {
00113 ROS_WARN("PlanarMovePlugin (ns = %s) missing <robotBaseFrame>, "
00114 "defaults to \"%s\"",
00115 robot_namespace_.c_str(), robot_base_frame_.c_str());
00116 }
00117 else
00118 {
00119 robot_base_frame_ = sdf->GetElement("robotBaseFrame")->Get<std::string>();
00120 }
00121
00122 ROS_INFO_STREAM("robotBaseFrame for force based move plugin: " << robot_base_frame_ << "\n");
00123
00124 this->link_ = parent->GetLink(robot_base_frame_);
00125
00126 odometry_rate_ = 20.0;
00127 if (!sdf->HasElement("odometryRate"))
00128 {
00129 ROS_WARN("PlanarMovePlugin (ns = %s) missing <odometryRate>, "
00130 "defaults to %f",
00131 robot_namespace_.c_str(), odometry_rate_);
00132 }
00133 else
00134 {
00135 odometry_rate_ = sdf->GetElement("odometryRate")->Get<double>();
00136 }
00137
00138 this->publish_odometry_tf_ = true;
00139 if (!sdf->HasElement("publishOdometryTf")) {
00140 ROS_WARN("PlanarMovePlugin Plugin (ns = %s) missing <publishOdometryTf>, defaults to %s",
00141 this->robot_namespace_.c_str(), this->publish_odometry_tf_ ? "true" : "false");
00142 } else {
00143 this->publish_odometry_tf_ = sdf->GetElement("publishOdometryTf")->Get<bool>();
00144 }
00145
00146 last_odom_publish_time_ = parent_->GetWorld()->GetSimTime();
00147 last_odom_pose_ = parent_->GetWorldPose();
00148 x_ = 0;
00149 y_ = 0;
00150 rot_ = 0;
00151 alive_ = true;
00152
00153 odom_transform_.setIdentity();
00154
00155
00156 if (!ros::isInitialized())
00157 {
00158 ROS_FATAL_STREAM("PlanarMovePlugin (ns = " << robot_namespace_
00159 << "). A ROS node for Gazebo has not been initialized, "
00160 << "unable to load plugin. Load the Gazebo system plugin "
00161 << "'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
00162 return;
00163 }
00164 rosnode_.reset(new ros::NodeHandle(robot_namespace_));
00165
00166 ROS_DEBUG("OCPlugin (%s) has started!",
00167 robot_namespace_.c_str());
00168
00169 tf_prefix_ = tf::getPrefixParam(*rosnode_);
00170
00171 if (publish_odometry_tf_)
00172 transform_broadcaster_.reset(new tf::TransformBroadcaster());
00173
00174
00175 ros::SubscribeOptions so =
00176 ros::SubscribeOptions::create<geometry_msgs::Twist>(command_topic_, 1,
00177 boost::bind(&GazeboRosForceBasedMove::cmdVelCallback, this, _1),
00178 ros::VoidPtr(), &queue_);
00179
00180 vel_sub_ = rosnode_->subscribe(so);
00181 odometry_pub_ = rosnode_->advertise<nav_msgs::Odometry>(odometry_topic_, 1);
00182
00183
00184 callback_queue_thread_ =
00185 boost::thread(boost::bind(&GazeboRosForceBasedMove::QueueThread, this));
00186
00187
00188 update_connection_ =
00189 event::Events::ConnectWorldUpdateBegin(
00190 boost::bind(&GazeboRosForceBasedMove::UpdateChild, this));
00191
00192 }
00193
00194
00195 void GazeboRosForceBasedMove::UpdateChild()
00196 {
00197 boost::mutex::scoped_lock scoped_lock(lock);
00198 math::Pose pose = parent_->GetWorldPose();
00199
00200 math::Vector3 angular_vel = parent_->GetWorldAngularVel();
00201
00202 double error = angular_vel.z - rot_;
00203
00204 link_->AddTorque(math::Vector3(0.0, 0.0, -error * torque_yaw_velocity_p_gain_));
00205
00206 float yaw = pose.rot.GetYaw();
00207
00208 math::Vector3 linear_vel = parent_->GetRelativeLinearVel();
00209
00210 link_->AddRelativeForce(math::Vector3((x_ - linear_vel.x)* force_x_velocity_p_gain_,
00211 (y_ - linear_vel.y)* force_y_velocity_p_gain_,
00212 0.0));
00213
00214
00215
00216
00217
00218
00219
00220 if (odometry_rate_ > 0.0) {
00221 common::Time current_time = parent_->GetWorld()->GetSimTime();
00222 double seconds_since_last_update =
00223 (current_time - last_odom_publish_time_).Double();
00224 if (seconds_since_last_update > (1.0 / odometry_rate_)) {
00225 publishOdometry(seconds_since_last_update);
00226 last_odom_publish_time_ = current_time;
00227 }
00228 }
00229 }
00230
00231
00232 void GazeboRosForceBasedMove::FiniChild() {
00233 alive_ = false;
00234 queue_.clear();
00235 queue_.disable();
00236 rosnode_->shutdown();
00237 callback_queue_thread_.join();
00238 }
00239
00240 void GazeboRosForceBasedMove::cmdVelCallback(
00241 const geometry_msgs::Twist::ConstPtr& cmd_msg)
00242 {
00243 boost::mutex::scoped_lock scoped_lock(lock);
00244 x_ = cmd_msg->linear.x;
00245 y_ = cmd_msg->linear.y;
00246 rot_ = cmd_msg->angular.z;
00247 }
00248
00249 void GazeboRosForceBasedMove::QueueThread()
00250 {
00251 static const double timeout = 0.01;
00252 while (alive_ && rosnode_->ok())
00253 {
00254 queue_.callAvailable(ros::WallDuration(timeout));
00255 }
00256 }
00257
00258 void GazeboRosForceBasedMove::publishOdometry(double step_time)
00259 {
00260
00261 ros::Time current_time = ros::Time::now();
00262 std::string odom_frame = tf::resolve(tf_prefix_, odometry_frame_);
00263 std::string base_footprint_frame =
00264 tf::resolve(tf_prefix_, robot_base_frame_);
00265
00266 math::Vector3 angular_vel = parent_->GetRelativeAngularVel();
00267 math::Vector3 linear_vel = parent_->GetRelativeLinearVel();
00268
00269 odom_transform_= odom_transform_ * this->getTransformForMotion(linear_vel.x, angular_vel.z, step_time);
00270
00271 tf::poseTFToMsg(odom_transform_, odom_.pose.pose);
00272 odom_.twist.twist.angular.z = angular_vel.z;
00273 odom_.twist.twist.linear.x = linear_vel.x;
00274
00275 odom_.header.stamp = current_time;
00276 odom_.header.frame_id = odom_frame;
00277 odom_.child_frame_id = base_footprint_frame;
00278
00279 if (transform_broadcaster_.get()){
00280 transform_broadcaster_->sendTransform(
00281 tf::StampedTransform(odom_transform_, current_time, odom_frame,
00282 base_footprint_frame));
00283 }
00284
00285 odom_.pose.covariance[0] = 0.001;
00286 odom_.pose.covariance[7] = 0.001;
00287 odom_.pose.covariance[14] = 1000000000000.0;
00288 odom_.pose.covariance[21] = 1000000000000.0;
00289 odom_.pose.covariance[28] = 1000000000000.0;
00290
00291 if (std::abs(angular_vel.z) < 0.0001) {
00292 odom_.pose.covariance[35] = 0.01;
00293 }else{
00294 odom_.pose.covariance[35] = 100.0;
00295 }
00296
00297 odom_.twist.covariance[0] = 0.001;
00298 odom_.twist.covariance[7] = 0.001;
00299 odom_.twist.covariance[14] = 0.001;
00300 odom_.twist.covariance[21] = 1000000000000.0;
00301 odom_.twist.covariance[28] = 1000000000000.0;
00302
00303 if (std::abs(angular_vel.z) < 0.0001) {
00304 odom_.twist.covariance[35] = 0.01;
00305 }else{
00306 odom_.twist.covariance[35] = 100.0;
00307 }
00308
00309
00310
00311 odometry_pub_.publish(odom_);
00312 }
00313
00314
00315 tf::Transform GazeboRosForceBasedMove::getTransformForMotion(double linear_vel_x, double angular_vel, double timeSeconds) const
00316 {
00317 tf::Transform tmp;
00318 tmp.setIdentity();
00319
00320
00321 if (std::abs(angular_vel) < 0.0001) {
00322
00323 tmp.setOrigin(tf::Vector3(static_cast<double>(linear_vel_x*timeSeconds), 0.0, 0.0));
00324 } else {
00325
00326 double distChange = linear_vel_x * timeSeconds;
00327 double angleChange = angular_vel * timeSeconds;
00328
00329 double arcRadius = distChange / angleChange;
00330
00331 tmp.setOrigin(tf::Vector3(std::sin(angleChange) * arcRadius,
00332 arcRadius - std::cos(angleChange) * arcRadius,
00333 0.0));
00334 tmp.setRotation(tf::createQuaternionFromYaw(angleChange));
00335 }
00336
00337 return tmp;
00338 }
00339
00340 GZ_REGISTER_MODEL_PLUGIN(GazeboRosForceBasedMove)
00341 }
00342