moveit2
The MoveIt Motion Planning Framework for ROS 2.
Loading...
Searching...
No Matches
publish_scene_from_text.cpp
Go to the documentation of this file.
1/*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2013, Ioan A. Sucan
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 Ioan A. Sucan 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: Ioan Sucan */
36
37#include <chrono>
39#include <rclcpp/executors/multi_threaded_executor.hpp>
40#include <rclcpp/logger.hpp>
41#include <rclcpp/logging.hpp>
42#include <rclcpp/node.hpp>
43#include <rclcpp/publisher.hpp>
44#include <rclcpp/version.h>
46#if RCLCPP_VERSION_GTE(20, 0, 0)
47#include <rclcpp/event_handler.hpp>
48#else
49#include <rclcpp/qos_event.hpp>
50#endif
51#include <rclcpp/utilities.hpp>
52
53using namespace std::chrono_literals;
54
55int main(int argc, char** argv)
56{
57 rclcpp::init(argc, argv);
58 auto node = rclcpp::Node::make_shared("publish_planning_scene");
59 moveit::setNodeLoggerName(node->get_name());
60
61 // decide whether to publish the full scene
62 bool full_scene = false;
63
64 // the index of the argument with the filename
65 int filename_index = 1;
66 if (argc > 2)
67 {
68 if (strncmp(argv[1], "--scene", 7) == 0)
69 {
70 full_scene = true;
71 filename_index = 2;
72 }
73 }
74
75 if (argc > 1)
76 {
77 rclcpp::executors::MultiThreadedExecutor executor;
78 executor.add_node(node);
79
80 rclcpp::Publisher<moveit_msgs::msg::PlanningScene>::SharedPtr pub_scene;
81 rclcpp::Publisher<moveit_msgs::msg::PlanningSceneWorld>::SharedPtr pub_world_scene;
82
83 if (full_scene)
84 {
85 pub_scene = node->create_publisher<moveit_msgs::msg::PlanningScene>(
87 }
88 else
89 {
90 pub_world_scene = node->create_publisher<moveit_msgs::msg::PlanningSceneWorld>(
92 }
93
95 opt.robot_description = "robot_description";
96 opt.load_kinematics_solvers = false;
97
98 auto rml = std::make_shared<robot_model_loader::RobotModelLoader>(node, opt);
99 planning_scene::PlanningScene ps(rml->getModel());
100
101 std::ifstream f(argv[filename_index]);
102 if (ps.loadGeometryFromStream(f))
103 {
104 RCLCPP_INFO(node->get_logger(), "Publishing geometry from '%s' ...", argv[filename_index]);
105 moveit_msgs::msg::PlanningScene ps_msg;
106 ps.getPlanningSceneMsg(ps_msg);
107 ps_msg.is_diff = true;
108
109 unsigned int attempts = 0;
110 while (pub_scene->get_subscription_count() < 1 && ++attempts < 100)
111 rclcpp::sleep_for(500ms);
112
113 if (full_scene)
114 {
115 pub_scene->publish(ps_msg);
116 }
117 else
118 {
119 pub_world_scene->publish(ps_msg.world);
120 }
121
122 rclcpp::sleep_for(1s);
123 }
124 }
125 else
126 {
127 RCLCPP_WARN(node->get_logger(),
128 "A filename was expected as argument. That file should be a text representation of the geometry in a "
129 "planning scene.");
130 }
131
132 rclcpp::shutdown();
133 return 0;
134}
This class maintains the representation of the environment as seen by a planning instance....
void getPlanningSceneMsg(moveit_msgs::msg::PlanningScene &scene) const
Construct a message (scene) with all the necessary data so that the scene can be later reconstructed ...
bool loadGeometryFromStream(std::istream &in)
Load the geometry of the planning scene from a stream.
static const std::string DEFAULT_PLANNING_SCENE_TOPIC
The name of the topic used by default for receiving full planning scenes or planning scene diffs.
void setNodeLoggerName(const std::string &name)
Call once after creating a node to initialize logging namespaces.
Definition logger.cpp:73
int main(int argc, char **argv)
Structure that encodes the options to be passed to the RobotModelLoader constructor.
std::string robot_description
The string name corresponding to the ROS param where the URDF is loaded; Using the same parameter nam...
bool load_kinematics_solvers
Flag indicating whether the kinematics solvers should be loaded as well, using specified ROS paramete...