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