00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022 #include "mav_ctrl_interface/ctrl_interface.h"
00023
00024 namespace mav
00025 {
00026
00027 CtrlInterface::CtrlInterface(ros::NodeHandle nh, ros::NodeHandle nh_private):
00028 nh_(nh),
00029 nh_private_(nh_private),
00030 tf_listener_(ros::Duration(10.0)),
00031 costmap_("my_costmap", tf_listener_)
00032 {
00033 ROS_INFO("Starting CtrlInterface");
00034
00035 ros::NodeHandle nh_mav (nh_, "mav");
00036
00037
00038
00039 initializeParams();
00040
00041
00042
00043 ctrl_type_ = mav::PositionCtrl;
00044
00045 des_vel_.linear.x = 0.0;
00046 des_vel_.linear.y = 0.0;
00047 des_vel_.linear.z = 0.0;
00048 des_vel_.angular.x = 0.0;
00049 des_vel_.angular.y = 0.0;
00050 des_vel_.angular.z = 0.0;
00051
00052 tf::Pose identity;
00053 identity.setIdentity();
00054 tf::poseTFToMsg(identity, cur_pose_.pose);
00055 cur_pose_.header.frame_id = fixed_frame_;
00056
00057 cur_goal_ = des_pose_ = cur_pose_;
00058
00059 navfn_.initialize("my_navfn_planner", &costmap_);
00060
00061
00062
00063 cmd_pose_publisher_ = nh_mav.advertise<geometry_msgs::PoseStamped>(
00064 "cmd/pose", 10);
00065 cmd_vel_publisher_ = nh_mav.advertise<geometry_msgs::TwistStamped>(
00066 "cmd/vel", 10);
00067
00068 goal_publisher_= nh_mav.advertise<geometry_msgs::PoseStamped>(
00069 "cmd/goal", 10);
00070 plan_publisher_ = nh_mav.advertise<nav_msgs::Path>(
00071 "plan", 10);
00072 array_publisher_ = nh_mav.advertise<geometry_msgs::PoseArray>(
00073 "plan_array", 10);
00074
00075
00076
00077 cur_pose_subscriber_ = nh_mav.subscribe(
00078 "pose", 10, &CtrlInterface::curPoseCallback, this);
00079 goal2D_subscriber_ = nh_mav.subscribe(
00080 "goal2D_rviz", 10, &CtrlInterface::goal2Dcallback, this);
00081 cmd_joy_vel_subscriber_ = nh_mav.subscribe(
00082 "cmd_joy/vel", 10, &CtrlInterface::cmdJoyVelCallback, this);
00083 cmd_plan_vel_subscriber_ = nh_mav.subscribe(
00084 "cmd_plan/vel", 10, &CtrlInterface::cmdPlanVelCallback, this);
00085
00086
00087
00088 change_des_pose_srv_ = nh_mav.advertiseService(
00089 "changeDesPose", &CtrlInterface::changeDesPose, this);
00090 pos_hold_srv_ = nh_mav.advertiseService(
00091 "positionHold", &CtrlInterface::positionHold, this);
00092 vel_hold_srv_ = nh_mav.advertiseService(
00093 "velocityHold", &CtrlInterface::velocityHold, this);
00094
00095 set_ctrl_type_client_ = nh_mav.serviceClient<mav_srvs::SetCtrlType>(
00096 "setCtrlType");
00097
00098
00099
00100 cmd_timer_ = nh_private_.createTimer(
00101 ros::Duration(0.100), &CtrlInterface::cmdTimerCallback, this);
00102 plan_timer_ = nh_private_.createTimer(
00103 ros::Duration(0.100), &CtrlInterface::planTimerCallback, this);
00104 }
00105
00106 CtrlInterface::~CtrlInterface()
00107 {
00108 ROS_INFO("Destroying QuadSimplePlanner");
00109
00110 }
00111
00112 void CtrlInterface::initializeParams()
00113 {
00114 yaw_turn_tolerance_ = 30.0 * (M_PI / 180.0);
00115 plan_goal_tolerance_ = 0.10;
00116
00117 goal_tf_tolerance_ = 5.0;
00118
00119 if (!nh_private_.getParam("fixed_frame", fixed_frame_))
00120 fixed_frame_ = "/odom";
00121
00122 if (!nh_private_.getParam("direct_pos_ctrl", direct_pos_ctrl_))
00123 direct_pos_ctrl_ = true;
00124
00125 if (!nh_private_.getParam("allow_joy_vel_cmd", allow_joy_vel_cmd_))
00126 allow_joy_vel_cmd_ = true;
00127 if (!nh_private_.getParam("allow_plan_vel_cmd", allow_plan_vel_cmd_))
00128 allow_plan_vel_cmd_ = true;
00129
00130 if (!nh_private_.getParam("wp_dist_max", wp_dist_max_))
00131 wp_dist_max_ = 1.50;
00132 if (!nh_private_.getParam("wp_dist_tolerance", wp_dist_tolerance_))
00133 wp_dist_tolerance_ = 0.10;
00134 if (!nh_private_.getParam("wp_angle_tolerance", wp_angle_tolerance_))
00135 wp_angle_tolerance_ = 10.0 * (M_PI / 180.0);
00136
00137 if (allow_joy_vel_cmd_)
00138 ROS_INFO("Joystick velocity commands are enabled.");
00139 else
00140 ROS_INFO("Joystick velocity commands are disabled.");
00141
00142 if (allow_plan_vel_cmd_)
00143 ROS_INFO("Planner velocity commands are enabled.");
00144 else
00145 ROS_INFO("Planner velocity commands are disabled.");
00146
00147 fixed_frame_ = tf_listener_.resolve(fixed_frame_);
00148 }
00149
00150
00151 void CtrlInterface::planTimerCallback(const ros::TimerEvent& event)
00152 {
00153 boost::mutex::scoped_lock(mutex_);
00154
00155 if (ctrl_type_ == mav::PositionCtrl && !direct_pos_ctrl_)
00156 {
00157 computeWpFromPlan();
00158 }
00159 }
00160
00161 void CtrlInterface::computeWpFromPlan()
00162 {
00163
00164
00165 if (plan_decomposed_.empty()) return;
00166
00167
00168
00169 unsigned int wp_idx;
00170
00171 for (wp_idx = 0; wp_idx < plan_decomposed_.size(); ++wp_idx)
00172 {
00173 const geometry_msgs::PoseStamped& plan_wp = plan_decomposed_[wp_idx];
00174
00175
00176 float dx = plan_wp.pose.position.x - cur_pose_.pose.position.x;
00177 float dy = plan_wp.pose.position.y - cur_pose_.pose.position.y;
00178 float wp_dist = sqrt(dx*dx + dy*dy);
00179
00180
00181 tf::Quaternion cur_q, wp_q;
00182 tf::quaternionMsgToTF(cur_pose_.pose.orientation, cur_q);
00183 tf::quaternionMsgToTF(plan_wp.pose.orientation, wp_q);
00184 float cur_angle = tf::getYaw(cur_q);
00185 float wp_angle = tf::getYaw(wp_q);
00186 float wp_angle_diff = wp_angle - cur_angle;
00187 if (wp_angle_diff >= M_PI) wp_angle_diff -= 2.0 * M_PI;
00188 else if (wp_angle_diff < -M_PI) wp_angle_diff += 2.0 * M_PI;
00189
00190
00191 if (wp_dist > wp_dist_tolerance_ ||
00192 std::abs(wp_angle_diff) > wp_angle_tolerance_)
00193 {
00194
00195
00196 des_pose_.pose.position.x = plan_wp.pose.position.x;
00197 des_pose_.pose.position.y = plan_wp.pose.position.y;
00198 des_pose_.pose.orientation = plan_wp.pose.orientation;
00199
00200 break;
00201 }
00202 }
00203
00204
00205 plan_decomposed_.erase(plan_decomposed_.begin(),
00206 plan_decomposed_.begin() + wp_idx);
00207
00208 }
00209
00210 void CtrlInterface::cmdTimerCallback(const ros::TimerEvent& event)
00211 {
00212 boost::mutex::scoped_lock(mutex_);
00213
00214 if (ctrl_type_ == mav::PositionCtrl)
00215 {
00216 publishCmdPose();
00217 }
00218 }
00219
00220 void CtrlInterface::cmdJoyVelCallback(const geometry_msgs::TwistStamped::ConstPtr twist_msg)
00221 {
00222 boost::mutex::scoped_lock(mutex_);
00223
00224 if (!allow_joy_vel_cmd_)
00225 {
00226 ROS_WARN("Received joystick velocity command, but allow_joy_vel_cmd is set to false");
00227 return;
00228 }
00229
00230 if (ctrl_type_ != mav::VelocityCtrl)
00231 {
00232 ROS_WARN("Received joystick velocity command, but ctrl_type is not VELOCITY");
00233 return;
00234 }
00235
00236
00237
00238 ROS_INFO("Executing velocity command from joystick.");
00239
00240 des_vel_ = twist_msg->twist;
00241
00242 publishCmdVel();
00243 }
00244
00245 void CtrlInterface::cmdPlanVelCallback(const geometry_msgs::Twist::ConstPtr twist_msg)
00246 {
00247 boost::mutex::scoped_lock(mutex_);
00248
00249 if (!allow_plan_vel_cmd_)
00250 {
00251 ROS_WARN("Received planner velocity command, but allow_plan_vel_cmd is set to false");
00252 return;
00253 }
00254
00255 if (ctrl_type_ != mav::VelocityCtrl)
00256 {
00257 ROS_WARN("Received joystick velocity command, but ctrl_type is not VELOCITY");
00258 return;
00259 }
00260
00261
00262
00263 ROS_INFO("Executing velocity command from planner.");
00264
00265 des_vel_ = *twist_msg;
00266
00267 publishCmdVel();
00268 }
00269
00270 void CtrlInterface::curPoseCallback(const geometry_msgs::PoseStamped::ConstPtr pose_msg)
00271 {
00272 boost::mutex::scoped_lock(mutex_);
00273
00274 std::string frame = tf_listener_.resolve(pose_msg->header.frame_id);
00275
00276 if (frame != fixed_frame_)
00277 {
00278 ROS_INFO_ONCE("Pose message does not match the fixed frame (%s, %s), transforming",
00279 frame.c_str(), fixed_frame_.c_str());
00280
00281 try
00282 {
00283 tf_listener_.waitForTransform(
00284 fixed_frame_, pose_msg->header.frame_id, pose_msg->header.stamp, ros::Duration(1.0));
00285
00286 tf_listener_.transformPose(fixed_frame_, *pose_msg, cur_pose_);
00287 }
00288 catch (tf::TransformException ex)
00289 {
00290 ROS_WARN("Could not transform goal pose %s", ex.what());
00291 return;
00292 }
00293 }
00294 else
00295 {
00296 cur_pose_ = *pose_msg;
00297 }
00298 }
00299
00300 void CtrlInterface::goal2Dcallback(const geometry_msgs::PoseStamped::ConstPtr goal_msg)
00301 {
00302 if (ctrl_type_ == mav::PositionCtrl)
00303 {
00304
00305 std::string frame = tf_listener_.resolve(goal_msg->header.frame_id);
00306 geometry_msgs::PoseStamped goal_fixed_frame;
00307
00308 if (frame != fixed_frame_)
00309 {
00310 ROS_INFO_ONCE("Goal messages does not match the fixed frame (%s, %s), transforming",
00311 frame.c_str(), fixed_frame_.c_str());
00312
00313 try
00314 {
00315 tf_listener_.waitForTransform(
00316 fixed_frame_, goal_msg->header.frame_id, goal_msg->header.stamp, ros::Duration(goal_tf_tolerance_));
00317
00318 tf_listener_.transformPose(fixed_frame_, *goal_msg, goal_fixed_frame);
00319 }
00320 catch (tf::TransformException ex)
00321 {
00322 ROS_WARN("Could not transform goal pose %s", ex.what());
00323 return;
00324 }
00325 }
00326 else
00327 {
00328 goal_fixed_frame = *goal_msg;
00329 }
00330
00331 boost::mutex::scoped_lock(mutex_);
00332
00333 if (direct_pos_ctrl_)
00334 {
00335
00336 ROS_INFO("Goal(2d) received. Executing direct poistion control to goal.");
00337
00338
00339 des_pose_.pose.position.x = goal_fixed_frame.pose.position.x;
00340 des_pose_.pose.position.y = goal_fixed_frame.pose.position.y;
00341
00342 double des_yaw = tf::getYaw(goal_fixed_frame.pose.orientation);
00343 tf::Quaternion des_q = tf::createQuaternionFromYaw(des_yaw);
00344 tf::quaternionTFToMsg(des_q, des_pose_.pose.orientation);
00345
00346 publishCmdPose();
00347 }
00348 else
00349 {
00350
00351 ROS_INFO("Goal(2d) received. Will use for path planning");
00352
00353
00354 cur_goal_.pose.position.x = goal_fixed_frame.pose.position.x;
00355 cur_goal_.pose.position.y = goal_fixed_frame.pose.position.y;
00356
00357 double goal_yaw = tf::getYaw(goal_fixed_frame.pose.orientation);
00358 tf::Quaternion goal_q = tf::createQuaternionFromYaw(goal_yaw);
00359 tf::quaternionTFToMsg(goal_q, cur_goal_.pose.orientation);
00360
00361 computePlan();
00362
00363
00364 publishPlans();
00365 }
00366 }
00367 else if (ctrl_type_ == mav::VelocityCtrl)
00368 {
00369 ROS_WARN("Received Goal(2d) while in velocity node, ignoring.");
00370 }
00371 else
00372 {
00373 ROS_WARN("Unknown control mode");
00374 }
00375 }
00376
00377 bool CtrlInterface::positionHold(mav_srvs::PositionHold::Request &req,
00378 mav_srvs::PositionHold::Response &res)
00379 {
00380 ROS_INFO("Position hold request received");
00381
00382 boost::mutex::scoped_lock(mutex_);
00383
00384
00385 des_pose_ = cur_pose_;
00386
00387
00388 cur_goal_ = cur_pose_;
00389 plan_.clear();
00390 plan_decomposed_.clear();
00391
00392 publishPlans();
00393 publishCmdPose();
00394
00395
00396 ROS_INFO("Setting ctrl mode to POSITION control");
00397 ctrl_type_ = mav::PositionCtrl;
00398
00399
00400 mav_srvs::SetCtrlType set_ctrl_type_srv;
00401 set_ctrl_type_srv.request.ctrl_type = ctrl_type_;
00402 return set_ctrl_type_client_.call(set_ctrl_type_srv);
00403 }
00404
00405 void CtrlInterface::publishPlans()
00406 {
00407
00408
00409 goal_publisher_.publish(cur_goal_);
00410
00411
00412
00413 navfn_.publishPlan(plan_, 1.0, 0.0, 1.0, 1.0);
00414
00415
00416
00417 nav_msgs::Path path_msg;
00418 path_msg.header.frame_id = cur_goal_.header.frame_id;
00419 path_msg.poses = plan_decomposed_;
00420
00421 plan_publisher_.publish(path_msg);
00422
00423
00424
00425 geometry_msgs::PoseArray array_msg;
00426 array_msg.header = path_msg.header;
00427
00428 for (unsigned int i = 0; i < path_msg.poses.size(); ++i)
00429 array_msg.poses.push_back(path_msg.poses[i].pose);
00430
00431 array_publisher_.publish(array_msg);
00432
00433
00434
00435
00436
00437
00438
00439
00440
00441
00442
00443
00444
00445 }
00446
00447 bool CtrlInterface::velocityHold(mav_srvs::VelocityHold::Request &req,
00448 mav_srvs::VelocityHold::Response &res)
00449 {
00450 ROS_INFO("Velocity hold request received");
00451
00452 boost::mutex::scoped_lock(mutex_);
00453
00454
00455 des_vel_.linear.x = 0.0;
00456 des_vel_.linear.y = 0.0;
00457 des_vel_.linear.z = 0.0;
00458 des_vel_.angular.x = 0.0;
00459 des_vel_.angular.y = 0.0;
00460 des_vel_.angular.z = 0.0;
00461 publishCmdVel();
00462
00463
00464 ROS_INFO("Setting ctrl type to VELOCITY control");
00465 ctrl_type_ = mav::VelocityCtrl;
00466
00467
00468 mav_srvs::SetCtrlType set_ctrl_type_srv;
00469 set_ctrl_type_srv.request.ctrl_type = ctrl_type_;
00470 return set_ctrl_type_client_.call(set_ctrl_type_srv);
00471 }
00472
00473 bool CtrlInterface::changeDesPose(mav_srvs::ChangeDesPose::Request &req,
00474 mav_srvs::ChangeDesPose::Response &res)
00475 {
00476 ROS_INFO("ChangeDesPose request received");
00477
00478 boost::mutex::scoped_lock(mutex_);
00479
00480 publishCmdPose();
00481
00482 des_pose_.pose.position.x += req.delta_x;
00483 des_pose_.pose.position.y += req.delta_y;
00484 des_pose_.pose.position.z += req.delta_z;
00485
00486 publishCmdPose();
00487
00488 return true;
00489 }
00490
00491 void CtrlInterface::publishCmdPose()
00492 {
00493 geometry_msgs::PoseStamped cmd_pose;
00494
00495 cmd_pose = des_pose_;
00496 cmd_pose.header.stamp = ros::Time::now();
00497
00498 cmd_pose_publisher_.publish(cmd_pose);
00499 }
00500
00501 void CtrlInterface::publishCmdVel()
00502 {
00503 geometry_msgs::TwistStamped cmd_vel;
00504
00505 cmd_vel.header.stamp = ros::Time::now();
00506 cmd_vel.header.frame_id = fixed_frame_;
00507 cmd_vel.twist = des_vel_;
00508
00509 cmd_vel_publisher_.publish(cmd_vel);
00510 }
00511
00512 void CtrlInterface::computePlan()
00513 {
00514
00515
00516 plan_.clear();
00517 plan_decomposed_.clear();
00518
00519
00520
00521 ros::Time start_time_plan = ros::Time::now();
00522 bool result_plan = navfn_.makePlan(cur_pose_, cur_goal_, plan_goal_tolerance_, plan_);
00523 ros::Time end_time_plan = ros::Time::now();
00524
00525 if (result_plan)
00526 {
00527 ROS_INFO("Plan computed in %.1f ms", (end_time_plan - start_time_plan).toSec() * 1000.0);
00528 }
00529 else
00530 {
00531
00532 ROS_WARN("Plan could not be computed!");
00533 return;
00534 }
00535
00536
00537
00538 ros::Time start_time_dec = ros::Time::now();
00539 bool result_dec = decomposePlan(plan_, plan_decomposed_);
00540 ros::Time end_time_dec = ros::Time::now();
00541
00542 if (result_dec)
00543 {
00544 ROS_INFO("Plan decomposed in %.1f ms", (end_time_dec - start_time_dec).toSec() * 1000.0);
00545 }
00546 else
00547 {
00548
00549 ROS_WARN("Plan could not be decomposed!");
00550 return;
00551 }
00552 }
00553
00554 bool CtrlInterface::decomposePlan(
00555 const std::vector<geometry_msgs::PoseStamped>& plan_in,
00556 std::vector<geometry_msgs::PoseStamped>& plan_out)
00557 {
00558 plan_out.clear();
00559
00560 int size = plan_in.size();
00561
00562 int start = 0;
00563 int end = size - 1;
00564
00565 costmap_2d::Costmap2D costmap;
00566 costmap_.getCostmapCopy(costmap);
00567
00568 bool result = decomposePlan(costmap, plan_in, plan_out, start, end);
00569
00570 if (!result) return false;
00571
00572
00573
00574
00575 geometry_msgs::PoseStamped& s_p = plan_out[0];
00576 geometry_msgs::PoseStamped& e_p = plan_out[plan_out.size() - 1];
00577
00578 s_p.pose.orientation = cur_pose_.pose.orientation;
00579 e_p.pose.orientation = cur_goal_.pose.orientation;
00580
00581 plan_out.push_back(plan_out[plan_out.size() - 1]);
00582
00583
00584 for (unsigned int i = 0; i < plan_out.size() - 2; ++i)
00585 {
00586 geometry_msgs::PoseStamped& a = plan_out[i];
00587 geometry_msgs::PoseStamped& b = plan_out[i+1];
00588
00589 float dx = b.pose.position.x - a.pose.position.x;
00590 float dy = b.pose.position.y - a.pose.position.y;
00591
00592 float angle = atan2(dy, dx);
00593
00594 tf::Quaternion b_q = tf::createQuaternionFromYaw(angle);
00595 tf::quaternionTFToMsg(b_q, b.pose.orientation);
00596 }
00597
00598
00599
00600 std::vector<geometry_msgs::PoseStamped> plan_out_f;
00601 for (unsigned int i = 0; i < plan_out.size() - 1; ++i)
00602 {
00603 geometry_msgs::PoseStamped& a = plan_out[i];
00604 geometry_msgs::PoseStamped& b = plan_out[i+1];
00605
00606 tf::Quaternion q_a, q_b;
00607
00608 tf::quaternionMsgToTF(a.pose.orientation, q_a);
00609 tf::quaternionMsgToTF(b.pose.orientation, q_b);
00610
00611 float yaw_a = tf::getYaw(q_a);
00612 float yaw_b = tf::getYaw(q_b);
00613
00614 float d_angle = (yaw_b - yaw_a);
00615 if (d_angle >= M_PI) d_angle -= 2.0 * M_PI;
00616 else if (d_angle < -M_PI) d_angle += 2.0 * M_PI;
00617
00618 plan_out_f.push_back(a);
00619
00620 if (std::abs(d_angle) > yaw_turn_tolerance_)
00621 {
00622 geometry_msgs::PoseStamped m = a;
00623 m.pose.orientation = b.pose.orientation;
00624 plan_out_f.push_back(m);
00625 }
00626 }
00627
00628 plan_out = plan_out_f;
00629
00630 return true;
00631 }
00632
00633 bool CtrlInterface::decomposePlan(
00634 const costmap_2d::Costmap2D& costmap,
00635 const std::vector<geometry_msgs::PoseStamped>& plan_in,
00636 std::vector<geometry_msgs::PoseStamped>& plan_out,
00637 int start, int end)
00638 {
00639
00640
00641 float resolution = 0.05;
00642 int cost_threshold = 100;
00643
00644
00645
00646 if (end - start == 1)
00647 {
00648 plan_out.resize(2);
00649 plan_out[0] = plan_in[start];
00650 plan_out[1] = plan_in[end];
00651 return true;
00652 }
00653
00654 if (start == end)
00655 {
00656 printf("ERROR\n");
00657 return false;
00658 }
00659
00660 const geometry_msgs::Point& p_start = plan_in[start].pose.position;
00661 const geometry_msgs::Point& p_end = plan_in[end].pose.position;
00662
00663 double dx = p_end.x - p_start.x;
00664 double dy = p_end.y - p_start.y;
00665 double L = sqrt(dx*dx + dy*dy);
00666
00667
00668 if (L < wp_dist_max_)
00669 {
00670 dx /= L;
00671 dy /= L;
00672
00673 dx *= resolution;
00674 dy *= resolution;
00675
00676 double l = 0.0;
00677 double x = p_start.x;
00678 double y = p_start.y;
00679
00680 bool obstacle_found = false;
00681
00682 while(l < L)
00683 {
00684 unsigned int mx, my;
00685 costmap.worldToMap(x, y, mx, my);
00686 int cost = costmap.getCost(mx, my);
00687
00688 if (cost > cost_threshold)
00689 {
00690 obstacle_found = true;
00691 break;
00692 }
00693 x += dx;
00694 y += dy;
00695 l += resolution;
00696 }
00697
00698 if (!obstacle_found)
00699 {
00700
00701 plan_out.resize(2);
00702 plan_out[0] = plan_in[start];
00703 plan_out[1] = plan_in[end];
00704 return true;
00705 }
00706 }
00707
00708
00709
00710
00711 int mid = (start + end) / 2;
00712
00713 std::vector<geometry_msgs::PoseStamped> plan_l;
00714 std::vector<geometry_msgs::PoseStamped> plan_r;
00715
00716 bool left_result = decomposePlan(costmap, plan_in, plan_l, start, mid);
00717 bool right_result = decomposePlan(costmap, plan_in, plan_r, mid, end);
00718
00719 if (!left_result || !right_result) return false;
00720
00721 plan_out.reserve(plan_l.size() + plan_r.size() - 1);
00722 plan_out.insert(plan_out.begin(), plan_l.begin(), plan_l.end());
00723 plan_out.insert(plan_out.end(), plan_r.begin() + 1, plan_r.end());
00724
00725 return true;
00726 }
00727
00728
00729 }