waypoint_provider.hpp
Go to the documentation of this file.
00001 /*
00002    Way point Provider
00003 
00004    highly inspired by yocs_waypoint_navi written by Jorge Santos
00005 
00006    LICENSE : BSD - https://raw.github.com/yujinrobot/yujin_ocs/license/LICENSE
00007 
00008    Author : Jihoon Lee
00009    Date   : Dec 2013
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    It simply parses waypoints in yaml file and provides latch topic
00026 
00027    Future improvements
00028     - Utilise database for persistent waypoint storing
00029     - add/remove waypoints via srv or pub/sub
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_


yocs_waypoint_provider
Author(s): Jihoon Lee
autogenerated on Thu Jun 6 2019 21:47:44