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
00030
00031
00032
00033
00034
00035
00036
00037
00038 #include <ros/ros.h>
00039
00040 #include <geometry_msgs/Twist.h>
00041 #include <nav_msgs/Path.h>
00042 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
00043 #include <tf2_ros/transform_listener.h>
00044
00045 #include <math.h>
00046 #include <string>
00047
00048 #include <neonavigation_common/compatibility.h>
00049
00050 class RecorderNode
00051 {
00052 public:
00053 RecorderNode();
00054 ~RecorderNode();
00055 void spin();
00056
00057 private:
00058 std::string topic_path_;
00059 std::string frame_robot_;
00060 std::string frame_global_;
00061 double dist_interval_;
00062 double ang_interval_;
00063 bool store_time_;
00064
00065 ros::NodeHandle nh_;
00066 ros::NodeHandle pnh_;
00067 ros::Publisher pub_path_;
00068 tf2_ros::Buffer tfbuf_;
00069 tf2_ros::TransformListener tfl_;
00070
00071 nav_msgs::Path path_;
00072 };
00073
00074 RecorderNode::RecorderNode()
00075 : nh_()
00076 , pnh_("~")
00077 , tfl_(tfbuf_)
00078 {
00079 neonavigation_common::compat::checkCompatMode();
00080 pnh_.param("frame_robot", frame_robot_, std::string("base_link"));
00081 pnh_.param("frame_global", frame_global_, std::string("map"));
00082 neonavigation_common::compat::deprecatedParam(pnh_, "path", topic_path_, std::string("recpath"));
00083 pnh_.param("dist_interval", dist_interval_, 0.3);
00084 pnh_.param("ang_interval", ang_interval_, 1.0);
00085 pnh_.param("store_time", store_time_, false);
00086
00087 pub_path_ = neonavigation_common::compat::advertise<nav_msgs::Path>(
00088 nh_, "path",
00089 pnh_, topic_path_, 10, true);
00090 }
00091 RecorderNode::~RecorderNode()
00092 {
00093 }
00094
00095 float dist2d(geometry_msgs::Point& a, geometry_msgs::Point& b)
00096 {
00097 return sqrtf(powf(a.x - b.x, 2) + powf(a.y - b.y, 2));
00098 }
00099
00100 void RecorderNode::spin()
00101 {
00102 ros::Rate loop_rate(50);
00103 path_.header.frame_id = frame_global_;
00104 path_.header.seq = 0;
00105
00106 while (ros::ok())
00107 {
00108 ros::Time now = ros::Time(0);
00109 if (store_time_)
00110 now = ros::Time::now();
00111 tf2::Stamped<tf2::Transform> transform;
00112 try
00113 {
00114 tf2::fromMsg(
00115 tfbuf_.lookupTransform(frame_global_, frame_robot_, now, ros::Duration(0.2)), transform);
00116 }
00117 catch (tf2::TransformException& e)
00118 {
00119 ROS_WARN("TF exception: %s", e.what());
00120 continue;
00121 }
00122 geometry_msgs::PoseStamped pose;
00123 tf2::Quaternion q;
00124 transform.getBasis().getRotation(q);
00125 pose.pose.orientation = tf2::toMsg(q);
00126 tf2::Vector3 origin = transform.getOrigin();
00127 pose.pose.position.x = origin.x();
00128 pose.pose.position.y = origin.y();
00129 pose.pose.position.z = origin.z();
00130 pose.header.frame_id = frame_global_;
00131 pose.header.stamp = now;
00132 pose.header.seq = path_.poses.size();
00133
00134 path_.header.seq++;
00135 path_.header.stamp = now;
00136
00137 if (path_.poses.size() == 0)
00138 {
00139 path_.poses.push_back(pose);
00140 pub_path_.publish(path_);
00141 }
00142 else if (dist2d(path_.poses.back().pose.position, pose.pose.position) > dist_interval_)
00143 {
00144 path_.poses.push_back(pose);
00145 pub_path_.publish(path_);
00146 }
00147
00148 ros::spinOnce();
00149 loop_rate.sleep();
00150 }
00151 }
00152
00153 int main(int argc, char** argv)
00154 {
00155 ros::init(argc, argv, "trajectory_recorder");
00156
00157 RecorderNode rec;
00158 rec.spin();
00159
00160 return 0;
00161 }