diff --git a/waypoint_nav/rviz_cfg/nav.rviz b/waypoint_nav/rviz_cfg/nav.rviz index 2d1d5c8..4e51bd4 100644 --- a/waypoint_nav/rviz_cfg/nav.rviz +++ b/waypoint_nav/rviz_cfg/nav.rviz @@ -269,7 +269,7 @@ Background Color: 48; 48; 48 Default Light: true Fixed Frame: map - Frame Rate: 30 + Frame Rate: 10 Name: root Tools: - Class: rviz/Interact diff --git a/waypoint_nav/src/waypoint_nav.cpp b/waypoint_nav/src/waypoint_nav.cpp index d983e6d..1a2128b 100644 --- a/waypoint_nav/src/waypoint_nav.cpp +++ b/waypoint_nav/src/waypoint_nav.cpp @@ -1,10 +1,10 @@ #include +#include #include #include #include #include #include -#include #include #include #include @@ -75,17 +75,19 @@ computeWpOrientation(); } current_waypoint_ = waypoints_.poses.begin(); - current_wp_num_ = 0; + wp_num_.data = 1; } else { ROS_ERROR("waypoints file doesn't have name"); } ros::NodeHandle nh; start_server_ = nh.advertiseService("start_wp_nav", &WaypointsNavigation::startNavigationCallback, this); + stop_server_ = nh.advertiseService("stop_wp_nav", &WaypointsNavigation::stopNavigationCallback, this); resume_server_ = nh.advertiseService("resume_nav", &WaypointsNavigation::resumeNavigationCallback, this); - cmd_vel_sub_ = nh.subscribe("/cmd_vel", 1, &WaypointsNavigation::cmdVelCallback, 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); + max_vel_pub_ = nh.advertise("max_vel", 5); + wp_num_pub_ = nh.advertise("waypoint_number", 5); clear_costmaps_srv_ = nh.serviceClient("/move_base/clear_costmaps"); } @@ -109,7 +111,7 @@ parser.GetNextDocument(node); #endif - // read waypoints + // Read waypoints from yaml file #ifdef NEW_YAMLCPP const YAML::Node &wp_node_tmp = node["waypoints"]; const YAML::Node *wp_node = wp_node_tmp ? &wp_node_tmp : NULL; @@ -118,8 +120,6 @@ #endif geometry_msgs::Pose pose; Waypoint point; - // float vel, rad; - // bool stop; if (wp_node != NULL) { for(int i=0; i < wp_node->size(); i++) { (*wp_node)[i]["point"]["x"] >> pose.position.x; @@ -132,16 +132,13 @@ (*wp_node)[i]["point"]["vel"] >> point.vel; (*wp_node)[i]["point"]["rad"] >> point.rad; (*wp_node)[i]["point"]["stop"] >> point.stop; - // point.vel = vel; - // point.rad = rad; - // point.stop = stop; waypoint_list_.push_back(point); } } else { return false; } - // read finish_pose + // 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; @@ -207,7 +204,7 @@ sleep(); } current_waypoint_ = waypoints_.poses.begin(); - current_wp_num_ = 0; + wp_num_.data = 1; has_activate_ = true; response.success = true; return true; @@ -234,6 +231,24 @@ /* + ++++++++++ Callback function for service of "stop_wp_nav" ++++++++++ + */ + bool stopNavigationCallback(std_srvs::Trigger::Request &request, std_srvs::Trigger::Response &response) + { + if (has_activate_) { + ROS_INFO("Waypoint navigation has been stopped"); + has_activate_ = false; + response.success = true; + } else { + ROS_WARN("Waypoint navigation is already stopped"); + response.success = false; + } + return true; + } + + + + /* ++++++++++ Callback function for cmd_vel topic ++++++++++ */ void cmdVelCallback(const geometry_msgs::Twist &msg) @@ -269,7 +284,7 @@ */ bool onNavigationPoint(const geometry_msgs::Point &dest, double dist_err = 0.8) { - if (waypoint_list_[current_wp_num_].stop) { + if (waypoint_list_[wp_num_.data-1].stop) { return navigationFinished(); } tf::StampedTransform robot_gl = getRobotPosGL(); @@ -343,7 +358,7 @@ */ void setMaxVel() { - float max_vel_rate = waypoint_list_[current_wp_num_].vel; + float max_vel_rate = waypoint_list_[wp_num_.data-1].vel; max_vel_msg_.data = max_vel_rate; max_vel_pub_.publish(max_vel_msg_); } @@ -371,11 +386,11 @@ continue; } // go to current waypoint - ROS_INFO_STREAM("Go to waypoint " << current_wp_num_+1); + ROS_INFO_STREAM("Go to waypoint " << wp_num_.data); startNavigationGL(*current_waypoint_); int resend_goal = 0; double start_nav_time = ros::Time::now().toSec(); - dist_err_ = waypoint_list_[current_wp_num_].rad; + dist_err_ = waypoint_list_[wp_num_.data-1].rad; try { // loop until reach waypoint while(!onNavigationPoint(current_waypoint_->position, dist_err_)) { @@ -392,22 +407,23 @@ if (resend_goal == 3) { ROS_WARN("Skip waypoint."); current_waypoint_++; - current_wp_num_++; + wp_num_.data++; startNavigationGL(*current_waypoint_); } start_nav_time = time; } + wp_num_pub_.publish(wp_num_); sleep(); } // if current waypoint is stop point - if (waypoint_list_[current_wp_num_].stop) { + if (waypoint_list_[wp_num_.data-1].stop) { has_activate_ = false; move_base_action_.cancelAllGoals(); ROS_INFO("Waiting for navigation to resume..."); } // update current waypoint current_waypoint_++; - current_wp_num_++; + wp_num_.data++; } catch(const SwitchRunningStatus &e) { ROS_INFO_STREAM("Running status switched"); @@ -430,12 +446,12 @@ std::string robot_frame_, world_frame_; tf::TransformListener tf_listener_; ros::Rate rate_; - ros::ServiceServer start_server_, resume_server_; + ros::ServiceServer start_server_, stop_server_, resume_server_; ros::Subscriber cmd_vel_sub_; - ros::Publisher wp_pub_, max_vel_pub_; + ros::Publisher wp_pub_, max_vel_pub_, wp_num_pub_; ros::ServiceClient clear_costmaps_srv_; double last_moved_time_, dist_err_; - unsigned short int current_wp_num_; + std_msgs::UInt16 wp_num_; std_msgs::Float32 max_vel_msg_; }; diff --git a/waypoint_saver/rviz_cfg/record_waypoints.rviz b/waypoint_saver/rviz_cfg/record_waypoints.rviz index 71a0db5..659846c 100644 --- a/waypoint_saver/rviz_cfg/record_waypoints.rviz +++ b/waypoint_saver/rviz_cfg/record_waypoints.rviz @@ -177,7 +177,7 @@ Background Color: 48; 48; 48 Default Light: true Fixed Frame: map - Frame Rate: 30 + Frame Rate: 10 Name: root Tools: - Class: rviz/Interact