00001 00006 #ifndef YOCS_WAYPOINT_NAVI_HPP_ 00007 #define YOCS_WAYPOINT_NAVI_HPP_ 00008 00009 #include <tf/tf.h> 00010 #include <tf/transform_listener.h> 00011 #include <ros/ros.h> 00012 #include <geometry_msgs/PointStamped.h> 00013 #include <move_base_msgs/MoveBaseAction.h> 00014 #include <actionlib/client/simple_action_client.h> 00015 #include <yocs_math_toolkit/common.hpp> 00016 #include <yocs_math_toolkit/geometry.hpp> 00017 #include <yocs_msgs/NavigationControl.h> 00018 #include <yocs_msgs/NavigationControlStatus.h> 00019 #include <yocs_msgs/TrajectoryList.h> 00020 #include <yocs_msgs/WaypointList.h> 00021 00022 00023 namespace yocs 00024 { 00025 00026 /* 00027 * TODO 00028 * * think about how to best visualise the waypoint(s)/trajectory(ies) which are being executed 00029 * * add RViz interface to yocs_waypoint_provider 00030 */ 00031 00032 class WaypointsGoalNode 00033 { 00034 public: 00035 WaypointsGoalNode(); 00036 ~WaypointsGoalNode(); 00037 00038 bool init(); 00039 00040 void waypointsCB(const yocs_msgs::WaypointList::ConstPtr& wps); 00041 00042 void trajectoriesCB(const yocs_msgs::TrajectoryList::ConstPtr& trajs); 00043 00044 void navCtrlCB(const yocs_msgs::NavigationControl::ConstPtr& nav_ctrl); 00045 00046 void spin(); 00047 00048 private: 00049 const geometry_msgs::PoseStamped NOWHERE; 00050 00051 enum { NONE = 0, 00052 GOAL, 00053 LOOP 00054 } mode_; 00055 00056 enum { IDLE = 0, 00057 START, 00058 ACTIVE, 00059 COMPLETED 00060 } state_; 00061 00062 double frequency_; 00063 double close_enough_; 00064 double goal_timeout_; 00065 std::string robot_frame_; 00066 std::string world_frame_; 00067 00068 std::vector<geometry_msgs::PoseStamped> waypoints_; 00069 std::vector<geometry_msgs::PoseStamped>::iterator waypoints_it_; 00070 00071 geometry_msgs::PoseStamped goal_; 00072 00073 yocs_msgs::WaypointList wp_list_; 00074 yocs_msgs::TrajectoryList traj_list_; 00075 00076 tf::TransformListener tf_listener_; 00077 ros::Subscriber waypoints_sub_; 00078 ros::Subscriber trajectories_sub_; 00079 00080 ros::Subscriber nav_ctrl_sub_; 00081 ros::Publisher status_pub_; 00082 bool idle_status_update_sent_; 00083 00084 actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> move_base_ac_; 00085 00086 bool cancelAllGoals(double timeout = 2.0); 00087 00088 void resetWaypoints(); 00089 00090 bool equals(const geometry_msgs::PoseStamped& a, const geometry_msgs::PoseStamped& b); 00091 00092 bool equals(const geometry_msgs::Point& a, const geometry_msgs::Point& b); 00093 00094 void publishStatusUpdate(const uint8_t& status); 00095 }; 00096 00097 } // namespace yocs 00098 00099 #endif /* YOCS_WAYPOINT_NAVI_HPP_ */