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 <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.",
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 
90  geometry_msgs::Quaternion quat_msg;
91  tf2::Quaternion quat;
92  quat.setRPY(0.0, 0.0, theta);
93  pose.pose.pose.orientation = tf2::toMsg(quat);
94  pose.pose.covariance[6 * 0 + 0] = std::pow(std_dev_x_->getFloat(), 2);
95  pose.pose.covariance[6 * 1 + 1] = std::pow(std_dev_y_->getFloat(), 2);
96  pose.pose.covariance[6 * 5 + 5] = std::pow(std_dev_theta_->getFloat(), 2);
97  ROS_INFO("Setting pose: %.3f %.3f %.3f [frame=%s]", x, y, theta, fixed_frame.c_str());
98  pub_.publish(pose);
99 }
100 
101 } // end namespace rviz
102 
rviz::InitialPoseTool::std_dev_x_
FloatProperty * std_dev_x_
Definition: initial_pose_tool.h:69
rviz::Tool
Definition: tool.h:56
rviz::InitialPoseTool::std_dev_y_
FloatProperty * std_dev_y_
Definition: initial_pose_tool.h:70
rviz::InitialPoseTool::InitialPoseTool
InitialPoseTool()
Definition: initial_pose_tool.cpp:42
tf2::Quaternion::setRPY
void setRPY(const tf2Scalar &roll, const tf2Scalar &pitch, const tf2Scalar &yaw)
rviz::Tool::context_
DisplayContext * context_
Definition: tool.h:207
rviz::PoseTool::onInitialize
void onInitialize() override
Definition: pose_tool.cpp:54
float_property.h
ros::Exception
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
ros::NodeHandle::advertise
Publisher advertise(AdvertiseOptions &ops)
rviz::FloatProperty::setMin
void setMin(float min)
Definition: float_property.cpp:51
rviz::InitialPoseTool::updateTopic
void updateTopic()
Definition: initial_pose_tool.cpp:68
rviz::FloatProperty
Property specialized to enforce floating point max/min.
Definition: float_property.h:37
PLUGINLIB_EXPORT_CLASS
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
rviz::InitialPoseTool::std_dev_theta_
FloatProperty * std_dev_theta_
Definition: initial_pose_tool.h:71
rviz::FloatProperty::getFloat
virtual float getFloat() const
Definition: float_property.h:79
rviz::Tool::shortcut_key_
char shortcut_key_
Definition: tool.h:209
rviz
Definition: add_display_dialog.cpp:54
ROS_ERROR_STREAM_NAMED
#define ROS_ERROR_STREAM_NAMED(name, args)
rviz::StringProperty
Property specialized for string values.
Definition: string_property.h:39
Quaternion.h
rviz::StringProperty::getStdString
std::string getStdString()
Definition: string_property.h:71
initial_pose_tool.h
rviz::DisplayContext::getFixedFrame
virtual QString getFixedFrame() const =0
Return the fixed frame name.
rviz::Tool::getPropertyContainer
virtual Property * getPropertyContainer() const
Return the container for properties of this Tool.
Definition: tool.h:76
string_property.h
rviz::InitialPoseTool::onPoseSet
void onPoseSet(double x, double y, double theta) override
Definition: initial_pose_tool.cpp:80
rviz::InitialPoseTool::onInitialize
void onInitialize() override
Definition: initial_pose_tool.cpp:61
rviz::Tool::setName
void setName(const QString &name)
Set the name of the tool.
Definition: tool.cpp:72
class_list_macros.hpp
tf2::Quaternion
tf2_geometry_msgs.h
tf2::toMsg
B toMsg(const A &a)
rviz::InitialPoseTool
Definition: initial_pose_tool.h:48
rviz::InitialPoseTool::nh_
ros::NodeHandle nh_
Definition: initial_pose_tool.h:65
display_context.h
ROS_INFO
#define ROS_INFO(...)
rviz::InitialPoseTool::topic_property_
StringProperty * topic_property_
Definition: initial_pose_tool.h:68
rviz::InitialPoseTool::pub_
ros::Publisher pub_
Definition: initial_pose_tool.h:66
ros::Time::now
static Time now()


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust, William Woodall
autogenerated on Fri Aug 2 2024 08:43:09