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 #include <ros/ros.h>
00026 #include <angles/angles.h>
00027
00028 #include <pthread.h>
00029
00030
00031 #include <actionlib/client/simple_action_client.h>
00032 #include <actionlib/server/simple_action_server.h>
00033 #include <move_base_msgs/MoveBaseAction.h>
00034
00035 #include <geometry_msgs/Twist.h>
00036 #include <geometry_msgs/PoseStamped.h>
00037 #include <nav_msgs/Odometry.h>
00038 #include <cob_srvs/SetString.h>
00039
00040 #include <tf/transform_listener.h>
00041
00042
00043
00044 class NodeClass
00045 {
00046 public:
00047
00048 ros::NodeHandle nh_;
00049
00050
00051 ros::Publisher topic_pub_command_;
00052
00053 ros::Subscriber goal_sub_, odometry_sub_;
00054
00055 ros::ServiceServer ss_;
00056
00057
00058 actionlib::SimpleActionServer<move_base_msgs::MoveBaseAction> as_;
00059 move_base_msgs::MoveBaseResult result_;
00060
00061
00062 actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> * action_client_;
00063
00064
00065
00066 NodeClass(std::string name) :
00067 as_(nh_, name, boost::bind(&NodeClass::actionCB, this, _1), false)
00068 {
00069 m_mutex = PTHREAD_MUTEX_INITIALIZER;
00070
00071 ros::NodeHandle private_nh("~");
00072
00073
00074 topic_pub_command_ = nh_.advertise<geometry_msgs::Twist>("cmd_vel", 1);
00075
00076
00077
00078
00079 ros::NodeHandle simple_nh("move_base_linear_simple");
00080 goal_sub_ = simple_nh.subscribe<geometry_msgs::PoseStamped>("goal", 1, boost::bind(&NodeClass::topicCB, this, _1));
00081
00082 action_client_ = new actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction>(nh_, name);
00083
00084
00085 odometry_sub_ = nh_.subscribe<nav_msgs::Odometry>("odom", 1, boost::bind(&NodeClass::odometryCB, this, _1));
00086
00087 ss_ = private_nh.advertiseService("set_global_frame", &NodeClass::serviceCB, this);
00088
00089
00090 if(!private_nh.hasParam("kv")) ROS_WARN("Used default parameter for kv [2.5]");
00091 private_nh.param("kv", kv_, 2.5);
00092
00093 if(!private_nh.hasParam("kp")) ROS_WARN("Used default parameter for kp [3.0]");
00094 private_nh.param("kp", kp_, 3.0);
00095
00096 if(!private_nh.hasParam("virt_mass")) ROS_WARN("Used default parameter for virt_mass [0.5]");
00097 private_nh.param("virt_mass", virt_mass_, 0.5);
00098
00099 if(!private_nh.hasParam("vmax")) ROS_WARN("Used default parameter for vmax [0.3 m/s]");
00100 private_nh.param("vmax", v_max_, 0.3);
00101
00102 if(!private_nh.hasParam("goal_threshold")) ROS_WARN("Used default parameter for goal_threshold [0.03 cm]");
00103 private_nh.param("goal_threshold", goal_threshold_, 0.03);
00104
00105 if(!private_nh.hasParam("speed_threshold")) ROS_WARN("Used default parameter for speed_threshold [0.08 m/s]");
00106 private_nh.param("speed_threshold", speed_threshold_, 0.08);
00107
00108 if(!private_nh.hasParam("kv_rot")) ROS_WARN("Used default parameter for kv_rot [2.0]");
00109 private_nh.param("kv_rot", kv_rot_, 2.0);
00110
00111 if(!private_nh.hasParam("kp_rot")) ROS_WARN("Used default parameter for kp_rot [2.0]");
00112 private_nh.param("kp_rot", kp_rot_, 2.0);
00113
00114 if(!private_nh.hasParam("virt_mass_rot")) ROS_WARN("Used default parameter for virt_mass_rot [0.5]");
00115 private_nh.param("virt_mass_rot", virt_mass_rot_, 0.5);
00116
00117 if(!private_nh.hasParam("vtheta_max")) ROS_WARN("Used default parameter for vtheta_max [0.3 rad/s]");
00118 private_nh.param("vtheta_max", vtheta_max_, 0.3);
00119
00120 if(!private_nh.hasParam("goal_threshold_rot")) ROS_WARN("Used default parameter for goal_threshold_rot [0.08 rad]");
00121 private_nh.param("goal_threshold_rot", goal_threshold_rot_, 0.08);
00122
00123 if(!private_nh.hasParam("speed_threshold_rot")) ROS_WARN("Used default parameter for speed_threshold_rot [0.08 rad/s]");
00124 private_nh.param("speed_threshold_rot", speed_threshold_rot_, 0.08);
00125
00126 if(!private_nh.hasParam("global_frame")) ROS_WARN("Used default parameter for global_frame [/map]");
00127 private_nh.param("global_frame", global_frame_, std::string("map"));
00128
00129 if(!private_nh.hasParam("robot_frame")) ROS_WARN("Used default parameter for robot_frame [/base_link]");
00130 private_nh.param("robot_frame", robot_frame_, std::string("base_link"));
00131
00132 if(!private_nh.hasParam("slow_down_distance")) ROS_WARN("Used default parameter for slow_down_distance [0.5m]");
00133 private_nh.param("slow_down_distance", slow_down_distance_, 0.5);
00134
00135 if(!private_nh.hasParam("goal_abortion_time")) ROS_WARN("Used default parameter for goal_abortion_time [5.0s]");
00136 private_nh.param("goal_abortion_time", goal_abortion_time_, 5.0);
00137
00138 if(!private_nh.hasParam("use_move_action")) ROS_WARN("Used default parameter for use_move_action [true]");
00139 private_nh.param("use_move_action", use_move_action_, true);
00140
00141
00142 zero_pose_.pose.position.x = 0.0;
00143 zero_pose_.pose.position.y = 0.0;
00144 zero_pose_.pose.position.z = 0.0;
00145 zero_pose_.pose.orientation.x = 0.0;
00146 zero_pose_.pose.orientation.y = 0.0;
00147 zero_pose_.pose.orientation.z = 0.0;
00148 zero_pose_.pose.orientation.w = 1.0;
00149 zero_pose_.header.frame_id = robot_frame_;
00150 zero_pose_.header.stamp = ros::Time::now();
00151 robot_pose_global_ = zero_pose_;
00152 robot_pose_global_.header.frame_id = global_frame_;
00153 goal_pose_global_ = robot_pose_global_;
00154
00155 move_ = false;
00156
00157
00158 last_time_ = ros::Time::now().toSec();
00159 vtheta_last_ = 0.0;
00160
00161
00162 as_.start();
00163 }
00164
00165 void topicCB(const geometry_msgs::PoseStamped::ConstPtr& goal)
00166 {
00167
00168 if(!goalValid(*goal))
00169 return;
00170
00171 if(use_move_action_)
00172 {
00173 ROS_INFO("In ROS goal callback, wrapping the PoseStamped in the action message and re-sending to the server.");
00174 move_base_msgs::MoveBaseGoal action_goal;
00175
00176 action_goal.target_pose = transformGoalToMap(*goal);
00177
00178 action_client_->sendGoal(action_goal);
00179 action_client_->stopTrackingGoal();
00180 }
00181 else
00182 {
00183 ROS_DEBUG("In ROS goal callback, using the PoseStamped as error and start control step.");
00184
00185 last_time_moving_ = ros::Time::now().toSec();
00186
00187 getRobotPoseGlobal();
00188
00189 if(last_time_ < 0)
00190 {
00191 vtheta_last_ = 0.0f;
00192 vx_last_ = 0.0f;
00193 vy_last_ = 0.0f;
00194 last_time_ = ros::Time::now().toSec();
00195 }
00196
00197 x_last_ = robot_pose_global_.pose.position.x;
00198 y_last_ = robot_pose_global_.pose.position.y;
00199 theta_last_ = tf::getYaw(robot_pose_global_.pose.orientation);
00200
00201 goal_pose_global_ = transformGoalToMap(*goal);
00202
00203 move_ = true;
00204
00205 }
00206
00207 }
00208
00209 void actionCB(const move_base_msgs::MoveBaseGoalConstPtr &goal)
00210 {
00211 if(!goalValid(goal->target_pose))
00212 {
00213 as_.setAborted(move_base_msgs::MoveBaseResult(), "Aborting because a transformation could not be found");
00214 return;
00215 }
00216
00217 ROS_INFO("In idle mode, new goal accepted");
00218
00219 last_time_moving_ = ros::Time::now().toSec();
00220
00221 getRobotPoseGlobal();
00222 x_last_ = robot_pose_global_.pose.position.x;
00223 y_last_ = robot_pose_global_.pose.position.y;
00224 theta_last_ = tf::getYaw(robot_pose_global_.pose.orientation);
00225 vtheta_last_ = 0.0f;
00226 vx_last_ = 0.0f;
00227 vy_last_ = 0.0f;
00228 last_time_ = ros::Time::now().toSec();
00229
00230 goal_pose_global_ = transformGoalToMap(goal->target_pose);
00231
00232 move_ = true;
00233
00234 ros::Rate loop_rate(100);
00235 while(nh_.ok()) {
00236 loop_rate.sleep();
00237
00238 if(as_.isPreemptRequested()) {
00239 if(as_.isNewGoalAvailable()) {
00240 ROS_INFO("New goal received, updating movement");
00241
00242 move_base_msgs::MoveBaseGoal new_goal = * as_.acceptNewGoal();
00243 goal_pose_global_ = transformGoalToMap(new_goal.target_pose);
00244
00245 last_time_moving_ = ros::Time::now().toSec();
00246 move_ = true;
00247 } else {
00248 ROS_INFO("Preempt requested, aborting movement");
00249
00250 as_.setPreempted();
00251
00252 move_ = false;
00253 stopMovement();
00254 return;
00255 }
00256 }
00257
00258 performControllerStep();
00259
00260 if(finished_) {
00261 as_.setSucceeded(move_base_msgs::MoveBaseResult(), "Goal reached.");
00262 ROS_INFO("Goal reached.");
00263 return;
00264 }
00265
00266 if(!as_.isActive()) {
00267 ROS_INFO("Goal not active anymore. Stop!");
00268 return;
00269 }
00270 }
00271
00272
00273
00274 stopMovement();
00275 as_.setAborted(move_base_msgs::MoveBaseResult(), "Aborting on the goal because the node has been killed");
00276 return;
00277 };
00278
00279
00280 void odometryCB(const nav_msgs::Odometry::ConstPtr &odometry){
00281 geometry_msgs::Vector3Stamped vec_stamped;
00282
00283 vec_stamped.vector = odometry->twist.twist.linear;
00284 vec_stamped.header.frame_id = "base_footprint";
00285 try
00286 {
00287 tf_listener_.waitForTransform(robot_frame_, vec_stamped.header.frame_id, ros::Time(0), ros::Duration(1.0));
00288 tf_listener_.transformVector(robot_frame_, vec_stamped, robot_twist_linear_robot_);
00289 }
00290 catch(tf::TransformException& ex){ROS_ERROR("%s",ex.what());}
00291
00292 vec_stamped.vector = odometry->twist.twist.angular;
00293 vec_stamped.header.frame_id = "base_footprint";
00294 try
00295 {
00296 tf_listener_.waitForTransform(robot_frame_, vec_stamped.header.frame_id, ros::Time(0), ros::Duration(1.0));
00297 tf_listener_.transformVector(robot_frame_, vec_stamped, robot_twist_angular_robot_);
00298 }
00299 catch(tf::TransformException& ex){ROS_ERROR("%s",ex.what());}
00300 }
00301
00302 bool serviceCB(cob_srvs::SetString::Request &req, cob_srvs::SetString::Response &res)
00303 {
00304 global_frame_ = req.data;
00305 ros::NodeHandle private_nh("~");
00306 private_nh.setParam("global_frame", req.data);
00307 ROS_INFO_STREAM("Set global frame to: " << req.data);
00308 res.success = true;
00309 res.message = "";
00310
00311 return true;
00312 }
00313
00314
00315 ~NodeClass()
00316 {
00317 }
00318
00319 void performControllerStep();
00320 bool getUseMoveAction(void);
00321
00322 private:
00323 tf::TransformListener tf_listener_;
00324 std::string global_frame_, robot_frame_;
00325 geometry_msgs::PoseStamped goal_pose_global_;
00326 geometry_msgs::PoseStamped zero_pose_;
00327 geometry_msgs::PoseStamped robot_pose_global_;
00328 geometry_msgs::Vector3Stamped robot_twist_linear_robot_, robot_twist_angular_robot_;
00329
00330 double slow_down_distance_, goal_abortion_time_;
00331
00332 bool finished_, move_;
00333
00334 bool use_move_action_;
00335
00336 pthread_mutex_t m_mutex;
00337
00338
00339 void publishVelocitiesGlobal(double vx, double vy, double theta);
00340 geometry_msgs::PoseStamped transformGoalToMap(geometry_msgs::PoseStamped goal_pose);
00341 geometry_msgs::PoseStamped getRobotPoseGlobal();
00342
00343
00344 double getDistance2d(geometry_msgs::PoseStamped a, geometry_msgs::PoseStamped b);
00345 double getDistance2d(geometry_msgs::Point a, geometry_msgs::Point b);
00346 double getThetaDiffRad(double target, double actual);
00347 double sign(double x);
00348 void stopMovement();
00349 bool notMovingDueToObstacle();
00350 bool goalValid(const geometry_msgs::PoseStamped& goal_pose);
00351
00352
00353 double vx_last_, vy_last_, x_last_, y_last_, theta_last_, vtheta_last_;
00354 double goal_threshold_, speed_threshold_;
00355 double goal_threshold_rot_, speed_threshold_rot_;
00356 double kp_, kv_, virt_mass_;
00357 double kp_rot_, kv_rot_, virt_mass_rot_;
00358 double last_time_;
00359 double v_max_, vtheta_max_;
00360 double last_time_moving_;
00361
00362 };
00363
00364 bool NodeClass::getUseMoveAction(void)
00365 {
00366 return use_move_action_;
00367 }
00368
00369 geometry_msgs::PoseStamped NodeClass::transformGoalToMap(geometry_msgs::PoseStamped goal_pose) {
00370 geometry_msgs::PoseStamped goal_global_;
00371 if(goal_pose.header.frame_id == global_frame_) return goal_pose;
00372 else if(tf_listener_.canTransform(global_frame_, goal_pose.header.frame_id, ros::Time(0), new std::string)) {
00373 tf_listener_.transformPose(global_frame_, ros::Time(0), goal_pose, "base_link", goal_global_);
00374 return goal_global_;
00375 } else {
00376 ROS_WARN("Can't transform goal to global frame %s", global_frame_.c_str());
00377 return robot_pose_global_;
00378 }
00379 }
00380
00381 geometry_msgs::PoseStamped NodeClass::getRobotPoseGlobal() {
00382 try
00383 {
00384 ros::Time now = ros::Time::now();
00385 tf_listener_.waitForTransform(global_frame_, robot_frame_, now, ros::Duration(5.0));
00386 tf_listener_.transformPose(global_frame_, now, zero_pose_, robot_frame_, robot_pose_global_);
00387 }
00388 catch(tf::TransformException& ex){
00389 ROS_WARN("Failed to find robot pose in global frame %s", global_frame_.c_str());
00390 robot_pose_global_ = zero_pose_;
00391 return zero_pose_;
00392 }
00393
00394 return robot_pose_global_;
00395 }
00396
00397 double NodeClass::getDistance2d(geometry_msgs::PoseStamped a, geometry_msgs::PoseStamped b) {
00398 return sqrt( pow(a.pose.position.x - b.pose.position.x,2) + pow(a.pose.position.y - b.pose.position.y,2) );
00399 }
00400
00401 double NodeClass::getDistance2d(geometry_msgs::Point a, geometry_msgs::Point b) {
00402 return sqrt( pow(a.x - b.x,2) + pow(a.y - b.y,2) );
00403 }
00404
00405 double NodeClass::sign(double x) {
00406 if(x >= 0.0f) return 1.0f;
00407 else return -1.0f;
00408 }
00409
00410 double NodeClass::getThetaDiffRad(double target, double actual) {
00411 return angles::shortest_angular_distance(actual, target);
00412 }
00413
00414 void NodeClass::publishVelocitiesGlobal(double vx, double vy, double theta) {
00415
00416 geometry_msgs::Vector3Stamped cmd_global, cmd_robot;
00417 geometry_msgs::Twist msg;
00418
00419 cmd_global.header.frame_id = global_frame_;
00420 cmd_global.header.stamp = ros::Time::now();
00421 cmd_global.vector.x = vx;
00422 cmd_global.vector.y = vy;
00423 try
00424 {
00425 tf_listener_.waitForTransform(robot_frame_, cmd_global.header.frame_id, cmd_global.header.stamp, ros::Duration(1.0));
00426 tf_listener_.transformVector(robot_frame_, cmd_global, cmd_robot);
00427 } catch (tf::TransformException ex){
00428 ROS_ERROR("%s",ex.what());
00429 cmd_robot.vector.x = 0.0f;
00430 cmd_robot.vector.y = 0.0f;
00431 }
00432
00433
00434 try
00435 {
00436 if ( std::isnan(cmd_robot.vector.x) || std::isnan(cmd_robot.vector.y) || std::isnan(theta) )
00437 {
00438 std::string err = "linear_nav: Output velocity contains NaN!";
00439 throw err;
00440 }
00441 if ( ! ( fabs(cmd_robot.vector.x) < 5.0 && fabs(cmd_robot.vector.y) < 5.0 && fabs(theta) < M_PI ) )
00442 {
00443 std::string err = "linear_nav: Output velocity too high";
00444 throw err;
00445 }
00446 }
00447 catch ( std::string err )
00448 {
00449 ROS_ERROR("%s", err.c_str());
00450 topic_pub_command_.publish(geometry_msgs::Twist());
00451 return;
00452 }
00453
00454
00455 msg.linear = cmd_robot.vector;
00456 msg.angular.z = theta;
00457 msg.linear.z = 0.0; msg.angular.x = 0.0; msg.angular.y = 0.0;
00458
00459 topic_pub_command_.publish(msg);
00460 }
00461
00462 void NodeClass::stopMovement() {
00463 publishVelocitiesGlobal(0.0f, 0.0f, 0.0f);
00464 vx_last_ = 0.0f;
00465 vy_last_ = 0.0f;
00466 vtheta_last_ = 0.0f;
00467 }
00468
00469 bool NodeClass::notMovingDueToObstacle() {
00470 if (move_ == true &&
00471 finished_ == false &&
00472 fabs(robot_twist_linear_robot_.vector.x) <= 0.01 &&
00473 fabs(robot_twist_linear_robot_.vector.y) <= 0.01 &&
00474 fabs(robot_twist_angular_robot_.vector.z) <= 0.01 &&
00475 ros::Time::now().toSec() - last_time_moving_ > goal_abortion_time_ )
00476 {
00477 return true;
00478 } else if ( fabs(robot_twist_linear_robot_.vector.x) > 0.01 ||
00479 fabs(robot_twist_linear_robot_.vector.y) > 0.01 ||
00480 fabs(robot_twist_angular_robot_.vector.z) > 0.01 )
00481 {
00482 last_time_moving_ = ros::Time::now().toSec();
00483 }
00484
00485 return false;
00486 }
00487
00488 bool NodeClass::goalValid(const geometry_msgs::PoseStamped& goal_pose)
00489 {
00490 if( goal_pose.pose.orientation.x == 0.0 &&
00491 goal_pose.pose.orientation.y == 0.0 &&
00492 goal_pose.pose.orientation.z == 0.0 &&
00493 goal_pose.pose.orientation.w == 0.0 )
00494 {
00495 ROS_WARN("Goal invalid! Received Quaternion with all values 0.0!");
00496 return false;
00497 }
00498 else if (!tf_listener_.waitForTransform(global_frame_, goal_pose.header.frame_id, goal_pose.header.stamp, ros::Duration(1.0)))
00499 {
00500 ROS_WARN_STREAM("Can not transform goal which is given in /"
00501 << goal_pose.header.frame_id << " into global frame /" << global_frame_);
00502 return false;
00503 }
00504 else if (!tf_listener_.waitForTransform(robot_frame_, goal_pose.header.frame_id, goal_pose.header.stamp, ros::Duration(1.0)))
00505 {
00506 ROS_WARN_STREAM("Can not transform goal which is given in /"
00507 << goal_pose.header.frame_id << " into robot frame /" << robot_frame_);
00508 return false;
00509 }
00510 else
00511 {
00512 return true;
00513 }
00514 }
00515
00516 void NodeClass::performControllerStep() {
00517 pthread_mutex_lock(&m_mutex);
00518
00519 double dt;
00520 double F_x, F_y, F_theta;
00521 double distance_to_goal;
00522 double theta, theta_goal;
00523 double cmd_vx, cmd_vy, cmd_vtheta;
00524 double vx_d, vy_d, vtheta_d, v_factor;
00525 double v_max_goal = v_max_;
00526
00527 if(!move_) {
00528 if(!use_move_action_)
00529 last_time_ = ros::Time::now().toSec();
00530 pthread_mutex_unlock(&m_mutex);
00531 return;
00532 }
00533
00534 getRobotPoseGlobal();
00535
00536 distance_to_goal = getDistance2d(robot_pose_global_, goal_pose_global_);
00537 theta = tf::getYaw(robot_pose_global_.pose.orientation);
00538 theta_goal = tf::getYaw(goal_pose_global_.pose.orientation);
00539
00540
00541 if( distance_to_goal <= goal_threshold_ &&
00542 sqrt(vx_last_ * vx_last_ + vy_last_ * vy_last_) <= speed_threshold_ &&
00543 fabs(getThetaDiffRad(theta_goal, theta)) <= goal_threshold_rot_ &&
00544 fabs(vtheta_last_) <= speed_threshold_rot_ )
00545 {
00546 finished_ = true;
00547 move_ = false;
00548 stopMovement();
00549 if(!use_move_action_)
00550 last_time_ = ros::Time::now().toSec();
00551 pthread_mutex_unlock(&m_mutex);
00552 return;
00553 } else if( notMovingDueToObstacle() == true ) {
00554 finished_ = false;
00555 move_ = false;
00556 stopMovement();
00557 if(use_move_action_)
00558 {
00559 as_.setAborted(move_base_msgs::MoveBaseResult(), "Cancel the goal because an obstacle is blocking the path.");
00560 }
00561 else
00562 {
00563 last_time_ = ros::Time::now().toSec();
00564 }
00565 ROS_INFO("Cancel the goal because an obstacle is blocking the path.");
00566 pthread_mutex_unlock(&m_mutex);
00567 return;
00568 } else finished_ = false;
00569
00570 dt = ros::Time::now().toSec() - last_time_;
00571 last_time_ = ros::Time::now().toSec();
00572
00573
00574 if(distance_to_goal < slow_down_distance_) {
00575
00576 double goal_linear_slope = v_max_ / slow_down_distance_;
00577 v_max_goal = distance_to_goal * goal_linear_slope;
00578
00579 if(v_max_goal > v_max_) v_max_goal = v_max_;
00580 else if(v_max_goal < 0.0f) v_max_goal = 0.0f;
00581 }
00582
00583
00584
00585 vx_d = kp_/kv_ * (goal_pose_global_.pose.position.x - robot_pose_global_.pose.position.x);
00586 vy_d = kp_/kv_ * (goal_pose_global_.pose.position.y - robot_pose_global_.pose.position.y);
00587 v_factor = v_max_goal / sqrt(vy_d*vy_d + vx_d*vx_d);
00588
00589 if(v_factor > 1.0) v_factor = 1.0;
00590
00591 F_x = - kv_ * vx_last_ + v_factor * kp_ * (goal_pose_global_.pose.position.x - robot_pose_global_.pose.position.x);
00592 F_y = - kv_ * vy_last_ + v_factor * kp_ * (goal_pose_global_.pose.position.y - robot_pose_global_.pose.position.y);
00593
00594 cmd_vx = vx_last_ + F_x / virt_mass_ * dt;
00595 cmd_vy = vy_last_ + F_y / virt_mass_ * dt;
00596
00597
00598
00599 vtheta_d = kp_rot_ / kv_rot_ * getThetaDiffRad(theta_goal, theta);
00600 v_factor = fabs(vtheta_max_ / vtheta_d);
00601 if(v_factor > 1.0) v_factor = 1.0;
00602
00603 F_theta = - kv_rot_ * vtheta_last_ + v_factor * kp_rot_ * getThetaDiffRad(theta_goal, theta);
00604 cmd_vtheta = vtheta_last_ + F_theta / virt_mass_rot_ * dt;
00605
00606
00607
00608 x_last_ = robot_pose_global_.pose.position.x;
00609 y_last_ = robot_pose_global_.pose.position.y;
00610 theta_last_ = theta;
00611 vx_last_ = cmd_vx;
00612 vy_last_ = cmd_vy;
00613 vtheta_last_ = cmd_vtheta;
00614
00615 publishVelocitiesGlobal(cmd_vx, cmd_vy, cmd_vtheta);
00616 pthread_mutex_unlock(&m_mutex);
00617 }
00618
00619
00620
00621
00622
00623 int main(int argc, char** argv)
00624 {
00625
00626 ros::init(argc, argv, "cob_linear");
00627
00628
00629 NodeClass nodeClass("move_base_linear");
00630
00631 if(nodeClass.getUseMoveAction())
00632 {
00633 ros::spin();
00634 }
00635 else
00636 {
00637 ros::Rate loop_rate(100);
00638 while(nodeClass.nh_.ok())
00639 {
00640 nodeClass.performControllerStep();
00641 loop_rate.sleep();
00642 ros::spinOnce();
00643 }
00644 }
00645
00646 return 0;
00647 }
00648