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"
36 
38 
39 namespace rviz
40 {
41 
43 {
44  shortcut_key_ = 'p';
45 
46  topic_property_ = new StringProperty( "Topic", "initialpose",
47  "The topic on which to publish initial pose estimates.",
48  getPropertyContainer(), SLOT( updateTopic() ), this );
49 }
50 
52 {
54  setName( "2D Pose Estimate" );
55  updateTopic();
56 }
57 
59 {
60  pub_ = nh_.advertise<geometry_msgs::PoseWithCovarianceStamped>( topic_property_->getStdString(), 1 );
61 }
62 
63 void InitialPoseTool::onPoseSet(double x, double y, double theta)
64 {
65  std::string fixed_frame = context_->getFixedFrame().toStdString();
66  geometry_msgs::PoseWithCovarianceStamped pose;
67  pose.header.frame_id = fixed_frame;
68  pose.header.stamp = ros::Time::now();
69  pose.pose.pose.position.x = x;
70  pose.pose.pose.position.y = y;
71 
72  tf::Quaternion quat;
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());
80  pub_.publish(pose);
81 }
82 
83 } // end namespace rviz
84 
void publish(const boost::shared_ptr< M > &message) const
char shortcut_key_
Definition: tool.h:177
std::string getStdString()
void setRPY(const tfScalar &roll, const tfScalar &pitch, const tfScalar &yaw)
#define ROS_INFO(...)
static void quaternionTFToMsg(const Quaternion &bt, geometry_msgs::Quaternion &msg)
virtual void onInitialize()
Definition: pose_tool.cpp:57
void setName(const QString &name)
Set the name of the tool.
Definition: tool.cpp:74
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
Property specialized for string values.
virtual void onPoseSet(double x, double y, double theta)
virtual QString getFixedFrame() const =0
Return the fixed frame name.
DisplayContext * context_
Definition: tool.h:175
static Time now()
virtual Property * getPropertyContainer() const
Return the container for properties of this Tool.
Definition: tool.h:75
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
StringProperty * topic_property_


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Sat Apr 27 2019 02:33:41