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()("help", "Show help message")("host", boost::program_options::value<std::string>(), "Host for the "
00059 "DB.")(
00060 "port", boost::program_options::value<std::size_t>(), "Port for the DB.");
00061
00062 boost::program_options::variables_map vm;
00063 boost::program_options::store(boost::program_options::parse_command_line(argc, argv, desc), vm);
00064 boost::program_options::notify(vm);
00065
00066 if (vm.count("help"))
00067 {
00068 std::cout << desc << std::endl;
00069 return 1;
00070 }
00071
00072 warehouse_ros::DatabaseConnection::Ptr conn = moveit_warehouse::loadDatabase();
00073 if (vm.count("host") && vm.count("port"))
00074 conn->setParams(vm["host"].as<std::string>(), vm["port"].as<std::size_t>());
00075 if (!conn->connect())
00076 return 1;
00077
00078 ros::AsyncSpinner spinner(1);
00079 spinner.start();
00080
00081 ros::NodeHandle nh;
00082 planning_scene_monitor::PlanningSceneMonitor psm(ROBOT_DESCRIPTION);
00083 if (!psm.getPlanningScene())
00084 {
00085 ROS_ERROR("Unable to initialize PlanningSceneMonitor");
00086 return 1;
00087 }
00088
00089 moveit_warehouse::PlanningSceneStorage pss(conn);
00090 moveit_warehouse::ConstraintsStorage cs(conn);
00091 moveit_warehouse::RobotStateStorage rs(conn);
00092 pss.reset();
00093 cs.reset();
00094 rs.reset();
00095
00096
00097 moveit_msgs::PlanningScene psmsg;
00098 psm.getPlanningScene()->getPlanningSceneMsg(psmsg);
00099 psmsg.name = "default";
00100 pss.addPlanningScene(psmsg);
00101 ROS_INFO("Added default scene: '%s'", psmsg.name.c_str());
00102
00103 moveit_msgs::RobotState rsmsg;
00104 robot_state::robotStateToRobotStateMsg(psm.getPlanningScene()->getCurrentState(), rsmsg);
00105 rs.addRobotState(rsmsg, "default");
00106 ROS_INFO("Added default state");
00107
00108 const std::vector<std::string>& gnames = psm.getRobotModel()->getJointModelGroupNames();
00109 for (std::size_t i = 0; i < gnames.size(); ++i)
00110 {
00111 const robot_model::JointModelGroup* jmg = psm.getRobotModel()->getJointModelGroup(gnames[i]);
00112 if (!jmg->isChain())
00113 continue;
00114 const std::vector<std::string>& lnames = jmg->getLinkModelNames();
00115 if (lnames.empty())
00116 continue;
00117
00118 moveit_msgs::OrientationConstraint ocm;
00119 ocm.link_name = lnames.back();
00120 ocm.header.frame_id = psm.getRobotModel()->getModelFrame();
00121 ocm.orientation.x = 0.0;
00122 ocm.orientation.y = 0.0;
00123 ocm.orientation.z = 0.0;
00124 ocm.orientation.w = 1.0;
00125 ocm.absolute_x_axis_tolerance = 0.1;
00126 ocm.absolute_y_axis_tolerance = 0.1;
00127 ocm.absolute_z_axis_tolerance = boost::math::constants::pi<double>();
00128 ocm.weight = 1.0;
00129 moveit_msgs::Constraints cmsg;
00130 cmsg.orientation_constraints.resize(1, ocm);
00131 cmsg.name = ocm.link_name + ":upright";
00132 cs.addConstraints(cmsg, psm.getRobotModel()->getName(), jmg->getName());
00133 ROS_INFO("Added default constraint: '%s'", cmsg.name.c_str());
00134 }
00135 ROS_INFO("Default MoveIt! Warehouse was reset.");
00136
00137 ros::shutdown();
00138
00139 return 0;
00140 }