Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029 #include <cstdio>
00030 #include "ros/ros.h"
00031 #include "ros/console.h"
00032
00033 #include "nav_msgs/Path.h"
00034 #include "std_msgs/String.h"
00035
00036 #include <geometry_msgs/Quaternion.h>
00037 #include <geometry_msgs/PoseStamped.h>
00038
00039 #include "tf/transform_listener.h"
00040
00041 #include <hector_nav_msgs/GetRobotTrajectory.h>
00042 #include <hector_nav_msgs/GetRecoveryInfo.h>
00043
00044 #include <tf/tf.h>
00045
00046 #include <algorithm>
00047
00048 using namespace std;
00049
00050 bool comparePoseStampedStamps (const geometry_msgs::PoseStamped& t1, const geometry_msgs::PoseStamped& t2) { return (t1.header.stamp < t2.header.stamp); }
00051
00052
00056 class PathContainer
00057 {
00058 public:
00059 PathContainer()
00060 {
00061 ros::NodeHandle private_nh("~");
00062
00063 private_nh.param("target_frame_name", p_target_frame_name_, std::string("map"));
00064 private_nh.param("source_frame_name", p_source_frame_name_, std::string("base_link"));
00065 private_nh.param("trajectory_update_rate", p_trajectory_update_rate_, 4.0);
00066 private_nh.param("trajectory_publish_rate", p_trajectory_publish_rate_, 0.25);
00067
00068 ros::NodeHandle nh;
00069 sys_cmd_sub_ = nh.subscribe("syscommand", 1, &PathContainer::sysCmdCallback, this);
00070 trajectory_pub_ = nh.advertise<nav_msgs::Path>("trajectory",1, true);
00071
00072 trajectory_provider_service_ = nh.advertiseService("trajectory", &PathContainer::trajectoryProviderCallBack, this);
00073 recovery_info_provider_service_ = nh.advertiseService("trajectory_recovery_info", &PathContainer::recoveryInfoProviderCallBack, this);
00074
00075 last_reset_time_ = ros::Time::now();
00076
00077 update_trajectory_timer_ = private_nh.createTimer(ros::Duration(1.0 / p_trajectory_update_rate_), &PathContainer::trajectoryUpdateTimerCallback, this, false);
00078 publish_trajectory_timer_ = private_nh.createTimer(ros::Duration(1.0 / p_trajectory_publish_rate_), &PathContainer::publishTrajectoryTimerCallback, this, false);
00079
00080 pose_source_.pose.orientation.w = 1.0;
00081 pose_source_.header.frame_id = p_source_frame_name_;
00082
00083 trajectory_.trajectory.header.frame_id = p_target_frame_name_;
00084 }
00085
00086 void sysCmdCallback(const std_msgs::String& sys_cmd)
00087 {
00088 if (sys_cmd.data == "reset"){
00089 last_reset_time_ = ros::Time::now();
00090 trajectory_.trajectory.poses.clear();
00091 trajectory_.trajectory.header.stamp = ros::Time::now();
00092 }
00093 }
00094
00095 void addCurrentTfPoseToTrajectory()
00096 {
00097 pose_source_.header.stamp = ros::Time(0);
00098
00099 geometry_msgs::PoseStamped pose_out;
00100
00101 tf_.transformPose(p_target_frame_name_, pose_source_, pose_out);
00102
00103 if (trajectory_.trajectory.poses.size() != 0){
00104
00105 if (pose_out.header.stamp != trajectory_.trajectory.poses.back().header.stamp){
00106 trajectory_.trajectory.poses.push_back(pose_out);
00107 }
00108 }else{
00109 trajectory_.trajectory.poses.push_back(pose_out);
00110 }
00111
00112 trajectory_.trajectory.header.stamp = pose_out.header.stamp;
00113 }
00114
00115 void trajectoryUpdateTimerCallback(const ros::TimerEvent& event)
00116 {
00117
00118 try{
00119 addCurrentTfPoseToTrajectory();
00120 }catch(tf::TransformException e)
00121 {
00122 ROS_WARN("Trajectory Server: Transform from %s to %s failed: %s \n", p_target_frame_name_.c_str(), pose_source_.header.frame_id.c_str(), e.what() );
00123 }
00124 }
00125
00126 void publishTrajectoryTimerCallback(const ros::TimerEvent& event)
00127 {
00128 trajectory_pub_.publish(trajectory_.trajectory);
00129 }
00130
00131 bool trajectoryProviderCallBack(hector_nav_msgs::GetRobotTrajectory::Request &req,
00132 hector_nav_msgs::GetRobotTrajectory::Response &res )
00133 {
00134 res = trajectory_;
00135
00136 return true;
00137 };
00138
00139 bool recoveryInfoProviderCallBack(hector_nav_msgs::GetRecoveryInfo::Request &req,
00140 hector_nav_msgs::GetRecoveryInfo::Response &res )
00141 {
00142 const ros::Time req_time = req.request_time;
00143
00144 geometry_msgs::PoseStamped tmp;
00145 tmp.header.stamp = req_time;
00146
00147 std::vector<geometry_msgs::PoseStamped>& poses = trajectory_.trajectory.poses;
00148
00149
00150 std::vector<geometry_msgs::PoseStamped>::iterator it = std::lower_bound(poses.begin(), poses.end(), tmp, comparePoseStampedStamps);
00151
00152
00153 if (it == poses.end()){
00154 addCurrentTfPoseToTrajectory();
00155 it = poses.end();
00156 --it;
00157 }
00158
00159 std::vector<geometry_msgs::PoseStamped>::iterator it_start = it;
00160
00161 const geometry_msgs::Point& req_coords ((*it).pose.position);
00162
00163 double dist_sqr_threshold = req.request_radius * req.request_radius;
00164
00165 double dist_sqr = 0.0;
00166
00167
00168 while (it != poses.begin() && dist_sqr < dist_sqr_threshold){
00169 const geometry_msgs::Point& curr_coords ((*it).pose.position);
00170
00171 dist_sqr = (req_coords.x - curr_coords.x) * (req_coords.x - curr_coords.x) +
00172 (req_coords.y - curr_coords.y) * (req_coords.y - curr_coords.y);
00173
00174 --it;
00175 }
00176
00177 if (dist_sqr < dist_sqr_threshold){
00178 ROS_INFO("Failed to find trajectory leading out of radius %f", req.request_radius);
00179 return false;
00180 }
00181
00182 std::vector<geometry_msgs::PoseStamped>::iterator it_end = it;
00183
00184 res.req_pose = *it_start;
00185 res.radius_entry_pose = *it_end;
00186
00187 std::vector<geometry_msgs::PoseStamped>& traj_out_poses = res.trajectory_radius_entry_pose_to_req_pose.poses;
00188
00189 res.trajectory_radius_entry_pose_to_req_pose.poses.clear();
00190 res.trajectory_radius_entry_pose_to_req_pose.header = res.req_pose.header;
00191
00192 for (std::vector<geometry_msgs::PoseStamped>::iterator it_tmp = it_start; it_tmp != it_end; --it_tmp){
00193 traj_out_poses.push_back(*it_tmp);
00194 }
00195
00196 return true;
00197 };
00198
00199
00200 std::string p_target_frame_name_;
00201 std::string p_source_frame_name_;
00202 double p_trajectory_update_rate_;
00203 double p_trajectory_publish_rate_;
00204
00205
00206 geometry_msgs::PoseStamped pose_source_;
00207
00208 ros::ServiceServer trajectory_provider_service_;
00209 ros::ServiceServer recovery_info_provider_service_;
00210
00211 ros::Timer update_trajectory_timer_;
00212 ros::Timer publish_trajectory_timer_;
00213
00214
00215
00216 ros::Subscriber sys_cmd_sub_;
00217 ros::Publisher trajectory_pub_;
00218
00219 hector_nav_msgs::GetRobotTrajectory::Response trajectory_;
00220
00221 tf::TransformListener tf_;
00222
00223 ros::Time last_reset_time_;
00224 ros::Time last_pose_save_time_;
00225 };
00226
00227
00228
00229 int main(int argc, char** argv)
00230 {
00231 ros::init(argc, argv, "hector_trajectory_server");
00232
00233 PathContainer pc;
00234
00235 ros::spin();
00236
00237 return 0;
00238 }