goal_tool.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, 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/PoseStamped.h>
33 
34 #include "rviz/display_context.h"
36 
38 
39 namespace rviz
40 {
41 
43 {
44  shortcut_key_ = 'g';
45 
46  topic_property_ = new StringProperty( "Topic", "goal",
47  "The topic on which to publish navigation goals.",
48  getPropertyContainer(), SLOT( updateTopic() ), this );
49 }
50 
52 {
54  setName( "2D Nav Goal" );
55  updateTopic();
56 }
57 
59 {
60  pub_ = nh_.advertise<geometry_msgs::PoseStamped>( topic_property_->getStdString(), 1 );
61 }
62 
63 void GoalTool::onPoseSet(double x, double y, double theta)
64 {
65  std::string fixed_frame = context_->getFixedFrame().toStdString();
66  tf::Quaternion quat;
67  quat.setRPY(0.0, 0.0, theta);
68  tf::Stamped<tf::Pose> p = tf::Stamped<tf::Pose>(tf::Pose(quat, tf::Point(x, y, 0.0)), ros::Time::now(), fixed_frame);
69  geometry_msgs::PoseStamped goal;
70  tf::poseStampedTFToMsg(p, goal);
71  ROS_INFO("Setting goal: Frame:%s, Position(%.3f, %.3f, %.3f), Orientation(%.3f, %.3f, %.3f, %.3f) = Angle: %.3f\n", fixed_frame.c_str(),
72  goal.pose.position.x, goal.pose.position.y, goal.pose.position.z,
73  goal.pose.orientation.x, goal.pose.orientation.y, goal.pose.orientation.z, goal.pose.orientation.w, theta);
74  pub_.publish(goal);
75 }
76 
77 } // end namespace rviz
78 
ros::Publisher pub_
Definition: goal_tool.h:63
virtual void onPoseSet(double x, double y, double theta)
Definition: goal_tool.cpp:63
StringProperty * topic_property_
Definition: goal_tool.h:65
void publish(const boost::shared_ptr< M > &message) const
static void poseStampedTFToMsg(const Stamped< Pose > &bt, geometry_msgs::PoseStamped &msg)
virtual void onInitialize()
Definition: goal_tool.cpp:51
char shortcut_key_
Definition: tool.h:177
std::string getStdString()
tf::Transform Pose
void setRPY(const tfScalar &roll, const tfScalar &pitch, const tfScalar &yaw)
#define ROS_INFO(...)
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 QString getFixedFrame() const =0
Return the fixed frame name.
DisplayContext * context_
Definition: tool.h:175
void updateTopic()
Definition: goal_tool.cpp:58
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)
ros::NodeHandle nh_
Definition: goal_tool.h:62


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Wed Aug 28 2019 04:01:51