moveit_visual_tools_demo.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2015, University of Colorado, Boulder
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 the Univ of CO, Boulder 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: Demo implementation of moveit_visual_tools
37  To use, add a Rviz Marker Display subscribed to topic /moveit_visual_tools
38 */
39 
40 // ROS
41 #include <ros/ros.h>
42 
43 // For visualizing things in rviz
45 
46 // MoveIt
48 
49 // C++
50 #include <string>
51 
52 namespace rvt = rviz_visual_tools;
53 
54 namespace moveit_visual_tools
55 {
56 static const std::string PLANNING_GROUP_NAME = "arm"; // RRBot Specific
57 static const std::string THIS_PACKAGE = "moveit_visual_tools";
58 
60 {
61 public:
66  {
67  visual_tools_.reset(new moveit_visual_tools::MoveItVisualTools("world", "/moveit_visual_tools"));
68  visual_tools_->loadPlanningSceneMonitor();
69  visual_tools_->loadMarkerPub(true);
70  visual_tools_->loadRobotStatePub("display_robot_state");
71  visual_tools_->setManualSceneUpdating();
72 
73  robot_state_ = visual_tools_->getSharedRobotState();
74  jmg_ = robot_state_->getJointModelGroup(PLANNING_GROUP_NAME);
75 
76  // Allow time to publish messages
77  ros::spinOnce();
78  ros::Duration(0.1).sleep();
79 
80  // Clear collision objects and markers
81  visual_tools_->deleteAllMarkers();
82  visual_tools_->removeAllCollisionObjects();
83  visual_tools_->triggerPlanningSceneUpdate();
84  ros::Duration(0.1).sleep();
85 
86  // Show message
87  Eigen::Affine3d text_pose = Eigen::Affine3d::Identity();
88  text_pose.translation().z() = 4;
89  visual_tools_->publishText(text_pose, "MoveIt! Visual Tools", rvt::WHITE, rvt::XLARGE, /*static_id*/ false);
90 
92 
93  runDeskTest();
94 
96  }
97 
98  void publishLabelHelper(const Eigen::Affine3d& pose, const std::string& label)
99  {
100  Eigen::Affine3d pose_copy = pose;
101  pose_copy.translation().x() -= 0.2;
102  visual_tools_->publishText(pose_copy, label, rvt::WHITE, rvt::LARGE, false);
103  }
104 
106  {
107  // Show 5 random robot states
108  for (std::size_t i = 0; i < 5; ++i)
109  {
110  robot_state_->setToRandomPositions(jmg_);
111  visual_tools_->publishRobotState(robot_state_, rvt::DEFAULT);
112  ros::Duration(0.1).sleep();
113  }
114 
115  // Show 5 robot state in different colors
116  for (std::size_t i = 0; i < 5; ++i)
117  {
118  robot_state_->setToRandomPositions(jmg_);
119  visual_tools_->publishRobotState(robot_state_, visual_tools_->getRandColor());
120  ros::Duration(0.1).sleep();
121  }
122 
123  // Hide the robot
124  visual_tools_->hideRobot();
125  ros::Duration(0.1).sleep();
126 
127  // Show the robot
128  visual_tools_->publishRobotState(robot_state_, rvt::DEFAULT);
129  ros::Duration(0.1).sleep();
130  }
131 
132  void runDeskTest()
133  {
134  double common_angle = M_PI * 1.1;
135  double x_offset = -3.0;
136 
137  // --------------------------------------------------------------------
138  ROS_INFO_STREAM_NAMED("visual_tools", "Moving robot");
139  Eigen::Affine3d robot_pose = Eigen::Affine3d::Identity();
140  robot_pose = robot_pose * Eigen::AngleAxisd(common_angle, Eigen::Vector3d::UnitZ());
141  robot_pose.translation().x() = x_offset;
142  visual_tools_->applyVirtualJointTransform(*robot_state_, robot_pose);
143  visual_tools_->publishRobotState(robot_state_, rvt::DEFAULT);
144 
145  // --------------------------------------------------------------------
146  ROS_INFO_STREAM_NAMED("visual_tools", "Publishing Collision Floor");
147  visual_tools_->publishCollisionFloor(-0.5, "Floor", rvt::GREY);
148 
149  // --------------------------------------------------------------------
150  ROS_INFO_STREAM_NAMED("visual_tools", "Publishing Collision Wall");
151  double wall_x = x_offset - 1.0;
152  double wall_y = -1.0;
153  double wall_width = 6.0;
154  double wall_height = 4;
155  visual_tools_->publishCollisionWall(wall_x, wall_y, common_angle, wall_width, wall_height, "Wall", rvt::GREEN);
156 
157  // --------------------------------------------------------------------
158  ROS_INFO_STREAM_NAMED("visual_tools", "Publishing Collision Table");
159  double table_x = x_offset + 1.0;
160  double table_y = 0;
161  double table_z = 0;
162  double table_width = 3;
163  double table_height = 1;
164  double table_depth = 1;
165  visual_tools_->publishCollisionTable(table_x, table_y, table_z, common_angle, table_width, table_height,
166  table_depth, "Table", rvt::BLUE);
167 
168  // Send ROS messages
169  visual_tools_->triggerPlanningSceneUpdate();
170 
171  // Show 5 random robot states
172  for (std::size_t i = 0; i < 5; ++i)
173  {
174  robot_state_->setToRandomPositions(jmg_);
175  visual_tools_->publishRobotState(robot_state_, rvt::DEFAULT);
176  ros::Duration(0.1).sleep();
177  }
178  }
179 
181  {
182  // Create pose
183  Eigen::Affine3d pose1 = Eigen::Affine3d::Identity();
184  Eigen::Affine3d pose2 = Eigen::Affine3d::Identity();
185  // geometry_msgs::Pose pose1;
186  // geometry_msgs::Pose pose2;
187 
188  double space_between_rows = 0.2;
189  double y = 0;
190  double step;
191 
192  // --------------------------------------------------------------------
193  ROS_INFO_STREAM_NAMED("visual_tools", "Publishing Collision Mesh");
194  std::string file_path = "file://" + ros::package::getPath(THIS_PACKAGE);
195  if (file_path == "file://")
196  ROS_FATAL_STREAM_NAMED("visual_tools", "Unable to get " << THIS_PACKAGE << " package path ");
197  file_path.append("/resources/demo_mesh.stl");
198  step = 0.4;
199  for (double i = 0; i <= 1.0; i += 0.1)
200  {
201  visual_tools_->publishCollisionMesh(visual_tools_->convertPose(pose1),
202  "Mesh_" + boost::lexical_cast<std::string>(i), file_path,
203  visual_tools_->getRandColor());
204  if (!i)
205  publishLabelHelper(pose1, "Mesh");
206  pose1.translation().x() += step;
207  }
208 
209  // --------------------------------------------------------------------
210  ROS_INFO_STREAM_NAMED("visual_tools", "Publishing Collision Block");
211  pose1 = Eigen::Affine3d::Identity();
212  y += space_between_rows * 2.0;
213  pose1.translation().y() = y;
214  for (double i = 0; i <= 1.0; i += 0.1)
215  {
216  double size = 0.1 * (i + 0.5);
217  visual_tools_->publishCollisionBlock(visual_tools_->convertPose(pose1),
218  "Block_" + boost::lexical_cast<std::string>(i), size,
219  visual_tools_->getRandColor());
220 
221  if (!i)
222  publishLabelHelper(pose1, "Block");
223  pose1.translation().x() += step;
224  }
225 
226  // --------------------------------------------------------------------
227  ROS_INFO_STREAM_NAMED("visual_tools", "Publishing Collision Rectanglular Cuboid");
228  double cuboid_min_size = 0.05;
229  double cuboid_max_size = 0.2;
230  pose1 = Eigen::Affine3d::Identity();
231  pose2 = Eigen::Affine3d::Identity();
232  y += space_between_rows * 2.0;
233  pose1.translation().y() = y;
234  pose2.translation().y() = y;
235  for (double i = 0; i <= 1.0; i += 0.1)
236  {
237  pose2 = pose1;
238  pose2.translation().x() += i * cuboid_max_size + cuboid_min_size;
239  pose2.translation().y() += (i * cuboid_max_size + cuboid_min_size) / 2.0;
240  pose2.translation().z() += i * cuboid_max_size + cuboid_min_size;
241  visual_tools_->publishCollisionCuboid(pose1.translation(), pose2.translation(),
242  "Rectangle_" + boost::lexical_cast<std::string>(i),
243  visual_tools_->getRandColor());
244  if (!i)
245  publishLabelHelper(pose1, "Cuboid");
246  pose1.translation().x() += step;
247  }
248 
249  // --------------------------------------------------------------------
250  ROS_INFO_STREAM_NAMED("visual_tools", "Publishing Collision Cylinder");
251  double cylinder_min_size = 0.01;
252  double cylinder_max_size = 0.3;
253  pose1 = Eigen::Affine3d::Identity();
254  pose2 = Eigen::Affine3d::Identity();
255  y += space_between_rows * 2.0;
256  pose1.translation().y() = y;
257  pose2.translation().y() = y;
258  for (double i = 0; i <= 1.0; i += 0.1)
259  {
260  double radius = 0.1;
261  pose2 = pose1;
262  pose2.translation().x() += i * cylinder_max_size + cylinder_min_size;
263  // pose2.translation().y() += i * cylinder_max_size + cylinder_min_size;
264  // pose2.translation().z() += i * cylinder_max_size + cylinder_min_size;
265  visual_tools_->publishCollisionCylinder(pose1.translation(), pose2.translation(),
266  "Cylinder_" + boost::lexical_cast<std::string>(i), radius,
267  visual_tools_->getRandColor());
268 
269  if (!i)
270  publishLabelHelper(pose1, "Cylinder");
271  pose1.translation().x() += step;
272 
273  pose1 = pose1 * Eigen::AngleAxisd(step * 2 * M_PI, Eigen::Vector3d::UnitZ());
274  }
275 
276  // TODO(davetcoleman): publishCollisionGraph
277  // TODO(davetcoleman): publishContactPoint
278  // TODO(davetcoleman): trajectory stuff
279  // TODO(davetcoleman): gripper stuff
280 
281  // Send ROS messages
282  visual_tools_->triggerPlanningSceneUpdate();
283  }
284 
285 private:
286  // A shared node handle
288 
289  // For visualizing things in rviz
291 
292  // MoveIt Components
293  planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_;
294 
296 
297  moveit::core::RobotStatePtr robot_state_;
298 }; // end class
299 
300 } // namespace moveit_visual_tools
301 
302 int main(int argc, char** argv)
303 {
304  ros::init(argc, argv, "visual_tools_demo");
305  ROS_INFO_STREAM("Visual Tools Demo");
306 
307  // Allow the action server to recieve and send ros messages
309  spinner.start();
310 
312 
313  ROS_INFO_STREAM("Shutting down.");
314 
315  return 0;
316 }
std::shared_ptr< MoveItVisualTools > MoveItVisualToolsPtr
const moveit::core::JointModelGroup * jmg_
bool sleep() const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
#define ROS_INFO_STREAM_NAMED(name, args)
#define M_PI
double y
void spinner()
void publishLabelHelper(const Eigen::Affine3d &pose, const std::string &label)
#define ROS_FATAL_STREAM_NAMED(name, args)
static const std::string THIS_PACKAGE
int main(int argc, char **argv)
ROSLIB_DECL std::string getPath(const std::string &package_name)
moveit_visual_tools::MoveItVisualToolsPtr visual_tools_
unsigned int step
#define ROS_INFO_STREAM(args)
ROSCPP_DECL void spinOnce()
static const std::string PLANNING_GROUP_NAME
planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_


moveit_visual_tools
Author(s): Dave Coleman
autogenerated on Thu Jul 16 2020 03:55:18