imarker_simple.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2017, PickNik Consulting
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of PickNik Consulting nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Dave Coleman
36  Desc: Use interactive markers in a C++ class via the external python node
37 */
38 
41 #include <string>
42 
43 namespace rviz_visual_tools
44 {
45 IMarkerSimple::IMarkerSimple(const std::string& name, double scale, const geometry_msgs::Pose& initial_pose)
46  : nh_("~"), latest_pose_(initial_pose)
47 {
48  // Create Marker Server
49  const std::string imarker_topic = nh_.getNamespace() + "/" + name;
50  imarker_server_.reset(new interactive_markers::InteractiveMarkerServer(imarker_topic, "", false));
51 
52  // ros::Duration(2.0).sleep();
53 
54  // Create imarker
56 
57  // Send imarker to Rviz
58  imarker_server_->applyChanges();
59 }
60 
61 geometry_msgs::Pose& IMarkerSimple::getPose()
62 {
63  return latest_pose_;
64 }
65 
66 void IMarkerSimple::setPose(const Eigen::Affine3d& pose)
67 {
68  geometry_msgs::Pose pose_msg;
70  setPose(pose_msg);
71 }
72 
73 void IMarkerSimple::setPose(const geometry_msgs::Pose& pose)
74 {
75  latest_pose_ = pose;
77 }
78 
79 void IMarkerSimple::iMarkerCallback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback)
80 {
81  // Ignore if not pose update
82  if (feedback->event_type != visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE)
83  {
84  return;
85  }
86 
87  latest_pose_ = feedback->pose;
88 
89  // Redirect to base class
91  imarker_callback_(feedback);
92 }
93 
95 {
97  imarker_server_->applyChanges();
98 }
99 
100 void IMarkerSimple::make6DofMarker(const geometry_msgs::Pose& pose, double scale)
101 {
102  ROS_INFO_STREAM_NAMED(name_, "Making 6dof interactive marker named " << name_);
103 
104  int_marker_.header.frame_id = "world";
105  int_marker_.pose = pose;
106  int_marker_.scale = scale;
107 
108  int_marker_.name = name_;
109 
110  // int_marker_.controls[0].interaction_mode = InteractiveMarkerControl::MENU;
111 
112  InteractiveMarkerControl control;
113  control.orientation.w = 1;
114  control.orientation.x = 1;
115  control.orientation.y = 0;
116  control.orientation.z = 0;
117  control.name = "rotate_x";
118  control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
119  int_marker_.controls.push_back(control);
120  control.name = "move_x";
121  control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
122  int_marker_.controls.push_back(control);
123 
124  control.orientation.w = 1;
125  control.orientation.x = 0;
126  control.orientation.y = 1;
127  control.orientation.z = 0;
128  control.name = "rotate_z";
129  control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
130  int_marker_.controls.push_back(control);
131  control.name = "move_z";
132  control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
133  int_marker_.controls.push_back(control);
134 
135  control.orientation.w = 1;
136  control.orientation.x = 0;
137  control.orientation.y = 0;
138  control.orientation.z = 1;
139  control.name = "rotate_y";
140  control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
141  int_marker_.controls.push_back(control);
142  control.name = "move_y";
143  control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
144  int_marker_.controls.push_back(control);
145 
146  imarker_server_->insert(int_marker_);
147  imarker_server_->setCallback(int_marker_.name, boost::bind(&IMarkerSimple::iMarkerCallback, this, _1));
148  // menu_handler_.apply(*imarker_server_, int_marker_.name);
149 }
150 
151 } // namespace rviz_visual_tools
visualization_msgs::InteractiveMarker int_marker_
#define ROS_INFO_STREAM_NAMED(name, args)
geometry_msgs::Pose latest_pose_
std::shared_ptr< interactive_markers::InteractiveMarkerServer > imarker_server_
const std::string & getNamespace() const
void setPose(const Eigen::Affine3d &pose)
void iMarkerCallback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
static void convertPoseSafe(const Eigen::Affine3d &pose, geometry_msgs::Pose &pose_msg)
Convert an Eigen pose to a geometry_msg pose - thread safe.
IMarkerSimple(const std::string &name="imarker", double scale=0.2, const geometry_msgs::Pose &initial_pose=getIdentityPose())
geometry_msgs::Pose & getPose()
void make6DofMarker(const geometry_msgs::Pose &pose, double scale=0.2)


rviz_visual_tools
Author(s): Dave Coleman
autogenerated on Mon Jun 10 2019 15:05:18