32 #include <geometry_msgs/PoseWithCovarianceStamped.h> 47 "The topic on which to publish initial pose estimates.",
66 geometry_msgs::PoseWithCovarianceStamped pose;
67 pose.header.frame_id = fixed_frame;
69 pose.pose.pose.position.x = x;
70 pose.pose.pose.position.y = y;
73 quat.
setRPY(0.0, 0.0, theta);
75 pose.pose.pose.orientation);
76 pose.pose.covariance[6*0+0] = 0.5 * 0.5;
77 pose.pose.covariance[6*1+1] = 0.5 * 0.5;
78 pose.pose.covariance[6*5+5] = M_PI/12.0 * M_PI/12.0;
79 ROS_INFO(
"Setting pose: %.3f %.3f %.3f [frame=%s]", x, y, theta, fixed_frame.c_str());
void publish(const boost::shared_ptr< M > &message) const
std::string getStdString()
void setRPY(const tfScalar &roll, const tfScalar &pitch, const tfScalar &yaw)
static void quaternionTFToMsg(const Quaternion &bt, geometry_msgs::Quaternion &msg)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
Property specialized for string values.
virtual QString getFixedFrame() const =0
Return the fixed frame name.
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)