33 #include "nav_msgs/Path.h" 34 #include "std_msgs/String.h" 36 #include <geometry_msgs/Quaternion.h> 37 #include <geometry_msgs/PoseStamped.h> 41 #include <hector_nav_msgs/GetRobotTrajectory.h> 42 #include <hector_nav_msgs/GetRecoveryInfo.h> 50 bool comparePoseStampedStamps (
const geometry_msgs::PoseStamped& t1,
const geometry_msgs::PoseStamped& t2) {
return (t1.header.stamp < t2.header.stamp); }
63 private_nh.
param(
"target_frame_name", p_target_frame_name_, std::string(
"map"));
64 private_nh.
param(
"source_frame_name", p_source_frame_name_, std::string(
"base_link"));
65 private_nh.
param(
"trajectory_update_rate", p_trajectory_update_rate_, 4.0);
66 private_nh.
param(
"trajectory_publish_rate", p_trajectory_publish_rate_, 0.25);
72 trajectory_pub_ = nh.
advertise<nav_msgs::Path>(
"trajectory",1,
true);
82 pose_source_.pose.orientation.w = 1.0;
83 pose_source_.header.frame_id = p_source_frame_name_;
85 trajectory_.trajectory.header.frame_id = p_target_frame_name_;
91 ROS_INFO(
"Waiting for tf transform data between frames %s and %s to become available", p_target_frame_name_.c_str(), p_source_frame_name_.c_str() );
93 bool transform_successful =
false;
95 while (!transform_successful){
96 transform_successful = tf_.canTransform(p_target_frame_name_, p_source_frame_name_,
ros::Time());
97 if (transform_successful)
break;
101 if ((now-start).toSec() > 20.0){
102 ROS_WARN_ONCE(
"No transform between frames %s and %s available after %f seconds of waiting. This warning only prints once.", p_target_frame_name_.c_str(), p_source_frame_name_.c_str(), (now-start).toSec());
110 ROS_INFO(
"Finished waiting for tf, waited %f seconds", (end-start).toSec());
116 if (sys_cmd.data ==
"reset")
119 trajectory_.trajectory.poses.clear();
126 pose_source_.header.stamp =
ros::Time(0);
128 geometry_msgs::PoseStamped pose_out;
130 tf_.transformPose(p_target_frame_name_, pose_source_, pose_out);
132 if (trajectory_.trajectory.poses.size() != 0){
134 if (pose_out.header.stamp != trajectory_.trajectory.poses.back().header.stamp){
135 trajectory_.trajectory.poses.push_back(pose_out);
138 trajectory_.trajectory.poses.push_back(pose_out);
141 trajectory_.trajectory.header.stamp = pose_out.header.stamp;
148 addCurrentTfPoseToTrajectory();
151 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() );
157 trajectory_pub_.publish(trajectory_.trajectory);
161 hector_nav_msgs::GetRobotTrajectory::Response &res )
163 res = getTrajectory();
167 inline const hector_nav_msgs::GetRobotTrajectoryResponse
getTrajectory()
const 173 hector_nav_msgs::GetRecoveryInfo::Response &res )
175 const ros::Time req_time = req.request_time;
177 geometry_msgs::PoseStamped tmp;
178 tmp.header.stamp = req_time;
180 std::vector<geometry_msgs::PoseStamped>
const & poses = trajectory_.trajectory.poses;
182 if(poses.size() == 0)
184 ROS_WARN(
"Failed to find trajectory leading out of radius %f" 185 " because no poses, i.e. no inverse trajectory, exists.", req.request_radius);
190 std::vector<geometry_msgs::PoseStamped>::const_iterator it
194 if (it == poses.end()){
195 addCurrentTfPoseToTrajectory();
200 std::vector<geometry_msgs::PoseStamped>::const_iterator it_start = it;
202 const geometry_msgs::Point& req_coords ((*it).pose.position);
204 double dist_sqr_threshold = req.request_radius * req.request_radius;
206 double dist_sqr = 0.0;
209 while (it != poses.begin() && dist_sqr < dist_sqr_threshold){
210 const geometry_msgs::Point& curr_coords ((*it).pose.position);
212 dist_sqr = (req_coords.x - curr_coords.x) * (req_coords.x - curr_coords.x) +
213 (req_coords.y - curr_coords.y) * (req_coords.y - curr_coords.y);
218 if (dist_sqr < dist_sqr_threshold){
219 ROS_INFO(
"Failed to find trajectory leading out of radius %f", req.request_radius);
223 std::vector<geometry_msgs::PoseStamped>::const_iterator it_end = it;
225 res.req_pose = *it_start;
226 res.radius_entry_pose = *it_end;
228 std::vector<geometry_msgs::PoseStamped>& traj_out_poses = res.trajectory_radius_entry_pose_to_req_pose.poses;
230 res.trajectory_radius_entry_pose_to_req_pose.poses.clear();
231 res.trajectory_radius_entry_pose_to_req_pose.header = res.req_pose.header;
233 for (std::vector<geometry_msgs::PoseStamped>::const_iterator it_tmp = it_start; it_tmp != it_end; --it_tmp){
234 traj_out_poses.push_back(*it_tmp);
268 int main(
int argc,
char** argv)
270 ros::init(argc, argv,
"hector_trajectory_server");
void addCurrentTfPoseToTrajectory()
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
double p_trajectory_publish_rate_
ros::Time last_reset_time_
int main(int argc, char **argv)
const hector_nav_msgs::GetRobotTrajectoryResponse getTrajectory() const
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
void sysCmdCallback(const std_msgs::String &sys_cmd)
std::string p_target_frame_name_
ROSCPP_DECL void spin(Spinner &spinner)
double p_trajectory_update_rate_
bool trajectoryProviderCallBack(hector_nav_msgs::GetRobotTrajectory::Request &req, hector_nav_msgs::GetRobotTrajectory::Response &res)
#define ROS_WARN_ONCE(...)
void publishTrajectoryTimerCallback(const ros::TimerEvent &event)
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
tf::TransformListener tf_
void trajectoryUpdateTimerCallback(const ros::TimerEvent &event)
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
bool comparePoseStampedStamps(const geometry_msgs::PoseStamped &t1, const geometry_msgs::PoseStamped &t2)
ros::Timer publish_trajectory_timer_
ros::ServiceServer trajectory_provider_service_
std::string p_source_frame_name_
hector_nav_msgs::GetRobotTrajectory::Response trajectory_
bool recoveryInfoProviderCallBack(hector_nav_msgs::GetRecoveryInfo::Request &req, hector_nav_msgs::GetRecoveryInfo::Response &res)
ros::Subscriber sys_cmd_sub_
ros::Publisher trajectory_pub_
ros::Time last_pose_save_time_
geometry_msgs::PoseStamped pose_source_
ros::ServiceServer recovery_info_provider_service_
ros::Timer update_trajectory_timer_