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> 52 ROS_INFO(
"Received an update to the planning scene...");
58 moveit_msgs::PlanningScene psmsg;
66 ROS_INFO(
"Scene name is empty. Not saving.");
74 ROS_INFO(
"Scene name is empty. Not saving planning request.");
82 if (msg->name.empty())
84 ROS_INFO(
"No name specified for constraints. Not saving.");
90 ROS_INFO(
"Replacing constraints '%s'", msg->name.c_str());
96 ROS_INFO(
"Adding constraints '%s'", msg->name.c_str());
103 std::vector<std::string> 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())
109 std::string name =
"S" + boost::lexical_cast<std::string>(n);
110 ROS_INFO(
"Adding robot state '%s'", name.c_str());
114 int main(
int argc,
char** argv)
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 " 121 "port", boost::program_options::value<std::size_t>(),
"Port for the DB.");
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);
127 if (vm.count(
"help"))
129 std::cout << desc << std::endl;
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())
147 ROS_ERROR(
"Unable to initialize PlanningSceneMonitor");
156 std::vector<std::string> names;
159 ROS_INFO(
"There are no previously stored scenes");
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());
168 ROS_INFO(
"There are no previously stored constraints");
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());
177 ROS_INFO(
"There are no previously stored robot states");
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());
187 boost::function<void(const moveit_msgs::MotionPlanRequestConstPtr&)> callback1 =
190 boost::function<void(const moveit_msgs::ConstraintsConstPtr&)> callback2 = boost::bind(&
onConstraints, _1, &cs);
192 boost::function<void(const moveit_msgs::RobotStateConstPtr&)> callback3 = boost::bind(&
onRobotState, _1, &rs);
195 std::vector<std::string> topics;
197 ROS_INFO_STREAM(
"Listening for scene updates on topics " << boost::algorithm::join(topics,
", "));
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="")
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)
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
ROSCPP_DECL void waitForShutdown()
void addPlanningQuery(const moveit_msgs::MotionPlanRequest &planning_query, const std::string &scene_name, const std::string &query_name="")