$search
00001 //================================================================================================= 00002 // Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt 00003 // All rights reserved. 00004 00005 // Redistribution and use in source and binary forms, with or without 00006 // modification, are permitted provided that the following conditions are met: 00007 // * Redistributions of source code must retain the above copyright 00008 // notice, this list of conditions and the following disclaimer. 00009 // * Redistributions in binary form must reproduce the above copyright 00010 // notice, this list of conditions and the following disclaimer in the 00011 // documentation and/or other materials provided with the distribution. 00012 // * Neither the name of the Simulation, Systems Optimization and Robotics 00013 // group, TU Darmstadt nor the names of its contributors may be used to 00014 // endorse or promote products derived from this software without 00015 // specific prior written permission. 00016 00017 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 00018 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 00019 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 00020 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY 00021 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 00022 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00023 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 00024 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 00025 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 00026 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 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 //Only add pose to trajectory if it's not already stored 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_ERROR("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 //Find the robot pose in the saved trajectory 00150 std::vector<geometry_msgs::PoseStamped>::iterator it = std::lower_bound(poses.begin(), poses.end(), tmp, comparePoseStampedStamps); 00151 00152 //If we didn't find the robot pose for the desired time, add the current robot pose to trajectory 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 //Iterate backwards till the start of the trajectory is reached or we find a pose that's outside the specified radius 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 //parameters 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 // Zero pose used for transformation to target_frame. 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 //ros::Subscriber pose_update_sub_; 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 }