00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include <moveit/warehouse/planning_scene_storage.h>
00038 #include <moveit/warehouse/constraints_storage.h>
00039 #include <moveit/warehouse/state_storage.h>
00040 #include <moveit/planning_scene_monitor/planning_scene_monitor.h>
00041 #include <boost/algorithm/string/join.hpp>
00042 #include <boost/program_options/cmdline.hpp>
00043 #include <boost/program_options/options_description.hpp>
00044 #include <boost/program_options/parsers.hpp>
00045 #include <boost/program_options/variables_map.hpp>
00046 #include <ros/ros.h>
00047
00048 static const std::string ROBOT_DESCRIPTION="robot_description";
00049
00050 void onSceneUpdate(planning_scene_monitor::PlanningSceneMonitor *psm, moveit_warehouse::PlanningSceneStorage *pss)
00051 {
00052 ROS_INFO("Received an update to the planning scene...");
00053
00054 if (!psm->getPlanningScene()->getName().empty())
00055 {
00056 if (!pss->hasPlanningScene(psm->getPlanningScene()->getName()))
00057 {
00058 moveit_msgs::PlanningScene psmsg;
00059 psm->getPlanningScene()->getPlanningSceneMsg(psmsg);
00060 pss->addPlanningScene(psmsg);
00061 }
00062 else
00063 ROS_INFO("Scene '%s' was previously added. Not adding again.", psm->getPlanningScene()->getName().c_str());
00064 }
00065 else
00066 ROS_INFO("Scene name is empty. Not saving.");
00067 }
00068
00069 void onMotionPlanRequest(const moveit_msgs::MotionPlanRequestConstPtr &req,
00070 planning_scene_monitor::PlanningSceneMonitor *psm,
00071 moveit_warehouse::PlanningSceneStorage *pss)
00072 {
00073 if (psm->getPlanningScene()->getName().empty())
00074 {
00075 ROS_INFO("Scene name is empty. Not saving planning request.");
00076 return;
00077 }
00078 pss->addPlanningQuery(*req, psm->getPlanningScene()->getName());
00079 }
00080
00081 void onConstraints(const moveit_msgs::ConstraintsConstPtr &msg, moveit_warehouse::ConstraintsStorage *cs)
00082 {
00083 if (msg->name.empty())
00084 {
00085 ROS_INFO("No name specified for constraints. Not saving.");
00086 return;
00087 }
00088
00089 if (cs->hasConstraints(msg->name))
00090 {
00091 ROS_INFO("Replacing constraints '%s'", msg->name.c_str());
00092 cs->removeConstraints(msg->name);
00093 cs->addConstraints(*msg);
00094 }
00095 else
00096 {
00097 ROS_INFO("Adding constraints '%s'", msg->name.c_str());
00098 cs->addConstraints(*msg);
00099 }
00100 }
00101
00102 void onRobotState(const moveit_msgs::RobotStateConstPtr &msg, moveit_warehouse::RobotStateStorage *rs)
00103 {
00104 std::vector<std::string> names;
00105 rs->getKnownRobotStates(names);
00106 std::set<std::string> names_set(names.begin(), names.end());
00107 std::size_t n = names.size();
00108 while (names_set.find("S" + boost::lexical_cast<std::string>(n)) != names_set.end())
00109 n++;
00110 std::string name = "S" + boost::lexical_cast<std::string>(n);
00111 ROS_INFO("Adding robot state '%s'", name.c_str());
00112 rs->addRobotState(*msg, name);
00113 }
00114
00115 int main(int argc, char **argv)
00116 {
00117 ros::init(argc, argv, "save_to_warehouse", ros::init_options::AnonymousName);
00118
00119 boost::program_options::options_description desc;
00120 desc.add_options()
00121 ("help", "Show help message")
00122 ("host", boost::program_options::value<std::string>(), "Host for the MongoDB.")
00123 ("port", boost::program_options::value<std::size_t>(), "Port for the MongoDB.");
00124
00125 boost::program_options::variables_map vm;
00126 boost::program_options::store(boost::program_options::parse_command_line(argc, argv, desc), vm);
00127 boost::program_options::notify(vm);
00128
00129 if (vm.count("help"))
00130 {
00131 std::cout << desc << std::endl;
00132 return 1;
00133 }
00134
00135 ros::AsyncSpinner spinner(1);
00136 spinner.start();
00137
00138 ros::NodeHandle nh;
00139 boost::shared_ptr<tf::TransformListener> tf(new tf::TransformListener());
00140 planning_scene_monitor::PlanningSceneMonitor psm(ROBOT_DESCRIPTION, tf);
00141 if (!psm.getPlanningScene())
00142 {
00143 ROS_ERROR("Unable to initialize PlanningSceneMonitor");
00144 return 1;
00145 }
00146
00147 psm.startSceneMonitor();
00148 psm.startWorldGeometryMonitor();
00149 moveit_warehouse::PlanningSceneStorage pss(vm.count("host") ? vm["host"].as<std::string>() : "",
00150 vm.count("port") ? vm["port"].as<std::size_t>() : 0);
00151 moveit_warehouse::ConstraintsStorage cs(vm.count("host") ? vm["host"].as<std::string>() : "",
00152 vm.count("port") ? vm["port"].as<std::size_t>() : 0);
00153 moveit_warehouse::RobotStateStorage rs(vm.count("host") ? vm["host"].as<std::string>() : "",
00154 vm.count("port") ? vm["port"].as<std::size_t>() : 0);
00155 std::vector<std::string> names;
00156 pss.getPlanningSceneNames(names);
00157 if (names.empty())
00158 ROS_INFO("There are no previously stored scenes");
00159 else
00160 {
00161 ROS_INFO("Previously stored scenes:");
00162 for (std::size_t i = 0 ; i < names.size() ; ++i)
00163 ROS_INFO(" * %s", names[i].c_str());
00164 }
00165 cs.getKnownConstraints(names);
00166 if (names.empty())
00167 ROS_INFO("There are no previously stored constraints");
00168 else
00169 {
00170 ROS_INFO("Previously stored constraints:");
00171 for (std::size_t i = 0 ; i < names.size() ; ++i)
00172 ROS_INFO(" * %s", names[i].c_str());
00173 }
00174 rs.getKnownRobotStates(names);
00175 if (names.empty())
00176 ROS_INFO("There are no previously stored robot states");
00177 else
00178 {
00179 ROS_INFO("Previously stored robot states:");
00180 for (std::size_t i = 0 ; i < names.size() ; ++i)
00181 ROS_INFO(" * %s", names[i].c_str());
00182 }
00183
00184 psm.addUpdateCallback(boost::bind(&onSceneUpdate, &psm, &pss));
00185
00186 boost::function<void(const moveit_msgs::MotionPlanRequestConstPtr&)> callback1 = boost::bind(&onMotionPlanRequest, _1, &psm, &pss);
00187 ros::Subscriber mplan_req_sub = nh.subscribe("motion_plan_request", 100, callback1);
00188 boost::function<void(const moveit_msgs::ConstraintsConstPtr&)> callback2 = boost::bind(&onConstraints, _1, &cs);
00189 ros::Subscriber constr_sub = nh.subscribe("constraints", 100, callback2);
00190 boost::function<void(const moveit_msgs::RobotStateConstPtr&)> callback3 = boost::bind(&onRobotState, _1, &rs);
00191 ros::Subscriber state_sub = nh.subscribe("robot_state", 100, callback3);
00192
00193 std::vector<std::string> topics;
00194 psm.getMonitoredTopics(topics);
00195 ROS_INFO_STREAM("Listening for scene updates on topics " << boost::algorithm::join(topics, ", "));
00196 ROS_INFO_STREAM("Listening for planning requests on topic " << mplan_req_sub.getTopic());
00197 ROS_INFO_STREAM("Listening for named constraints on topic " << constr_sub.getTopic());
00198 ROS_INFO_STREAM("Listening for states on topic " << state_sub.getTopic());
00199
00200 ros::waitForShutdown();
00201 return 0;
00202 }