Go to the documentation of this file.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 <moveit/robot_state/conversions.h>
00042 #include <moveit/kinematic_constraints/utils.h>
00043 #include <boost/algorithm/string/join.hpp>
00044 #include <boost/program_options/cmdline.hpp>
00045 #include <boost/program_options/options_description.hpp>
00046 #include <boost/program_options/parsers.hpp>
00047 #include <boost/program_options/variables_map.hpp>
00048 #include <boost/math/constants/constants.hpp>
00049 #include <ros/ros.h>
00050
00051 static const std::string ROBOT_DESCRIPTION="robot_description";
00052
00053 int main(int argc, char **argv)
00054 {
00055 ros::init(argc, argv, "initialize_demo_db", ros::init_options::AnonymousName);
00056
00057 boost::program_options::options_description desc;
00058 desc.add_options()
00059 ("help", "Show help message")
00060 ("host", boost::program_options::value<std::string>(), "Host for the MongoDB.")
00061 ("port", boost::program_options::value<std::size_t>(), "Port for the MongoDB.");
00062
00063 boost::program_options::variables_map vm;
00064 boost::program_options::store(boost::program_options::parse_command_line(argc, argv, desc), vm);
00065 boost::program_options::notify(vm);
00066
00067 if (vm.count("help"))
00068 {
00069 std::cout << desc << std::endl;
00070 return 1;
00071 }
00072
00073 ros::AsyncSpinner spinner(1);
00074 spinner.start();
00075
00076 ros::NodeHandle nh;
00077 planning_scene_monitor::PlanningSceneMonitor psm(ROBOT_DESCRIPTION);
00078 if (!psm.getPlanningScene())
00079 {
00080 ROS_ERROR("Unable to initialize PlanningSceneMonitor");
00081 return 1;
00082 }
00083
00084 bool done = false;
00085 unsigned int attempts = 0;
00086 while (!done && attempts < 5)
00087 {
00088 attempts++;
00089 try
00090 {
00091 moveit_warehouse::PlanningSceneStorage pss(vm.count("host") ? vm["host"].as<std::string>() : "",
00092 vm.count("port") ? vm["port"].as<std::size_t>() : 0);
00093 moveit_warehouse::ConstraintsStorage cs(vm.count("host") ? vm["host"].as<std::string>() : "",
00094 vm.count("port") ? vm["port"].as<std::size_t>() : 0);
00095 moveit_warehouse::RobotStateStorage rs(vm.count("host") ? vm["host"].as<std::string>() : "",
00096 vm.count("port") ? vm["port"].as<std::size_t>() : 0);
00097 pss.reset();
00098 cs.reset();
00099 rs.reset();
00100
00101
00102 moveit_msgs::PlanningScene psmsg;
00103 psm.getPlanningScene()->getPlanningSceneMsg(psmsg);
00104 psmsg.name = "default";
00105 pss.addPlanningScene(psmsg);
00106 ROS_INFO("Added default scene: '%s'", psmsg.name.c_str());
00107
00108 moveit_msgs::RobotState rsmsg;
00109 robot_state::robotStateToRobotStateMsg(psm.getPlanningScene()->getCurrentState(), rsmsg);
00110 rs.addRobotState(rsmsg, "default");
00111 ROS_INFO("Added default state");
00112
00113 const std::vector<std::string> &gnames = psm.getRobotModel()->getJointModelGroupNames();
00114 for (std::size_t i = 0 ; i < gnames.size() ; ++i)
00115 {
00116 const robot_model::JointModelGroup *jmg = psm.getRobotModel()->getJointModelGroup(gnames[i]);
00117 if (!jmg->isChain())
00118 continue;
00119 const std::vector<std::string> &lnames = jmg->getLinkModelNames();
00120 if (lnames.empty())
00121 continue;
00122
00123 moveit_msgs::OrientationConstraint ocm;
00124 ocm.link_name = lnames.back();
00125 ocm.header.frame_id = psm.getRobotModel()->getModelFrame();
00126 ocm.orientation.x = 0.0;
00127 ocm.orientation.y = 0.0;
00128 ocm.orientation.z = 0.0;
00129 ocm.orientation.w = 1.0;
00130 ocm.absolute_x_axis_tolerance = 0.1;
00131 ocm.absolute_y_axis_tolerance = 0.1;
00132 ocm.absolute_z_axis_tolerance = boost::math::constants::pi<double>();
00133 ocm.weight = 1.0;
00134 moveit_msgs::Constraints cmsg;
00135 cmsg.orientation_constraints.resize(1, ocm);
00136 cmsg.name = ocm.link_name + ":upright";
00137 cs.addConstraints(cmsg, psm.getRobotModel()->getName(), jmg->getName());
00138 ROS_INFO("Added default constraint: '%s'", cmsg.name.c_str());
00139 }
00140 done = true;
00141 ROS_INFO("Default MoveIt! Warehouse was reset. Done.");
00142 }
00143 catch(mongo_ros::DbConnectException &ex)
00144 {
00145 ROS_WARN("MongoDB does not appear to be initialized yet. Waiting for a few seconds before trying again ...");
00146 ros::WallDuration(15.0).sleep();
00147 }
00148 }
00149
00150 ros::shutdown();
00151
00152 return 0;
00153 }