broadcast.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2012, Willow Garage, Inc.
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 Willow Garage 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 
40 
41 #include <boost/program_options/cmdline.hpp>
42 #include <boost/program_options/options_description.hpp>
43 #include <boost/program_options/parsers.hpp>
44 #include <boost/program_options/variables_map.hpp>
45 
46 #include <ros/ros.h>
47 
48 static const std::string PLANNING_SCENE_TOPIC = "planning_scene";
49 static const std::string PLANNING_REQUEST_TOPIC = "motion_plan_request";
50 static const std::string PLANNING_RESULTS_TOPIC = "motion_plan_results";
51 
52 static const std::string CONSTRAINTS_TOPIC = "constraints";
53 
54 static const std::string STATES_TOPIC = "robot_states";
55 
56 int main(int argc, char** argv)
57 {
58  ros::init(argc, argv, "publish_warehouse_data", ros::init_options::AnonymousName);
59 
60  // time to wait in between publishing messages
61  double delay = 0.001;
62 
63  boost::program_options::options_description desc;
64  desc.add_options()("help", "Show help message")("host", boost::program_options::value<std::string>(), "Host for the "
65  "DB.")(
66  "port", boost::program_options::value<std::size_t>(),
67  "Port for the DB.")("scene", boost::program_options::value<std::string>(), "Name of scene to publish.")(
68  "planning_requests", "Also publish the planning requests that correspond to the scene")(
69  "planning_results", "Also publish the planning results that correspond to the scene")(
70  "constraint", boost::program_options::value<std::string>(), "Name of constraint to publish.")(
71  "state", boost::program_options::value<std::string>(),
72  "Name of the robot state to publish.")("delay", boost::program_options::value<double>()->default_value(delay),
73  "Time to wait in between publishing messages (s)");
74 
75  boost::program_options::variables_map vm;
76  boost::program_options::store(boost::program_options::parse_command_line(argc, argv, desc), vm);
77  boost::program_options::notify(vm);
78 
79  if (vm.count("help") || (!vm.count("scene") && !vm.count("constraint") && !vm.count("state")))
80  {
81  std::cout << desc << std::endl;
82  return 1;
83  }
84  try
85  {
86  delay = vm["delay"].as<double>();
87  }
88  catch (...)
89  {
90  std::cout << desc << std::endl;
91  return 2;
92  }
93  // Set up db
95  if (vm.count("host") && vm.count("port"))
96  conn->setParams(vm["host"].as<std::string>(), vm["port"].as<std::size_t>());
97  if (!conn->connect())
98  return 1;
99 
101  spinner.start();
102 
103  ros::NodeHandle nh;
104  ros::Publisher pub_scene, pub_req, pub_res, pub_constr, pub_state;
105  ros::Duration wait_time(delay);
106 
107  // publish the scene
108  if (vm.count("scene"))
109  {
110  pub_scene = nh.advertise<moveit_msgs::PlanningScene>(PLANNING_SCENE_TOPIC, 10);
111  bool req = vm.count("planning_requests");
112  bool res = vm.count("planning_results");
113  if (req)
114  pub_req = nh.advertise<moveit_msgs::MotionPlanRequest>(PLANNING_REQUEST_TOPIC, 100);
115  if (res)
116  pub_res = nh.advertise<moveit_msgs::RobotTrajectory>(PLANNING_RESULTS_TOPIC, 100);
117 
119  ros::spinOnce();
120 
121  std::vector<std::string> scene_names;
122  pss.getPlanningSceneNames(vm["scene"].as<std::string>(), scene_names);
123 
124  for (std::size_t i = 0; i < scene_names.size(); ++i)
125  {
127  if (pss.getPlanningScene(pswm, scene_names[i]))
128  {
129  ROS_INFO("Publishing scene '%s'",
131  pub_scene.publish(static_cast<const moveit_msgs::PlanningScene&>(*pswm));
132  ros::spinOnce();
133 
134  // publish optional data associated to the scene
135  if (req || res)
136  {
137  std::vector<moveit_warehouse::MotionPlanRequestWithMetadata> planning_queries;
138  std::vector<std::string> query_names;
139  pss.getPlanningQueries(planning_queries, query_names, pswm->name);
140  ROS_INFO("There are %d planning queries associated to the scene", (int)planning_queries.size());
141  ros::WallDuration(0.5).sleep();
142  for (std::size_t i = 0; i < planning_queries.size(); ++i)
143  {
144  if (req)
145  {
146  ROS_INFO("Publishing query '%s'", query_names[i].c_str());
147  pub_req.publish(static_cast<const moveit_msgs::MotionPlanRequest&>(*planning_queries[i]));
148  ros::spinOnce();
149  }
150  if (res)
151  {
152  std::vector<moveit_warehouse::RobotTrajectoryWithMetadata> planning_results;
153  pss.getPlanningResults(planning_results, query_names[i], pswm->name);
154  for (std::size_t j = 0; j < planning_results.size(); ++j)
155  {
156  pub_res.publish(static_cast<const moveit_msgs::RobotTrajectory&>(*planning_results[j]));
157  ros::spinOnce();
158  }
159  }
160  }
161  }
162  wait_time.sleep();
163  }
164  }
165  }
166 
167  // publish constraints
168  if (vm.count("constraint"))
169  {
171  pub_constr = nh.advertise<moveit_msgs::Constraints>(CONSTRAINTS_TOPIC, 100);
172  std::vector<std::string> cnames;
173  cs.getKnownConstraints(vm["constraint"].as<std::string>(), cnames);
174 
175  for (std::size_t i = 0; i < cnames.size(); ++i)
176  {
178  if (cs.getConstraints(cwm, cnames[i]))
179  {
180  ROS_INFO("Publishing constraints '%s'",
182  pub_constr.publish(static_cast<const moveit_msgs::Constraints&>(*cwm));
183  ros::spinOnce();
184  wait_time.sleep();
185  }
186  }
187  }
188 
189  // publish constraints
190  if (vm.count("state"))
191  {
193  pub_state = nh.advertise<moveit_msgs::RobotState>(STATES_TOPIC, 100);
194  std::vector<std::string> rnames;
195  rs.getKnownRobotStates(vm["state"].as<std::string>(), rnames);
196 
197  for (std::size_t i = 0; i < rnames.size(); ++i)
198  {
200  if (rs.getRobotState(rswm, rnames[i]))
201  {
202  ROS_INFO("Publishing state '%s'", rswm->lookupString(moveit_warehouse::RobotStateStorage::STATE_NAME).c_str());
203  pub_state.publish(static_cast<const moveit_msgs::RobotState&>(*rswm));
204  ros::spinOnce();
205  wait_time.sleep();
206  }
207  }
208  }
209 
210  ros::WallDuration(1.0).sleep();
211  ROS_INFO("Done.");
212 
213  return 0;
214 }
static const std::string PLANNING_SCENE_TOPIC
Definition: broadcast.cpp:48
void publish(const boost::shared_ptr< M > &message) const
bool sleep() const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
void getKnownRobotStates(std::vector< std::string > &names, const std::string &robot="") const
int main(int argc, char **argv)
Definition: broadcast.cpp:56
static const std::string PLANNING_REQUEST_TOPIC
Definition: broadcast.cpp:49
static const std::string STATES_TOPIC
Definition: broadcast.cpp:54
void spinner()
bool sleep() const
warehouse_ros::DatabaseConnection::Ptr loadDatabase()
Load a database connection.
#define ROS_INFO(...)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void getKnownConstraints(std::vector< std::string > &names, const std::string &robot="", const std::string &group="") const
static const std::string CONSTRAINTS_TOPIC
Definition: broadcast.cpp:52
bool getConstraints(ConstraintsWithMetadata &msg_m, const std::string &name, const std::string &robot="", const std::string &group="") const
Get the constraints named name. Return false on failure.
static const std::string CONSTRAINTS_ID_NAME
static const std::string STATE_NAME
Definition: state_storage.h:56
bool getRobotState(RobotStateWithMetadata &msg_m, const std::string &name, const std::string &robot="") const
Get the constraints named name. Return false on failure.
ROSCPP_DECL void spinOnce()
static const std::string PLANNING_RESULTS_TOPIC
Definition: broadcast.cpp:50


warehouse
Author(s): Ioan Sucan
autogenerated on Sun Oct 18 2020 13:18:37