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
00041 #include <boost/program_options/cmdline.hpp>
00042 #include <boost/program_options/options_description.hpp>
00043 #include <boost/program_options/parsers.hpp>
00044 #include <boost/program_options/variables_map.hpp>
00045
00046 #include <ros/ros.h>
00047
00048 static const std::string PLANNING_SCENE_TOPIC = "planning_scene";
00049 static const std::string PLANNING_REQUEST_TOPIC = "motion_plan_request";
00050 static const std::string PLANNING_RESULTS_TOPIC = "motion_plan_results";
00051
00052 static const std::string CONSTRAINTS_TOPIC = "constraints";
00053
00054 static const std::string STATES_TOPIC = "robot_states";
00055
00056 int main(int argc, char **argv)
00057 {
00058 ros::init(argc, argv, "publish_warehouse_data", ros::init_options::AnonymousName);
00059
00060
00061 double delay = 0.001;
00062
00063 boost::program_options::options_description desc;
00064 desc.add_options()
00065 ("help", "Show help message")
00066 ("host", boost::program_options::value<std::string>(), "Host for the MongoDB.")
00067 ("port", boost::program_options::value<std::size_t>(), "Port for the MongoDB.")
00068 ("scene", boost::program_options::value<std::string>(), "Name of scene to publish.")
00069 ("planning_requests", "Also publish the planning requests that correspond to the scene")
00070 ("planning_results", "Also publish the planning results that correspond to the scene")
00071 ("constraint", boost::program_options::value<std::string>(), "Name of constraint to publish.")
00072 ("state", boost::program_options::value<std::string>(), "Name of the robot state to publish.")
00073 ("delay", boost::program_options::value<double>()->default_value(delay), "Time to wait in between publishing messages (s)");
00074
00075 boost::program_options::variables_map vm;
00076 boost::program_options::store(boost::program_options::parse_command_line(argc, argv, desc), vm);
00077 boost::program_options::notify(vm);
00078
00079 if (vm.count("help") || (!vm.count("scene") && !vm.count("constraint") && !vm.count("state")))
00080 {
00081 std::cout << desc << std::endl;
00082 return 1;
00083 }
00084 try
00085 {
00086 delay = vm["delay"].as<double>();
00087 }
00088 catch(...)
00089 {
00090 std::cout << desc << std::endl;
00091 return 2;
00092 }
00093
00094 ros::AsyncSpinner spinner(1);
00095 spinner.start();
00096
00097 ros::NodeHandle nh;
00098 ros::Publisher pub_scene, pub_req, pub_res, pub_constr, pub_state;
00099 ros::Duration wait_time(delay);
00100
00101
00102 if (vm.count("scene"))
00103 {
00104 pub_scene = nh.advertise<moveit_msgs::PlanningScene>(PLANNING_SCENE_TOPIC, 10);
00105 bool req = vm.count("planning_requests");
00106 bool res = vm.count("planning_results");
00107 if (req)
00108 pub_req = nh.advertise<moveit_msgs::MotionPlanRequest>(PLANNING_REQUEST_TOPIC, 100);
00109 if (res)
00110 pub_res = nh.advertise<moveit_msgs::RobotTrajectory>(PLANNING_RESULTS_TOPIC, 100);
00111
00112 moveit_warehouse::PlanningSceneStorage pss(vm.count("host") ? vm["host"].as<std::string>() : "",
00113 vm.count("port") ? vm["port"].as<std::size_t>() : 0);
00114 ros::spinOnce();
00115
00116 std::vector<std::string> scene_names;
00117 pss.getPlanningSceneNames(vm["scene"].as<std::string>(), scene_names);
00118
00119 for (std::size_t i = 0 ; i < scene_names.size() ; ++i)
00120 {
00121 moveit_warehouse::PlanningSceneWithMetadata pswm;
00122 if (pss.getPlanningScene(pswm, scene_names[i]))
00123 {
00124 ROS_INFO("Publishing scene '%s'", pswm->lookupString(moveit_warehouse::PlanningSceneStorage::PLANNING_SCENE_ID_NAME).c_str());
00125 pub_scene.publish(static_cast<const moveit_msgs::PlanningScene&>(*pswm));
00126 ros::spinOnce();
00127
00128
00129 if (req || res)
00130 {
00131 std::vector<moveit_warehouse::MotionPlanRequestWithMetadata> planning_queries;
00132 std::vector<std::string> query_names;
00133 pss.getPlanningQueries(planning_queries, query_names, pswm->name);
00134 ROS_INFO("There are %d planning queries associated to the scene", (int)planning_queries.size());
00135 ros::WallDuration(0.5).sleep();
00136 for (std::size_t i = 0 ; i < planning_queries.size() ; ++i)
00137 {
00138 if (req)
00139 {
00140 ROS_INFO("Publishing query '%s'", query_names[i].c_str());
00141 pub_req.publish(static_cast<const moveit_msgs::MotionPlanRequest&>(*planning_queries[i]));
00142 ros::spinOnce();
00143 }
00144 if (res)
00145 {
00146 std::vector<moveit_warehouse::RobotTrajectoryWithMetadata> planning_results;
00147 pss.getPlanningResults(planning_results, query_names[i], pswm->name);
00148 for (std::size_t j = 0 ; j < planning_results.size() ; ++j)
00149 {
00150 pub_res.publish(static_cast<const moveit_msgs::RobotTrajectory&>(*planning_results[j]));
00151 ros::spinOnce();
00152 }
00153 }
00154 }
00155 }
00156 wait_time.sleep();
00157 }
00158 }
00159 }
00160
00161
00162 if (vm.count("constraint"))
00163 {
00164 moveit_warehouse::ConstraintsStorage cs(vm.count("host") ? vm["host"].as<std::string>() : "",
00165 vm.count("port") ? vm["port"].as<std::size_t>() : 0);
00166 pub_constr = nh.advertise<moveit_msgs::Constraints>(CONSTRAINTS_TOPIC, 100);
00167 std::vector<std::string> cnames;
00168 cs.getKnownConstraints(vm["constraint"].as<std::string>(), cnames);
00169
00170 for (std::size_t i = 0 ; i < cnames.size() ; ++i)
00171 {
00172 moveit_warehouse::ConstraintsWithMetadata cwm;
00173 if (cs.getConstraints(cwm, cnames[i]))
00174 {
00175 ROS_INFO("Publishing constraints '%s'", cwm->lookupString(moveit_warehouse::ConstraintsStorage::CONSTRAINTS_ID_NAME).c_str());
00176 pub_constr.publish(static_cast<const moveit_msgs::Constraints&>(*cwm));
00177 ros::spinOnce();
00178 wait_time.sleep();
00179 }
00180 }
00181 }
00182
00183
00184 if (vm.count("state"))
00185 {
00186 moveit_warehouse::RobotStateStorage rs(vm.count("host") ? vm["host"].as<std::string>() : "",
00187 vm.count("port") ? vm["port"].as<std::size_t>() : 0);
00188 pub_state = nh.advertise<moveit_msgs::RobotState>(STATES_TOPIC, 100);
00189 std::vector<std::string> rnames;
00190 rs.getKnownRobotStates(vm["state"].as<std::string>(), rnames);
00191
00192 for (std::size_t i = 0 ; i < rnames.size() ; ++i)
00193 {
00194 moveit_warehouse::RobotStateWithMetadata rswm;
00195 if (rs.getRobotState(rswm, rnames[i]))
00196 {
00197 ROS_INFO("Publishing state '%s'", rswm->lookupString(moveit_warehouse::RobotStateStorage::STATE_NAME).c_str());
00198 pub_state.publish(static_cast<const moveit_msgs::RobotState&>(*rswm));
00199 ros::spinOnce();
00200 wait_time.sleep();
00201 }
00202 }
00203 }
00204
00205 ros::WallDuration(1.0).sleep();
00206 ROS_INFO("Done.");
00207
00208 return 0;
00209 }