diff --git a/waypoint_nav/CMakeLists.txt b/waypoint_nav/CMakeLists.txt index af262e2..ffbceca 100644 --- a/waypoint_nav/CMakeLists.txt +++ b/waypoint_nav/CMakeLists.txt @@ -7,196 +7,43 @@ ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## is used, also find other catkin packages -find_package(catkin REQUIRED) - -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) +find_package(catkin REQUIRED + move_base_msgs + geometry_msgs + move_base + roscpp + actionlib_msgs + actionlib + tf + std_srvs +) -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() -################################################ -## Declare ROS messages, services and actions ## -################################################ +find_package(PkgConfig) +pkg_search_module(yaml-cpp REQUIRED yaml-cpp) +pkg_search_module(waypoint_saver REQUIRED waypoint_saver) -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend tag for "message_generation" -## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependency has been pulled in -## but can be declared for certainty nonetheless: -## * add a exec_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) +if(NOT ${yaml-cpp_VERSION} VERSION_LESS "0.5") +add_definitions(-DNEW_YAMLCPP) +endif() -## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) - -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -# generate_messages( -# DEPENDENCIES -# std_msgs # Or other packages containing msgs -# ) - -################################################ -## Declare ROS dynamic reconfigure parameters ## -################################################ - -## To declare and build dynamic reconfigure parameters within this -## package, follow these steps: -## * In the file package.xml: -## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" -## * In this file (CMakeLists.txt): -## * add "dynamic_reconfigure" to -## find_package(catkin REQUIRED COMPONENTS ...) -## * uncomment the "generate_dynamic_reconfigure_options" section below -## and list every .cfg file to be processed - -## Generate dynamic reconfigure parameters in the 'cfg' folder -# generate_dynamic_reconfigure_options( -# cfg/DynReconf1.cfg -# cfg/DynReconf2.cfg -# ) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if your package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need catkin_package( -# INCLUDE_DIRS include -# LIBRARIES waypoint_nav -# CATKIN_DEPENDS other_catkin_pkg -# DEPENDS system_lib + INCLUDE_DIRS include + LIBRARIES ${PROJECT_NAME} ) -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations include_directories( -# include -# ${catkin_INCLUDE_DIRS} + include + ${catkin_INCLUDE_DIRS} + ${yaml-cpp_INCLUDE_DIRS} + ${waypoint_saver_INCLUDE_DIRS} ) -## Declare a C++ library -# add_library(${PROJECT_NAME} -# src/${PROJECT_NAME}/waypoint_nav.cpp -# ) +add_executable(waypoint_nav src/waypoint_nav.cpp) -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(waypoint_nav + ${catkin_LIBRARIES} + ${yaml-cpp_LIBRARIES} +) -## Declare a C++ executable -## With catkin_make all packages are built within a single CMake context -## The recommended prefix ensures that target names across packages don't collide -# add_executable(${PROJECT_NAME}_node src/waypoint_nav_node.cpp) - -## Rename C++ executable without prefix -## The above recommended prefix causes long target names, the following renames the -## target back to the shorter version for ease of user use -## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" -# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") - -## Add cmake target dependencies of the executable -## same as for the library above -# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Specify libraries to link a library or executable target against -# target_link_libraries(${PROJECT_NAME}_node -# ${catkin_LIBRARIES} -# ) - -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# catkin_install_python(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html -# install(TARGETS ${PROJECT_NAME}_node -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark libraries for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html -# install(TARGETS ${PROJECT_NAME} -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} -# ) - -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) - -## Mark other files for installation (e.g. launch and bag files, etc.) -# install(FILES -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_waypoint_nav.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() - -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) diff --git a/waypoint_nav/include/waypoint_nav.h b/waypoint_nav/include/waypoint_nav.h index e69de29..de4896e 100644 --- a/waypoint_nav/include/waypoint_nav.h +++ b/waypoint_nav/include/waypoint_nav.h @@ -0,0 +1,34 @@ +// #include +// #include // include Waypoint class +// #include + + +// class WaypointArray : public geometry_msgs::PoseArray { +// /* +// std_msgs/Header header +// geometry_msgs/Pose[] poses +// */ +// public: +// std::vector waypoint_list; + + +// void add_waypoint(double x, double y, double z, float vel, float rad) +// { +// Waypoint point; +// point.set_x(x); +// point.set_y(y); +// point.set_z(z); +// point.set_vel(vel); +// point.set_rad(rad); +// waypoint_list.push_back(point); +// } + + +// geometry_msgs::PoseArray get_posearray_msg() +// { +// geometry_msgs::PoseArray posearray; +// posearray.header = header; +// posearray.poses = poses; +// return posearray; +// } +// }; \ No newline at end of file diff --git a/waypoint_nav/launch/waypoint_nav.launch b/waypoint_nav/launch/waypoint_nav.launch index e69de29..82fc85f 100644 --- a/waypoint_nav/launch/waypoint_nav.launch +++ b/waypoint_nav/launch/waypoint_nav.launch @@ -0,0 +1,10 @@ + + + + + + + + + + \ No newline at end of file diff --git a/waypoint_nav/package.xml b/waypoint_nav/package.xml index 45f9037..cdeeed2 100644 --- a/waypoint_nav/package.xml +++ b/waypoint_nav/package.xml @@ -16,40 +16,27 @@ TODO - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - catkin + move_base + yaml-cpp + move_base_msgs + geometry_msgs + roscpp + actionlib_msgs + actionlib + tf + std_srvs + + move_base + yaml-cpp + move_base_msgs + geometry_msgs + roscpp + actionlib_msgs + actionlib + tf + diff --git a/waypoint_nav/param/test.yaml b/waypoint_nav/param/test.yaml index 4adc38e..faed547 100644 --- a/waypoint_nav/param/test.yaml +++ b/waypoint_nav/param/test.yaml @@ -1,27 +1,7 @@ waypoints: -- point: {x: 15.549, y: 1.99887, z: 0} -- point: {x: 27.7033, y: 0.740999, z: 0} -- point: {x: 45.603441, y: -10.556894, z: 0.0} -- point: {x: 53.4473, y: -1.98026, z: 0} -- point: {x: 53.5381, y: 13.9875, z: 0} -- point: {x: 53.0202, y: 25.7033, z: 0} -- point: {x: 52.624, y: 40.8326, z: 0} -- point: {x: 52.4651, y: 56.1678, z: 0} -- point: {x: 47.4651, y: 55.9678, z: 0} -- point: {x: 41.1833, y: 55.7508, z: 0} -- point: {x: 30.9773, y: 56.3972, z: 0} -- point: {x: 19.605, y: 55.9349, z: 0} -- point: {x: 9.74244, y: 57.775, z: 0} -- point: {x: 9.04664, y: 53.1814, z: 0} -- point: {x: 12.2474, y: 45.1187, z: 0} -- point: {x: 17.5568, y: 32.2592, z: 0} -- point: {x: 19.6331, y: 27.0299, z: 0} -- point: {x: 23.7635, y: 17.3945, z: 0} -- point: {x: 27.6797, y: 6.92171, z: 0} -- point: {x: 29.8247, y: 1.30448, z: 0} -- point: {x: 12.7818, y: 1.13019, z: 0} +- point: {x: 0.5, y: 0, z: 0, vel: 1, rad: 0.8} finish_pose: - header: {seq: 0, stamp: 1623386648.025251, frame_id: base_link} - pose: - position: {x: -0.8863756, y: 0.5020624, z: 0} - orientation: {x: 0, y: 0, z: 0.999797, w: -0.0201511} \ No newline at end of file + header: {seq: 0, stamp: 0.000000000, frame_id: } + pose: + position: {x: 0, y: 0, z: 0} + orientation: {x: 0, y: 0, z: 0, w: 0} diff --git a/waypoint_nav/src/waypoint_nav.cpp b/waypoint_nav/src/waypoint_nav.cpp index ffed191..126a4f1 100644 --- a/waypoint_nav/src/waypoint_nav.cpp +++ b/waypoint_nav/src/waypoint_nav.cpp @@ -1,10 +1,517 @@ #include -#include - -#include -#include +#include +#include +#include +#include #include +#include +#include +#include +#include +#include +#include + +#include +#include // include Waypoint class #include #include -#include \ No newline at end of file +#include +#include +#include +#include + +#ifdef NEW_YAMLCPP +template +void operator >> (const YAML::Node& node, T& i) +{ + i = node.as(); +} +#endif + + +class SwitchRunningStatus : public std::exception { +public: + SwitchRunningStatus() : std::exception() { } +}; + + + +class WaypointsNavigation{ +public: + WaypointsNavigation() : + has_activate_(false), + move_base_action_("move_base", true), + rate_(10), + last_moved_time_(0), + dist_err_(0.8) + { + while((move_base_action_.waitForServer(ros::Duration(1.0)) == false) && (ros::ok() == true)) + { + ROS_INFO("Waiting..."); + } + ros::NodeHandle private_nh("~"); + private_nh.param("robot_frame", robot_frame_, std::string("base_link")); + private_nh.param("world_frame", world_frame_, std::string("map")); + + double max_update_rate; + private_nh.param("max_update_rate", max_update_rate, 10.0); + rate_ = ros::Rate(max_update_rate); + std::string filename = ""; + private_nh.param("filename", filename, filename); + if(filename != "") { + ROS_INFO_STREAM("Read waypoints data from " << filename); + if(!readFile(filename)) { + ROS_ERROR("Failed loading waypoints file"); + } else { + last_waypoint_ = waypoints_.poses.end()-2; + finish_pose_ = waypoints_.poses.end()-1; + computeWpOrientation(); + } + current_waypoint_ = waypoints_.poses.begin(); + } else { + ROS_ERROR("waypoints file doesn't have name"); + } + + private_nh.param("dist_err", dist_err_, dist_err_); + + 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); + cmd_vel_sub_ = nh.subscribe("icart_mini/cmd_vel", 1, &WaypointsNavigation::cmdVelCallback, this); + wp_pub_ = nh.advertise("waypoints", 10); + clear_costmaps_srv_ = nh.serviceClient("/move_base/clear_costmaps"); + } + + + + 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; + + YAML::Node node; + + #ifdef NEW_YAMLCPP + node = YAML::Load(ifs); + #else + YAML::Parser parser(ifs); + parser.GetNextDocument(node); + #endif + + #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) { + 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; + 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); + waypoint_list_.push_back(point); + } + } else { + return false; + } + + #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) { + (*fp_node)["pose"]["position"]["x"] >> pose.position.x; + (*fp_node)["pose"]["position"]["y"] >> pose.position.y; + (*fp_node)["pose"]["position"]["z"] >> pose.position.z; + (*fp_node)["pose"]["orientation"]["x"] >> pose.orientation.x; + (*fp_node)["pose"]["orientation"]["y"] >> pose.orientation.y; + (*fp_node)["pose"]["orientation"]["z"] >> pose.orientation.z; + (*fp_node)["pose"]["orientation"]["w"] >> pose.orientation.w; + 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(1.0); + point.set_rad(0.3); + waypoint_list_.push_back(point); + } else { + return false; + } + + } catch(YAML::ParserException &e) { + return false; + + } catch(YAML::RepresentationException &e) { + return false; + } + + return true; + } + + + + void computeWpOrientation() + { + for(std::vector::iterator it = waypoints_.poses.begin(); it != finish_pose_; it++) { + double goal_direction = atan2((it+1)->position.y - (it)->position.y, + (it+1)->position.x - (it)->position.x); + (it)->orientation = tf::createQuaternionMsgFromYaw(goal_direction); + } + waypoints_.header.frame_id = world_frame_; + } + + + + bool startNavigationCallback(std_srvs::Trigger::Request &request, std_srvs::Trigger::Response &response) + { + if(has_activate_) { + response.success = false; + return false; + } + std_srvs::Empty empty; + while(!clear_costmaps_srv_.call(empty)) { + ROS_WARN("Resend clear costmap service"); + sleep(); + } + current_waypoint_ = waypoints_.poses.begin(); + has_activate_ = true; + response.success = true; + return true; + } + + + + bool pauseNavigationCallback(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_) { + ROS_WARN("Navigation is already active"); + response.success = false; + } + 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; + } + + + + 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) + { + ROS_INFO("command velocity all zero"); + + } else { + last_moved_time_ = ros::Time::now().toSec(); + } + } + + + + 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; + } + + + + bool navigationFinished() + { + return move_base_action_.getState() == actionlib::SimpleClientGoalState::SUCCEEDED; + } + + + + bool onNavigationPoint(const geometry_msgs::Point &dest, double dist_err = 0.8) + { + tf::StampedTransform robot_gl = getRobotPosGL(); + const double wx = dest.x; + const double wy = dest.y; + const double rx = robot_gl.getOrigin().x(); + const double ry = robot_gl.getOrigin().y(); + const double dist = std::sqrt(std::pow(wx - rx, 2) + std::pow(wy - ry, 2)); + return dist < dist_err; + } + + + + tf::StampedTransform getRobotPosGL() + { + tf::StampedTransform robot_gl; + try { + tf_listener_.lookupTransform(world_frame_, robot_frame_, ros::Time(0.0), robot_gl); + } catch(tf::TransformException &e) { + ROS_WARN_STREAM("tf::TransformException: " << e.what()); + } + + return robot_gl; + } + + + + void sleep() + { + rate_.sleep(); + ros::spinOnce(); + publishPoseArray(); + } + + + + void startNavigationGL(const geometry_msgs::Point &dest) + { + geometry_msgs::Pose pose; + pose.position = dest; + pose.orientation = tf::createQuaternionMsgFromYaw(0.0); + startNavigationGL(pose); + } + + + + void startNavigationGL(const geometry_msgs::Pose &dest) + { + move_base_msgs::MoveBaseGoal move_base_goal; + move_base_goal.target_pose.header.stamp = ros::Time::now(); + move_base_goal.target_pose.header.frame_id = world_frame_; + move_base_goal.target_pose.pose.position = dest.position; + move_base_goal.target_pose.pose.orientation = dest.orientation; + move_base_action_.sendGoal(move_base_goal); + } + + + + void publishPoseArray() + { + waypoints_.header.stamp = ros::Time::now(); + wp_pub_.publish(waypoints_); + } + + + + 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(); + 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_++; + startNavigationGL(*current_waypoint_); + } + start_nav_time = time; + } + sleep(); + } + + current_waypoint_++; + 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"); + } + + sleep(); + } + } + + + +private: + actionlib::SimpleActionClient move_base_action_; + geometry_msgs::PoseArray waypoints_; + 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::Subscriber cmd_vel_sub_; + ros::Publisher wp_pub_; + ros::ServiceClient clear_costmaps_srv_; + double last_moved_time_, dist_err_; +}; + + + + + + +int main(int argc, char *argv[]){ + ros::init(argc, argv, ROS_PACKAGE_NAME); + WaypointsNavigation w_nav; + // w_nav.run(); + + return 0; +} \ No newline at end of file diff --git a/waypoint_saver/CMakeLists.txt b/waypoint_saver/CMakeLists.txt index aac7f4d..b10b611 100644 --- a/waypoint_saver/CMakeLists.txt +++ b/waypoint_saver/CMakeLists.txt @@ -11,14 +11,14 @@ catkin_package( - #INCLUDE_DIRS include + INCLUDE_DIRS include LIBRARIES ${PROJECT_NAME} ) include_directories( - #include + include ${catkin_INCLUDE_DIRS} ${yaml-cpp_INCLUDE_DIRS} ) diff --git a/waypoint_saver/include/waypoint_saver.h b/waypoint_saver/include/waypoint_saver.h index e69de29..706b689 100644 --- a/waypoint_saver/include/waypoint_saver.h +++ b/waypoint_saver/include/waypoint_saver.h @@ -0,0 +1,22 @@ +#include + + +class Waypoint : public geometry_msgs::PointStamped { +public: + float vel; + float rad; + + Waypoint() : vel(1.0), rad(0.8) {} + + 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; } + + 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; } +}; \ No newline at end of file diff --git a/waypoint_saver/src/waypoint_saver.cpp b/waypoint_saver/src/waypoint_saver.cpp index afe356b..8505937 100644 --- a/waypoint_saver/src/waypoint_saver.cpp +++ b/waypoint_saver/src/waypoint_saver.cpp @@ -9,26 +9,7 @@ #include #include - -class Waypoint : public geometry_msgs::PointStamped { -public: - float vel; - float rad; - - Waypoint() : vel(1.0), rad(0.8) {} - - 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; } - - void set_x(double x) { point.x = x; } - void set_y(double y) { point.x = y; } - void set_z(double z) { point.x = z; } - void set_vel(float v) { vel = v; } - void set_rad(float r) { rad = r; } -}; +#include // include Waypoint class @@ -198,7 +179,6 @@ { ros::init(argc, argv, "waypoints_saver"); WaypointsSaver saver; - saver.save2(); saver.run(); return 0;