43 #include <boost/algorithm/string/join.hpp> 44 #include <boost/program_options/cmdline.hpp> 45 #include <boost/program_options/options_description.hpp> 46 #include <boost/program_options/parsers.hpp> 47 #include <boost/program_options/variables_map.hpp> 48 #include <boost/math/constants/constants.hpp> 53 int main(
int argc,
char** argv)
57 boost::program_options::options_description desc;
58 desc.add_options()(
"help",
"Show help message")(
"host", boost::program_options::value<std::string>(),
"Host for the " 60 "port", boost::program_options::value<std::size_t>(),
"Port for the DB.");
62 boost::program_options::variables_map vm;
63 boost::program_options::store(boost::program_options::parse_command_line(argc, argv, desc), vm);
64 boost::program_options::notify(vm);
68 std::cout << desc << std::endl;
73 if (vm.count(
"host") && vm.count(
"port"))
74 conn->setParams(vm[
"host"].as<std::string>(), vm[
"port"].as<std::size_t>());
85 ROS_ERROR(
"Unable to initialize PlanningSceneMonitor");
97 moveit_msgs::PlanningScene psmsg;
99 psmsg.name =
"default";
101 ROS_INFO(
"Added default scene: '%s'", psmsg.name.c_str());
103 moveit_msgs::RobotState rsmsg;
104 robot_state::robotStateToRobotStateMsg(psm.
getPlanningScene()->getCurrentState(), rsmsg);
108 const std::vector<std::string>& gnames = psm.
getRobotModel()->getJointModelGroupNames();
109 for (std::size_t i = 0; i < gnames.size(); ++i)
111 const robot_model::JointModelGroup* jmg = psm.
getRobotModel()->getJointModelGroup(gnames[i]);
114 const std::vector<std::string>& lnames = jmg->getLinkModelNames();
118 moveit_msgs::OrientationConstraint ocm;
119 ocm.link_name = lnames.back();
121 ocm.orientation.x = 0.0;
122 ocm.orientation.y = 0.0;
123 ocm.orientation.z = 0.0;
124 ocm.orientation.w = 1.0;
125 ocm.absolute_x_axis_tolerance = 0.1;
126 ocm.absolute_y_axis_tolerance = 0.1;
127 ocm.absolute_z_axis_tolerance = boost::math::constants::pi<double>();
129 moveit_msgs::Constraints cmsg;
130 cmsg.orientation_constraints.resize(1, ocm);
131 cmsg.name = ocm.link_name +
":upright";
133 ROS_INFO(
"Added default constraint: '%s'", cmsg.name.c_str());
135 ROS_INFO(
"Default MoveIt! Warehouse was reset.");
void addRobotState(const moveit_msgs::RobotState &msg, const std::string &name, const std::string &robot="")
void addConstraints(const moveit_msgs::Constraints &msg, const std::string &robot="", const std::string &group="")
static const std::string ROBOT_DESCRIPTION
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
const robot_model::RobotModelConstPtr & getRobotModel() const
const planning_scene::PlanningScenePtr & getPlanningScene()
warehouse_ros::DatabaseConnection::Ptr loadDatabase()
Load a database connection.
void addPlanningScene(const moveit_msgs::PlanningScene &scene)
ROSCPP_DECL void shutdown()
int main(int argc, char **argv)