43 #include <geometry_msgs/Twist.h> 44 #include <nav_msgs/Path.h> 47 #include <std_srvs/Empty.h> 59 bool clearPath(std_srvs::Empty::Request& req,
60 std_srvs::Empty::Response& res);
92 pub_path_ = neonavigation_common::compat::advertise<nav_msgs::Path>(
102 float dist2d(geometry_msgs::Point& a, geometry_msgs::Point& b)
104 return std::sqrt(std::pow(a.x - b.x, 2) + std::pow(a.y - b.y, 2));
108 std_srvs::Empty::Response& )
118 path_.header.seq = 0;
133 ROS_WARN(
"TF exception: %s", e.what());
136 geometry_msgs::PoseStamped pose;
138 transform.getBasis().getRotation(q);
140 tf2::Vector3 origin = transform.getOrigin();
141 pose.pose.position.x = origin.x();
142 pose.pose.position.y = origin.y();
143 pose.pose.position.z = origin.z();
145 pose.header.stamp = now;
146 pose.header.seq =
path_.poses.size();
149 path_.header.stamp = now;
151 if (
path_.poses.size() == 0)
153 path_.poses.push_back(pose);
158 path_.poses.push_back(pose);
167 int main(
int argc,
char** argv)
169 ros::init(argc, argv,
"trajectory_recorder");
float dist2d(geometry_msgs::Point &a, geometry_msgs::Point &b)
void publish(const boost::shared_ptr< M > &message) const
int main(int argc, char **argv)
virtual geometry_msgs::TransformStamped lookupTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, const ros::Duration timeout) const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
ros::ServiceServer srs_clear_path_
std::string frame_global_
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
void fromMsg(const A &, B &b)
bool clearPath(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
ROSCPP_DECL void spinOnce()
void deprecatedParam(const ros::NodeHandle &nh, const std::string &key, T ¶m, const T &default_value)
tf2_ros::TransformListener tfl_