initial_pose_tool.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2012, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 #include <tf/transform_listener.h>
31 
32 #include <geometry_msgs/PoseWithCovarianceStamped.h>
33 
34 #include "rviz/display_context.h"
37 
39 
40 namespace rviz
41 {
43 {
44  shortcut_key_ = 'p';
45 
47  new StringProperty("Topic", "initialpose", "The topic on which to publish initial pose estimates.",
48  getPropertyContainer(), SLOT(updateTopic()), this);
49  std_dev_x_ = new FloatProperty("X std deviation", 0.5, "X standard deviation for initial pose [m]",
51  std_dev_y_ = new FloatProperty("Y std deviation", 0.5, "Y standard deviation for initial pose [m]",
54  new FloatProperty("Theta std deviation", M_PI / 12.0,
55  "Theta standard deviation for initial pose [rad]", getPropertyContainer());
56  std_dev_x_->setMin(0);
57  std_dev_y_->setMin(0);
59 }
60 
62 {
64  setName("2D Pose Estimate");
65  updateTopic();
66 }
67 
69 {
70  try
71  {
72  pub_ = nh_.advertise<geometry_msgs::PoseWithCovarianceStamped>(topic_property_->getStdString(), 1);
73  }
74  catch (const ros::Exception& e)
75  {
76  ROS_ERROR_STREAM_NAMED("InitialPoseTool", e.what());
77  }
78 }
79 
80 void InitialPoseTool::onPoseSet(double x, double y, double theta)
81 {
82  std::string fixed_frame = context_->getFixedFrame().toStdString();
83  geometry_msgs::PoseWithCovarianceStamped pose;
84  pose.header.frame_id = fixed_frame;
85  pose.header.stamp = ros::Time::now();
86  pose.pose.pose.position.x = x;
87  pose.pose.pose.position.y = y;
88 
89  tf::Quaternion quat;
90  quat.setRPY(0.0, 0.0, theta);
91  tf::quaternionTFToMsg(quat, pose.pose.pose.orientation);
92  pose.pose.covariance[6 * 0 + 0] = std::pow(std_dev_x_->getFloat(), 2);
93  pose.pose.covariance[6 * 1 + 1] = std::pow(std_dev_y_->getFloat(), 2);
94  pose.pose.covariance[6 * 5 + 5] = std::pow(std_dev_theta_->getFloat(), 2);
95  ROS_INFO("Setting pose: %.3f %.3f %.3f [frame=%s]", x, y, theta, fixed_frame.c_str());
96  pub_.publish(pose);
97 }
98 
99 } // end namespace rviz
100 
void setMin(float min)
#define ROS_ERROR_STREAM_NAMED(name, args)
virtual Property * getPropertyContainer() const
Return the container for properties of this Tool.
Definition: tool.h:76
Property specialized to enforce floating point max/min.
char shortcut_key_
Definition: tool.h:209
std::string getStdString()
void publish(const boost::shared_ptr< M > &message) const
void setRPY(const tfScalar &roll, const tfScalar &pitch, const tfScalar &yaw)
#define ROS_INFO(...)
static void quaternionTFToMsg(const Quaternion &bt, geometry_msgs::Quaternion &msg)
void onPoseSet(double x, double y, double theta) override
void setName(const QString &name)
Set the name of the tool.
Definition: tool.cpp:72
FloatProperty * std_dev_theta_
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.
DisplayContext * context_
Definition: tool.h:207
virtual float getFloat() const
static Time now()
FloatProperty * std_dev_x_
void onInitialize() override
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
FloatProperty * std_dev_y_
StringProperty * topic_property_
void onInitialize() override
Definition: pose_tool.cpp:54


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Mon Jan 9 2023 03:07:20