40 #include <geometry_msgs/Twist.h> 41 #include <nav_msgs/Path.h> 87 pub_path_ = neonavigation_common::compat::advertise<nav_msgs::Path>(
95 float dist2d(geometry_msgs::Point& a, geometry_msgs::Point& b)
97 return sqrtf(powf(a.x - b.x, 2) + powf(a.y - b.y, 2));
104 path_.header.seq = 0;
119 ROS_WARN(
"TF exception: %s", e.what());
122 geometry_msgs::PoseStamped pose;
124 transform.getBasis().getRotation(q);
126 tf2::Vector3 origin = transform.getOrigin();
127 pose.pose.position.x = origin.x();
128 pose.pose.position.y = origin.y();
129 pose.pose.position.z = origin.z();
131 pose.header.stamp = now;
132 pose.header.seq =
path_.poses.size();
135 path_.header.stamp = now;
137 if (
path_.poses.size() == 0)
139 path_.poses.push_back(pose);
144 path_.poses.push_back(pose);
153 int main(
int argc,
char** argv)
155 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)
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)
ROSCPP_DECL void spinOnce()
void deprecatedParam(const ros::NodeHandle &nh, const std::string &key, T ¶m, const T &default_value)
tf2_ros::TransformListener tfl_