save_to_warehouse.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 
41 #include <boost/algorithm/string/join.hpp>
42 #include <boost/program_options/cmdline.hpp>
43 #include <boost/program_options/options_description.hpp>
44 #include <boost/program_options/parsers.hpp>
45 #include <boost/program_options/variables_map.hpp>
46 #include <ros/ros.h>
48 
49 static const std::string ROBOT_DESCRIPTION = "robot_description";
50 
52 {
53  ROS_INFO("Received an update to the planning scene...");
54 
55  if (!psm->getPlanningScene()->getName().empty())
56  {
57  if (!pss->hasPlanningScene(psm->getPlanningScene()->getName()))
58  {
59  moveit_msgs::PlanningScene psmsg;
60  psm->getPlanningScene()->getPlanningSceneMsg(psmsg);
61  pss->addPlanningScene(psmsg);
62  }
63  else
64  ROS_INFO("Scene '%s' was previously added. Not adding again.", psm->getPlanningScene()->getName().c_str());
65  }
66  else
67  ROS_INFO("Scene name is empty. Not saving.");
68 }
69 
70 void onMotionPlanRequest(const moveit_msgs::MotionPlanRequestConstPtr& req,
72 {
73  if (psm->getPlanningScene()->getName().empty())
74  {
75  ROS_INFO("Scene name is empty. Not saving planning request.");
76  return;
77  }
78  pss->addPlanningQuery(*req, psm->getPlanningScene()->getName());
79 }
80 
81 void onConstraints(const moveit_msgs::ConstraintsConstPtr& msg, moveit_warehouse::ConstraintsStorage* cs)
82 {
83  if (msg->name.empty())
84  {
85  ROS_INFO("No name specified for constraints. Not saving.");
86  return;
87  }
88 
89  if (cs->hasConstraints(msg->name))
90  {
91  ROS_INFO("Replacing constraints '%s'", msg->name.c_str());
92  cs->removeConstraints(msg->name);
93  cs->addConstraints(*msg);
94  }
95  else
96  {
97  ROS_INFO("Adding constraints '%s'", msg->name.c_str());
98  cs->addConstraints(*msg);
99  }
100 }
101 
102 void onRobotState(const moveit_msgs::RobotStateConstPtr& msg, moveit_warehouse::RobotStateStorage* rs)
103 {
104  std::vector<std::string> names;
105  rs->getKnownRobotStates(names);
106  std::set<std::string> names_set(names.begin(), names.end());
107  std::size_t n = names.size();
108  while (names_set.find("S" + boost::lexical_cast<std::string>(n)) != names_set.end())
109  n++;
110  std::string name = "S" + boost::lexical_cast<std::string>(n);
111  ROS_INFO("Adding robot state '%s'", name.c_str());
112  rs->addRobotState(*msg, name);
113 }
114 
115 int main(int argc, char** argv)
116 {
117  ros::init(argc, argv, "save_to_warehouse", ros::init_options::AnonymousName);
118 
119  boost::program_options::options_description desc;
120  desc.add_options()("help", "Show help message")("host", boost::program_options::value<std::string>(),
121  "Host for the "
122  "DB.")("port", boost::program_options::value<std::size_t>(),
123  "Port for the DB.");
124 
125  boost::program_options::variables_map vm;
126  boost::program_options::store(boost::program_options::parse_command_line(argc, argv, desc), vm);
127  boost::program_options::notify(vm);
128 
129  if (vm.count("help"))
130  {
131  std::cout << desc << std::endl;
132  return 1;
133  }
134  // Set up db
136  if (vm.count("host") && vm.count("port"))
137  conn->setParams(vm["host"].as<std::string>(), vm["port"].as<std::size_t>());
138  if (!conn->connect())
139  return 1;
140 
142  spinner.start();
143 
144  ros::NodeHandle nh;
145  std::shared_ptr<tf2_ros::Buffer> tf_buffer = std::make_shared<tf2_ros::Buffer>();
146  std::shared_ptr<tf2_ros::TransformListener> tf_listener =
147  std::make_shared<tf2_ros::TransformListener>(*tf_buffer, nh);
149  if (!psm.getPlanningScene())
150  {
151  ROS_ERROR("Unable to initialize PlanningSceneMonitor");
152  return 1;
153  }
154 
155  psm.startSceneMonitor();
156  psm.startWorldGeometryMonitor();
160  std::vector<std::string> names;
161  pss.getPlanningSceneNames(names);
162  if (names.empty())
163  ROS_INFO("There are no previously stored scenes");
164  else
165  {
166  ROS_INFO("Previously stored scenes:");
167  for (std::size_t i = 0; i < names.size(); ++i)
168  ROS_INFO(" * %s", names[i].c_str());
169  }
170  cs.getKnownConstraints(names);
171  if (names.empty())
172  ROS_INFO("There are no previously stored constraints");
173  else
174  {
175  ROS_INFO("Previously stored constraints:");
176  for (std::size_t i = 0; i < names.size(); ++i)
177  ROS_INFO(" * %s", names[i].c_str());
178  }
179  rs.getKnownRobotStates(names);
180  if (names.empty())
181  ROS_INFO("There are no previously stored robot states");
182  else
183  {
184  ROS_INFO("Previously stored robot states:");
185  for (std::size_t i = 0; i < names.size(); ++i)
186  ROS_INFO(" * %s", names[i].c_str());
187  }
188 
189  psm.addUpdateCallback(boost::bind(&onSceneUpdate, &psm, &pss));
190 
191  boost::function<void(const moveit_msgs::MotionPlanRequestConstPtr&)> callback1 =
192  boost::bind(&onMotionPlanRequest, _1, &psm, &pss);
193  ros::Subscriber mplan_req_sub = nh.subscribe("motion_plan_request", 100, callback1);
194  boost::function<void(const moveit_msgs::ConstraintsConstPtr&)> callback2 = boost::bind(&onConstraints, _1, &cs);
195  ros::Subscriber constr_sub = nh.subscribe("constraints", 100, callback2);
196  boost::function<void(const moveit_msgs::RobotStateConstPtr&)> callback3 = boost::bind(&onRobotState, _1, &rs);
197  ros::Subscriber state_sub = nh.subscribe("robot_state", 100, callback3);
198 
199  std::vector<std::string> topics;
200  psm.getMonitoredTopics(topics);
201  ROS_INFO_STREAM("Listening for scene updates on topics " << boost::algorithm::join(topics, ", "));
202  ROS_INFO_STREAM("Listening for planning requests on topic " << mplan_req_sub.getTopic());
203  ROS_INFO_STREAM("Listening for named constraints on topic " << constr_sub.getTopic());
204  ROS_INFO_STREAM("Listening for states on topic " << state_sub.getTopic());
205 
207  return 0;
208 }
std::string getTopic() const
void onConstraints(const moveit_msgs::ConstraintsConstPtr &msg, moveit_warehouse::ConstraintsStorage *cs)
int main(int argc, char **argv)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
void addRobotState(const moveit_msgs::RobotState &msg, const std::string &name, const std::string &robot="")
void addConstraints(const moveit_msgs::Constraints &msg, const std::string &robot="", const std::string &group="")
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
#define ROS_ERROR(...)
void getKnownRobotStates(std::vector< std::string > &names, const std::string &robot="") const
void removeConstraints(const std::string &name, const std::string &robot="", const std::string &group="")
void spinner()
const planning_scene::PlanningScenePtr & getPlanningScene()
void onRobotState(const moveit_msgs::RobotStateConstPtr &msg, moveit_warehouse::RobotStateStorage *rs)
void getPlanningSceneNames(std::vector< std::string > &names) const
warehouse_ros::DatabaseConnection::Ptr loadDatabase()
Load a database connection.
void onMotionPlanRequest(const moveit_msgs::MotionPlanRequestConstPtr &req, planning_scene_monitor::PlanningSceneMonitor *psm, moveit_warehouse::PlanningSceneStorage *pss)
#define ROS_INFO(...)
void onSceneUpdate(planning_scene_monitor::PlanningSceneMonitor *psm, moveit_warehouse::PlanningSceneStorage *pss)
void addPlanningScene(const moveit_msgs::PlanningScene &scene)
void getKnownConstraints(std::vector< std::string > &names, const std::string &robot="", const std::string &group="") const
tf2_ros::Buffer * tf_buffer
#define ROS_INFO_STREAM(args)
bool hasConstraints(const std::string &name, const std::string &robot="", const std::string &group="") const
static const std::string ROBOT_DESCRIPTION
bool hasPlanningScene(const std::string &name) const
ROSCPP_DECL void waitForShutdown()
void addPlanningQuery(const moveit_msgs::MotionPlanRequest &planning_query, const std::string &scene_name, const std::string &query_name="")


warehouse
Author(s): Ioan Sucan
autogenerated on Fri Aug 20 2021 03:01:32