diff --git a/waypoint_nav/param/waypoints.yaml b/waypoint_nav/param/waypoints.yaml index 718999c..92f70d6 100644 --- a/waypoint_nav/param/waypoints.yaml +++ b/waypoint_nav/param/waypoints.yaml @@ -1,10 +1,10 @@ waypoints: -- point: {x: -0.996902, y: -0.499032, z: 0.0, vel: 1.0, rad: 0.5} -- point: {x: 0.45187, y: -0.576395, z: 0.0, vel: 0.75, rad: 0.3} -- point: {x: 0.66645, y: 0.504773, z: 0.0, vel: 0.5, rad: 0.3} -- point: {x: 0.515219, y: 2.015219, z: 0.0, vel: 0.8, rad: 0.8} +- point: {x: -0.45, y: -0.499032, z: 0.0, vel: 1.0, rad: 0.3, stop: false} +- point: {x: 0.50, y: -0.576395, z: 0.0, vel: 0.5, rad: 0.1, stop: true} +- point: {x: 0.50, y: 0.80, z: 0.0, vel: 1.0, rad: 0.3, stop: false} +- point: {x: 0.515219, y: 1.8, z: 0.0, vel: 0.3, rad: 0.1, stop: true} finish_pose: header: {seq: 0.0, stamp: 1620367430.3803937, frame_id: base_link} pose: - position: {x: -0.550981, y: 2.010154, z: 0.0} + position: {x: -0.550981, y: 1.8, z: 0.0} orientation: {x: 0.0, y: 0.0, z: 0.99749, w: 0.07074} \ No newline at end of file diff --git a/waypoint_nav/rviz_cfg/nav.rviz b/waypoint_nav/rviz_cfg/nav.rviz index cc1fea4..2d1d5c8 100644 --- a/waypoint_nav/rviz_cfg/nav.rviz +++ b/waypoint_nav/rviz_cfg/nav.rviz @@ -5,7 +5,7 @@ Property Tree Widget: Expanded: ~ Splitter Ratio: 0.5 - Tree Height: 595 + Tree Height: 532 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -148,7 +148,7 @@ Enabled: true Head Length: 0.07000000029802322 Head Radius: 0.029999999329447746 - Name: PoseArray + Name: Particles Queue Size: 10 Shaft Length: 0.23000000417232513 Shaft Radius: 0.009999999776482582 @@ -157,6 +157,15 @@ Unreliable: false Value: true - Alpha: 1 + Class: rviz/Polygon + Color: 25; 255; 0 + Enabled: true + Name: Footprint + Queue Size: 10 + Topic: /move_base/global_costmap/obstacle_layer_footprint/footprint_stamped + Unreliable: false + Value: true + - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 25; 255; 0 @@ -180,30 +189,12 @@ Topic: /move_base/NavfnROS/plan Unreliable: false Value: true - - Alpha: 1 - Class: rviz/Polygon - Color: 25; 255; 0 - Enabled: true - Name: Polygon - Queue Size: 10 - Topic: /move_base/global_costmap/obstacle_layer_footprint/footprint_stamped - Unreliable: false - Value: true - - Alpha: 1 - Class: rviz/Polygon - Color: 25; 255; 0 - Enabled: true - Name: Polygon - Queue Size: 10 - Topic: /move_base/local_costmap/obstacle_layer_footprint/footprint_stamped - Unreliable: false - Value: true - Alpha: 0.699999988079071 Class: rviz/Map - Color Scheme: map - Draw Behind: false + Color Scheme: costmap + Draw Behind: true Enabled: true - Name: Map + Name: GlobalCostmap Topic: /move_base/global_costmap/costmap Unreliable: false Use Timestamp: false @@ -211,41 +202,14 @@ - Alpha: 0.699999988079071 Class: rviz/Map Color Scheme: costmap - Draw Behind: true + Draw Behind: false Enabled: true - Name: Map + Name: LocalCostmap Topic: /move_base/local_costmap/costmap Unreliable: false Use Timestamp: false Value: true - Alpha: 1 - Axes Length: 1 - Axes Radius: 0.10000000149011612 - Class: rviz/Pose - Color: 0; 0; 127 - Enabled: true - Head Length: 0.30000001192092896 - Head Radius: 0.10000000149011612 - Name: Pose - Queue Size: 10 - Shaft Length: 1 - Shaft Radius: 0.05000000074505806 - Shape: Arrow - Topic: /move_base_simple/goal - Unreliable: false - Value: true - - Alpha: 1 - Class: rviz/PointStamped - Color: 204; 41; 204 - Enabled: true - History Length: 10 - Name: PointStamped - Queue Size: 10 - Radius: 0.20000000298023224 - Topic: /target_point - Unreliable: false - Value: true - - Alpha: 1 Arrow Length: 0.30000001192092896 Axes Length: 0.30000001192092896 Axes Radius: 0.009999999776482582 @@ -254,7 +218,7 @@ Enabled: true Head Length: 0.07000000029802322 Head Radius: 0.029999999329447746 - Name: PoseArray + Name: Waypoints Queue Size: 10 Shaft Length: 0.23000000417232513 Shaft Radius: 0.009999999776482582 @@ -263,16 +227,43 @@ Unreliable: false Value: true - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Class: rviz/Pose + Color: 0; 0; 127 + Enabled: false + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Name: Pose + Queue Size: 10 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Shape: Arrow + Topic: /move_base_simple/goal + Unreliable: false + Value: false + - Alpha: 1 + Class: rviz/PointStamped + Color: 204; 41; 204 + Enabled: false + History Length: 10 + Name: PointStamped + Queue Size: 10 + Radius: 0.20000000298023224 + Topic: /target_point + Unreliable: false + Value: false + - Alpha: 1 Class: rviz/PointStamped Color: 239; 41; 41 - Enabled: true + Enabled: false History Length: 1 Name: PointStamped Queue Size: 10 Radius: 0.4000000059604645 Topic: /filtered_target_point Unreliable: false - Value: true + Value: false Enabled: true Global Options: Background Color: 48; 48; 48 @@ -324,10 +315,10 @@ Window Geometry: Displays: collapsed: false - Height: 847 + Height: 778 Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 000000ff00000000fd000000040000000000000193000002f4fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003e00000290000000cb00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000002200530074006100740065005400720069006700670065007200500061006e0065006c01000002d40000005e0000004100ffffff000000010000010f000002b3fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003e000002b3000000a500fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005e00000003bfc0100000002fb0000000800540069006d00650000000000000005e00000046900fffffffb0000000800540069006d0065010000000000000450000000000000000000000447000002f400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd000000040000000000000193000002affc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003e00000251000000cb00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000002200530074006100740065005400720069006700670065007200500061006e0065006c0100000295000000580000004100ffffff000000010000010f000002b3fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003e000002b3000000a500fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005e00000003bfc0100000002fb0000000800540069006d00650000000000000005e00000046900fffffffb0000000800540069006d0065010000000000000450000000000000000000000376000002af00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false StateTriggerPanel: @@ -338,6 +329,6 @@ collapsed: false Views: collapsed: true - Width: 1504 - X: 24 - Y: -19 + Width: 1295 + X: -1 + Y: -17 diff --git a/waypoint_nav/src/velocity_controller.cpp b/waypoint_nav/src/velocity_controller.cpp index fe4ce2d..d064beb 100644 --- a/waypoint_nav/src/velocity_controller.cpp +++ b/waypoint_nav/src/velocity_controller.cpp @@ -24,13 +24,11 @@ cmd_vel_sub_ = nh.subscribe("/cmd_vel_in", 10, &VelocityController::cmdVelCallback, this); cmd_vel_pub_ = nh.advertise("/cmd_vel_out", 10); - std::string planner = ""; - std::string param_max, param_min; + std::string planner = "base_local_planner/TrajectoryPlannerROS"; + std::string param_max = "/move_base/TrajectoryPlannerROS/max_vel_x"; + std::string param_min = "/move_base/TrajectoryPlannerROS/min_vel_x"; nh.param("/move_base/base_local_planner", planner, planner); - if (planner == "base_local_planner/TrajectoryPlannerROS") { - param_max = "/move_base/TrajectoryPlannerROS/max_vel_x"; - param_min = "/move_base/TrajectoryPlannerROS/min_vel_x"; - } else if (planner == "dwa_local_planner/DWAPlannerROS") { + if (planner == "dwa_local_planner/DWAPlannerROS") { param_max = "/move_base/DWAPlannerROS/max_vel_trans"; param_min = "/move_base/DWAPlannerROS/min_vel_trans"; } @@ -40,6 +38,7 @@ } + void maxVelCallback(const std_msgs::Float32 &msg) { current_max_vel_ = standard_vel_ * msg.data; @@ -48,6 +47,7 @@ } + void cmdVelCallback(const geometry_msgs::Twist &msg) { pub_msg_.linear.x = std::min(float(msg.linear.x), current_max_vel_); diff --git a/waypoint_nav/src/waypoint_nav.cpp b/waypoint_nav/src/waypoint_nav.cpp index afa6b81..da9bd38 100644 --- a/waypoint_nav/src/waypoint_nav.cpp +++ b/waypoint_nav/src/waypoint_nav.cpp @@ -24,7 +24,6 @@ #include #include #include -#include #ifdef NEW_YAMLCPP template @@ -44,6 +43,9 @@ class WaypointsNavigation{ public: + /* + ++++++++++ Constructer ++++++++++ + */ WaypointsNavigation() : has_activate_(false), move_base_action_("move_base", true), @@ -64,12 +66,11 @@ rate_ = ros::Rate(max_update_rate); std::string filename = ""; private_nh.param("filename", filename, filename); - if(filename != "") { + if (filename != "") { ROS_INFO_STREAM("Read waypoints data from " << filename); - if(!readFile(filename)) { + if (!readFile(filename)) { ROS_ERROR("Failed loading waypoints file"); } else { - last_waypoint_ = waypoints_.poses.end()-2; finish_pose_ = waypoints_.poses.end()-1; computeWpOrientation(); } @@ -81,12 +82,7 @@ ros::NodeHandle nh; start_server_ = nh.advertiseService("start_wp_nav", &WaypointsNavigation::startNavigationCallback, this); - pause_server_ = nh.advertiseService("pause_wp_nav", &WaypointsNavigation::pauseNavigationCallback,this); - unpause_server_ = nh.advertiseService("unpause_wp_nav", &WaypointsNavigation::unpauseNavigationCallback,this); - stop_server_ = nh.advertiseService("stop_wp_nav", &WaypointsNavigation::pauseNavigationCallback,this); - // suspend_server_ = nh.advertiseService("suspend_wp_pose", &WaypointsNavigation::suspendPoseCallback, this); - // resume_server_ = nh.advertiseService("resume_wp_pose", &WaypointsNavigation::resumePoseCallback, this); - search_server_ = nh.advertiseService("near_wp_nav",&WaypointsNavigation::searchPoseCallback, this); + resume_server_ = nh.advertiseService("resume_nav", &WaypointsNavigation::resumeNavigationCallback, this); cmd_vel_sub_ = nh.subscribe("/cmd_vel", 1, &WaypointsNavigation::cmdVelCallback, this); wp_pub_ = nh.advertise("waypoints", 10); max_vel_pub_ = nh.advertise("/max_vel", 1); @@ -95,15 +91,17 @@ + /* + ++++++++++ Read waypoints file ++++++++++ + */ bool readFile(const std::string &filename) { waypoints_.poses.clear(); try { std::ifstream ifs(filename.c_str(), std::ifstream::in); - if(ifs.good() == false) return false; + if (ifs.good() == false) return false; YAML::Node node; - #ifdef NEW_YAMLCPP node = YAML::Load(ifs); #else @@ -111,43 +109,46 @@ parser.GetNextDocument(node); #endif + // read waypoints #ifdef NEW_YAMLCPP const YAML::Node &wp_node_tmp = node["waypoints"]; const YAML::Node *wp_node = wp_node_tmp ? &wp_node_tmp : NULL; #else const YAML::Node *wp_node = node.FindValue("waypoints"); #endif - geometry_msgs::Pose pose; Waypoint point; float vel, rad; - if(wp_node != NULL) { + bool stop; + if (wp_node != NULL) { for(int i=0; i < wp_node->size(); i++) { (*wp_node)[i]["point"]["x"] >> pose.position.x; (*wp_node)[i]["point"]["y"] >> pose.position.y; (*wp_node)[i]["point"]["z"] >> pose.position.z; (*wp_node)[i]["point"]["vel"] >> vel; (*wp_node)[i]["point"]["rad"] >> rad; + (*wp_node)[i]["point"]["stop"] >> stop; waypoints_.poses.push_back(pose); point.set_x(pose.position.x); point.set_y(pose.position.y); point.set_z(pose.position.z); point.set_vel(vel); point.set_rad(rad); + point.set_stop(stop); waypoint_list_.push_back(point); } } else { return false; } + // read finish_pose #ifdef NEW_YAMLCPP const YAML::Node &fp_node_tmp = node["finish_pose"]; const YAML::Node *fp_node = fp_node_tmp ? &fp_node_tmp : NULL; #else const YAML::Node *fp_node = node.FindValue("finish_pose"); #endif - - if(fp_node != NULL) { + if (fp_node != NULL) { (*fp_node)["pose"]["position"]["x"] >> pose.position.x; (*fp_node)["pose"]["position"]["y"] >> pose.position.y; (*fp_node)["pose"]["position"]["z"] >> pose.position.z; @@ -159,8 +160,6 @@ point.set_x(pose.position.x); point.set_y(pose.position.y); point.set_z(pose.position.z); - point.set_vel(1.0); - point.set_rad(0.3); waypoint_list_.push_back(point); } else { return false; @@ -178,6 +177,9 @@ + /* + ++++++++++ Compute target orientation for each waypoint ++++++++++ + */ void computeWpOrientation() { for(std::vector::iterator it = waypoints_.poses.begin(); it != finish_pose_; it++) { @@ -190,9 +192,12 @@ + /* + ++++++++++ Callback function for "StartWaypointsNavigation" button on rviz ++++++++++ + */ bool startNavigationCallback(std_srvs::Trigger::Request &request, std_srvs::Trigger::Response &response) { - if(has_activate_) { + if (has_activate_) { response.success = false; return false; } @@ -210,132 +215,38 @@ - bool pauseNavigationCallback(std_srvs::Trigger::Request &request, std_srvs::Trigger::Response &response) + /* + ++++++++++ Callback function for "ResumeWaypointsNavigation" button on rviz ++++++++++ + */ + bool resumeNavigationCallback(std_srvs::Trigger::Request &request, std_srvs::Trigger::Response &response) { - if(!has_activate_) { - ROS_WARN("Navigation is already pause"); - response.success = false; - return false; - } - - has_activate_ = false; - response.success = true; - return true; - } - - - - bool unpauseNavigationCallback(std_srvs::Trigger::Request &request, std_srvs::Trigger::Response &response) - { - if(has_activate_) { + if (has_activate_) { ROS_WARN("Navigation is already active"); response.success = false; + } else { + ROS_INFO("Navigation has resumed"); + has_activate_ = true; + response.success = true; } - has_activate_ = true; - response.success = true; return true; } - void stopNavigationCallback(std_srvs::Empty::Request &request, std_srvs::Empty::Response &response) - { - has_activate_ = false; - move_base_action_.cancelAllGoals(); - } - - - - // bool resumePoseCallback(fulanghua_srvs::Pose::Request &request, fulanghua_srvs::Pose::Response &response) - // { - // if(has_activate_) { - // response.status = false; - // return false; - // } - // std_srvs::Empty empty; - // clear_costmaps_srv_.call(empty); - // //move_base_action_.cancelAllGoals(); - - // ///< @todo calculating metric with request orientation - // double min_dist = std::numeric_limits::max(); - // for(std::vector::iterator it = current_waypoint_; it != finish_pose_; it++) { - // double dist = hypot(it->position.x - request.pose.position.x, it->position.y - request.pose.position.y); - // if(dist < min_dist) { - // min_dist = dist; - // current_waypoint_ = it; - // } - // } - // response.status = true; - // has_activate_ = true; - // return true; - // } - - - - // bool suspendPoseCallback(fulanghua_srvs::Pose::Request &request, fulanghua_srvs::Pose::Response &response) - // { - // if(!has_activate_) { - // response.status = false; - // return false; - // } - - // //move_base_action_.cancelAllGoals(); - // startNavigationGL(request.pose); - // bool isNavigationFinished = false; - // while(!isNavigationFinished && ros::ok()) { - // actionlib::SimpleClientGoalState state = move_base_action_.getState(); - // if(state == actionlib::SimpleClientGoalState::SUCCEEDED) { - // isNavigationFinished = true; - // response.status = true; - // } else if(state == actionlib::SimpleClientGoalState::ABORTED) { - // isNavigationFinished = true; - // response.status = false; - // } - // sleep(); - // } - // has_activate_ = false; - // return true; - // } - - - - bool searchPoseCallback(std_srvs::Trigger::Request &request, std_srvs::Trigger::Response &response) - { - if(has_activate_) { - response.success = false; - return false; - } - - tf::StampedTransform robot_gl = getRobotPosGL(); - std_srvs::Empty empty; - clear_costmaps_srv_.call(empty); - - double min_dist = std::numeric_limits::max(); - for(std::vector::iterator it = current_waypoint_; it != finish_pose_; it++) { - double dist = hypot(it->position.x - robot_gl.getOrigin().x(), it->position.y - robot_gl.getOrigin().y()); - if(dist < min_dist) { - min_dist = dist; - current_waypoint_ = it; - } - } - response.success = true; - has_activate_ = true; - return true; - } - - - + /* + ++++++++++ Callback function for cmd_vel topic ++++++++++ + */ void cmdVelCallback(const geometry_msgs::Twist &msg) { - if(msg.linear.x > -0.001 && msg.linear.x < 0.001 && - msg.linear.y > -0.001 && msg.linear.y < 0.001 && - msg.linear.z > -0.001 && msg.linear.z < 0.001 && - msg.angular.x > -0.001 && msg.angular.x < 0.001 && - msg.angular.y > -0.001 && msg.angular.y < 0.001 && - msg.angular.z > -0.001 && msg.angular.z < 0.001) + if (has_activate_ && + msg.linear.x > -0.001 && msg.linear.x < 0.001 && + msg.linear.y > -0.001 && msg.linear.y < 0.001 && + msg.linear.z > -0.001 && msg.linear.z < 0.001 && + msg.angular.x > -0.001 && msg.angular.x < 0.001 && + msg.angular.y > -0.001 && msg.angular.y < 0.001 && + msg.angular.z > -0.001 && msg.angular.z < 0.001) { - ROS_INFO("command velocity all zero"); - + // ROS_INFO("command velocity all zero"); } else { last_moved_time_ = ros::Time::now().toSec(); } @@ -343,26 +254,9 @@ - bool shouldSendGoal() - { - bool ret = true; - actionlib::SimpleClientGoalState state = move_base_action_.getState(); - if((state != actionlib::SimpleClientGoalState::ACTIVE) && - (state != actionlib::SimpleClientGoalState::PENDING) && - (state != actionlib::SimpleClientGoalState::RECALLED) && - (state != actionlib::SimpleClientGoalState::PREEMPTED)) - { - ret = false; - } - - if(waypoints_.poses.empty()) { - ret = false; - } - return ret; - } - - - + /* + ++++++++++ Check if robot reached the goal sent to move_base ++++++++++ + */ bool navigationFinished() { return move_base_action_.getState() == actionlib::SimpleClientGoalState::SUCCEEDED; @@ -370,8 +264,14 @@ + /* + ++++++++++ Check if robot reached current waypoint ++++++++++ + */ bool onNavigationPoint(const geometry_msgs::Point &dest, double dist_err = 0.8) { + if (waypoint_list_[current_wp_num_].get_stop()) { + return navigationFinished(); + } tf::StampedTransform robot_gl = getRobotPosGL(); const double wx = dest.x; const double wy = dest.y; @@ -383,6 +283,9 @@ + /* + ++++++++++ Get gloabl position of robot ++++++++++ + */ tf::StampedTransform getRobotPosGL() { tf::StampedTransform robot_gl; @@ -391,12 +294,14 @@ } catch(tf::TransformException &e) { ROS_WARN_STREAM("tf::TransformException: " << e.what()); } - return robot_gl; } + /* + ++++++++++ Sleep function used in main loop function ++++++++++ + */ void sleep() { rate_.sleep(); @@ -406,16 +311,20 @@ - void startNavigationGL(const geometry_msgs::Point &dest) + /* + ++++++++++ Publish waypoints to be displayed as arrows on rviz ++++++++++ + */ + void publishPoseArray() { - geometry_msgs::Pose pose; - pose.position = dest; - pose.orientation = tf::createQuaternionMsgFromYaw(0.0); - startNavigationGL(pose); + waypoints_.header.stamp = ros::Time::now(); + wp_pub_.publish(waypoints_); } + /* + ++++++++++ Send goal to move_base ++++++++++ + */ void startNavigationGL(const geometry_msgs::Pose &dest) { setMaxVel(); @@ -428,76 +337,81 @@ } - - void publishPoseArray() - { - waypoints_.header.stamp = ros::Time::now(); - wp_pub_.publish(waypoints_); - } - + /* + ++++++++++ Publish ratio to maximum speed ++++++++++ + */ void setMaxVel() { float max_vel_rate = waypoint_list_[current_wp_num_].get_vel(); - std_msgs::Float32 msg; - msg.data = max_vel_rate; - max_vel_pub_.publish(msg); + max_vel_msg_.data = max_vel_rate; + max_vel_pub_.publish(max_vel_msg_); } - + /* + ++++++++++ Main loop function ++++++++++ + */ void run() { while(ros::ok()) { - try { - if(has_activate_) { - if(current_waypoint_ == last_waypoint_) { - ROS_INFO("prepare finish pose"); - } else { - ROS_INFO("calculate waypoint direction"); - ROS_INFO_STREAM("goal_direction = " << current_waypoint_->orientation); - ROS_INFO_STREAM("current_waypoint_+1 " << (current_waypoint_+1)->position.y); - ROS_INFO_STREAM("current_waypoint_" << current_waypoint_->position.y); - } - - startNavigationGL(*current_waypoint_); - int resend_goal = 0; - double start_nav_time = ros::Time::now().toSec(); - dist_err_ = waypoint_list_[current_wp_num_].get_rad(); - while(!onNavigationPoint(current_waypoint_->position, dist_err_)) { - if(!has_activate_) throw SwitchRunningStatus(); - - double time = ros::Time::now().toSec(); - if(time - start_nav_time > 10.0 && time - last_moved_time_ > 10.0) { - ROS_WARN("Resend the navigation goal."); - std_srvs::Empty empty; - clear_costmaps_srv_.call(empty); - startNavigationGL(*current_waypoint_); - resend_goal++; - if(resend_goal == 3) { - ROS_WARN("Skip waypoint."); - current_waypoint_++; - current_wp_num_++; - startNavigationGL(*current_waypoint_); - } - start_nav_time = time; - } - sleep(); - } - - current_waypoint_++; - current_wp_num_++; - if(current_waypoint_ == finish_pose_) { - startNavigationGL(*current_waypoint_); - while(!navigationFinished() && ros::ok()) sleep(); - has_activate_ = false; - } - } - } catch(const SwitchRunningStatus &e) { - ROS_INFO_STREAM("running status switched"); + // has_activate_ is false, nothing to do + if (!has_activate_) { + sleep(); + continue; } + // go to fianal goal and finish process + if (current_waypoint_ == finish_pose_) { + ROS_INFO_STREAM("Go to final goal"); + startNavigationGL(*current_waypoint_); + while(!navigationFinished() && ros::ok()) sleep(); + ROS_INFO("Final goal reached!!"); + has_activate_ = false; + continue; + } + // go to current waypoint + ROS_INFO_STREAM("Go to waypoint " << current_wp_num_+1); + startNavigationGL(*current_waypoint_); + int resend_goal = 0; + double start_nav_time = ros::Time::now().toSec(); + dist_err_ = waypoint_list_[current_wp_num_].get_rad(); + try { + // loop until reach waypoint + while(!onNavigationPoint(current_waypoint_->position, dist_err_)) { + if (!has_activate_) { + throw SwitchRunningStatus(); + } + double time = ros::Time::now().toSec(); + if (time - start_nav_time > 10.0 && time - last_moved_time_ > 10.0) { + ROS_WARN("Resend the navigation goal."); + std_srvs::Empty empty; + clear_costmaps_srv_.call(empty); + startNavigationGL(*current_waypoint_); + resend_goal++; + if (resend_goal == 3) { + ROS_WARN("Skip waypoint."); + current_waypoint_++; + current_wp_num_++; + startNavigationGL(*current_waypoint_); + } + start_nav_time = time; + } + sleep(); + } + // if current waypoint is stop point + if (waypoint_list_[current_wp_num_].get_stop()) { + has_activate_ = false; + move_base_action_.cancelAllGoals(); + ROS_INFO("Waiting for navigation to resume..."); + } + // update current waypoint + current_waypoint_++; + current_wp_num_++; + } catch(const SwitchRunningStatus &e) { + ROS_INFO_STREAM("Running status switched"); + } sleep(); } } @@ -511,18 +425,18 @@ visualization_msgs::MarkerArray marker_; std::vector waypoint_list_; std::vector::iterator current_waypoint_; - std::vector::iterator last_waypoint_; std::vector::iterator finish_pose_; bool has_activate_; std::string robot_frame_, world_frame_; tf::TransformListener tf_listener_; ros::Rate rate_; - ros::ServiceServer start_server_, pause_server_, unpause_server_, stop_server_, suspend_server_, resume_server_ ,search_server_; + ros::ServiceServer start_server_, resume_server_; ros::Subscriber cmd_vel_sub_; ros::Publisher wp_pub_, max_vel_pub_; ros::ServiceClient clear_costmaps_srv_; double last_moved_time_, dist_err_; unsigned short int current_wp_num_; + std_msgs::Float32 max_vel_msg_; }; diff --git a/waypoint_saver/include/waypoint_saver.h b/waypoint_saver/include/waypoint_saver.h index 706b689..c386710 100644 --- a/waypoint_saver/include/waypoint_saver.h +++ b/waypoint_saver/include/waypoint_saver.h @@ -5,18 +5,26 @@ public: float vel; float rad; + bool stop; - Waypoint() : vel(1.0), rad(0.8) {} + Waypoint() : + vel(1.0), + rad(0.8), + stop(false) + { + } double get_x() { return point.x; } double get_y() { return point.y; } double get_z() { return point.z; } float get_vel() { return vel; } float get_rad() { return rad; } + bool get_stop() { return stop; } void set_x(double x) { point.x = x; } void set_y(double y) { point.y = y; } void set_z(double z) { point.z = z; } void set_vel(float v) { vel = v; } void set_rad(float r) { rad = r; } + void set_stop(bool s) { stop = s; } }; \ No newline at end of file