Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013 #ifndef _YOCS_WAYPOINT_PROVIDER_HPP_
00014 #define _YOCS_WAYPOINT_PROVIDER_HPP_
00015
00016 #include <geometry_msgs/Pose.h>
00017 #include <ros/ros.h>
00018 #include <visualization_msgs/MarkerArray.h>
00019 #include <yocs_msgs/Trajectory.h>
00020 #include <yocs_msgs/TrajectoryList.h>
00021 #include <yocs_msgs/WaypointListService.h>
00022 #include <yocs_msgs/WaypointList.h>
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032 namespace yocs {
00033 class WaypointProvider {
00034 public:
00035 WaypointProvider(ros::NodeHandle& n, yocs_msgs::WaypointList& wps, yocs_msgs::TrajectoryList& trajs);
00036 ~WaypointProvider();
00037
00038 void spin();
00039 protected:
00040 void init();
00041 bool processWaypointsService(yocs_msgs::WaypointListService::Request& request,
00042 yocs_msgs::WaypointListService::Response& response);
00043 void generateWaypointMarkers(const yocs_msgs::WaypointList& wps, visualization_msgs::MarkerArray& wp_markers);
00044 void generateTrajectoryMarkers(const yocs_msgs::TrajectoryList& trajs,
00045 visualization_msgs::MarkerArray& traj_markers);
00046 void createMarkerArrow(const int i, const yocs_msgs::Waypoint& wp, visualization_msgs::Marker& marker);
00047 void createMarkerLineStrip(const int i, const yocs_msgs::Trajectory& traj, visualization_msgs::Marker& marker);
00048 void createMarkerLabel(const std::string frame_id,
00049 const int id,
00050 const std::string ns,
00051 const std::string wp_name,
00052 const geometry_msgs::Pose wp_pose,
00053 visualization_msgs::Marker& marker);
00054
00055 private:
00056 bool initialized_;
00057 ros::NodeHandle nh_;
00058 ros::Publisher waypoints_pub_, trajectories_pub_;
00059 ros::Publisher waypoints_marker_pub_, trajectory_marker_pub_;
00060 ros::ServiceServer waypoints_srv_;
00061
00062 yocs_msgs::WaypointList waypoints_;
00063 yocs_msgs::TrajectoryList trajectories_;
00064 visualization_msgs::MarkerArray waypoint_markers_, trajectory_markers_;
00065
00066 int marker_index_;
00067 int label_index_;
00068 };
00069 }
00070
00071 #endif // _YOCS_WAYPOINT_PROVIDER_HPP_