TEST_F(TestTransitionWrapper, wrapper) {
{
rclcpp_lifecycle::Transition t(12, "no_states_set");
EXPECT_EQ(12, t.id());
EXPECT_STREQ("no_states_set", t.label().c_str());
}
{
std::string transition_name = "no_states_set";
rclcpp_lifecycle::Transition t(12, transition_name);
transition_name = "not_no_states_set";
EXPECT_EQ(12, t.id());
EXPECT_STREQ("no_states_set", t.label().c_str());
}
{
rclcpp_lifecycle::State start_state(1, "start_state");
rclcpp_lifecycle::State goal_state(2, "goal_state");
rclcpp_lifecycle::Transition t(
12,
"from_start_to_goal",
std::move(start_state),
std::move(goal_state));
EXPECT_EQ(12, t.id());
EXPECT_FALSE(t.label().empty());
EXPECT_STREQ("from_start_to_goal", t.label().c_str());
}
}
void MotionPlanningFrame::computeLoadQueryButtonClicked()
{
if (planning_scene_storage_)
{
QList<QTreeWidgetItem *> sel = ui_->planning_scene_tree->selectedItems();
if (!sel.empty())
{
QTreeWidgetItem *s = sel.front();
if (s->type() == ITEM_TYPE_QUERY)
{
std::string scene = s->parent()->text(0).toStdString();
std::string query_name = s->text(0).toStdString();
moveit_warehouse::MotionPlanRequestWithMetadata mp;
bool got_q = false;
try
{
got_q = planning_scene_storage_->getPlanningQuery(mp, scene, query_name);
}
catch (std::runtime_error &ex)
{
ROS_ERROR("%s", ex.what());
}
if (got_q)
{
robot_state::RobotStatePtr start_state(new robot_state::RobotState(*planning_display_->getQueryStartState()));
robot_state::robotStateMsgToRobotState(planning_display_->getPlanningSceneRO()->getTransforms(), mp->start_state, *start_state);
planning_display_->setQueryStartState(*start_state);
robot_state::RobotStatePtr goal_state(new robot_state::RobotState(*planning_display_->getQueryGoalState()));
for (std::size_t i = 0 ; i < mp->goal_constraints.size() ; ++i)
if (mp->goal_constraints[i].joint_constraints.size() > 0)
{
std::map<std::string, double> vals;
for (std::size_t j = 0 ; j < mp->goal_constraints[i].joint_constraints.size() ; ++j)
vals[mp->goal_constraints[i].joint_constraints[j].joint_name] = mp->goal_constraints[i].joint_constraints[j].position;
goal_state->setVariablePositions(vals);
break;
}
planning_display_->setQueryGoalState(*goal_state);
}
else
ROS_ERROR("Failed to load planning query '%s'. Has the message format changed since the query was saved?", query_name.c_str());
}
}
}
}
void EpicNavigationNodeOMPL::initAlg()
{
// Only initialize the algorithm if all necessary variables have been defined.
if (!goal_assigned || !start_assigned || occupancy_grid == nullptr) {
return;
}
// Setup the OMPL state space.
ompl::base::RealVectorBounds ompl_bounds(2);
ompl_bounds.setLow(0, 0.0);
ompl_bounds.setHigh(0, height);
ompl_bounds.setLow(1, 0.0);
ompl_bounds.setHigh(1, width);
ompl::base::StateSpacePtr ompl_space(new ompl::base::RealVectorStateSpace(2));
ompl_space->as<ompl::base::RealVectorStateSpace>()->setBounds(ompl_bounds);
// The state validity checker holds the map representation so we can check if a cell is valid or not.
ompl::base::SpaceInformationPtr ompl_space_info(new ompl::base::SpaceInformation(ompl_space));
ompl_space_info->setStateValidityChecker(ompl::base::StateValidityCheckerPtr(
new EpicNavigationNodeOMPLStateValidityChecker(ompl_space_info, occupancy_grid)));
ompl_space_info->setup();
// We relinquish control of the occupancy_grid. It will be managed by the EpicNavigatioNodeOMPLStateValidityChecker object.
occupancy_grid = nullptr;
// Create the problem definition.
ompl::base::ScopedState<> start_state(ompl_space);
start_state->as<ompl::base::RealVectorStateSpace::StateType>()->values[0] = start_location.second;
start_state->as<ompl::base::RealVectorStateSpace::StateType>()->values[1] = start_location.first;
ompl::base::ScopedState<> goal_state(ompl_space);
goal_state->as<ompl::base::RealVectorStateSpace::StateType>()->values[0] = goal_location.second;
goal_state->as<ompl::base::RealVectorStateSpace::StateType>()->values[1] = goal_location.first;
ompl::base::ProblemDefinitionPtr ompl_problem_def(new ompl::base::ProblemDefinition(ompl_space_info));
ompl_problem_def->setOptimizationObjective(omplGetPathLengthObjective(ompl_space_info));
if (algorithm == EPIC_ALGORITHM_RRT_CONNECT) {
ompl_planner = ompl::base::PlannerPtr(new ompl::geometric::RRTConnect(ompl_space_info));
} else {
}
init_alg = true;
}
bool
SSLPathPlanner::doPlanForRobot (const int& id/*, bool& is_stop*/)
{
// is_send_plan = true;
tmp_robot_id_queried = id;
State* tmp_start = manifold->allocState ();
tmp_start->as<RealVectorStateManifold::StateType> ()->values[0] = team_state_[id].pose.x;
tmp_start->as<RealVectorStateManifold::StateType> ()->values[1] = team_state_[id].pose.y;
PathGeometric direct_path (planner_setup->getSpaceInformation ());
direct_path.states.push_back (manifold->allocState ());
manifold->copyState (direct_path.states[0], tmp_start);
State* tmp_goal = manifold->allocState ();
tmp_goal->as<RealVectorStateManifold::StateType> ()->values[0] = pose_control.pose[id].pose.x;
tmp_goal->as<RealVectorStateManifold::StateType> ()->values[1] = pose_control.pose[id].pose.y;
// std::cout<<pose_control.pose[id].pose.x<<"\t"<<pose_control.pose[id].pose.y<<std::endl;
direct_path.states.push_back (manifold->allocState ());
manifold->copyState (direct_path.states[1], tmp_goal);
Point_2 p_start (team_state_[id].pose.x, team_state_[id].pose.y);
Point_2 p_goal (pose_control.pose[id].pose.x, pose_control.pose[id].pose.y);
//too close to the target point, no need for planning, just take it as a next_target_pose
if (sqrt (getSquaredDistance (p_start, p_goal)) < VERY_CRITICAL_DIST)
{
next_target_poses[id].x = pose_control.pose[id].pose.x;
next_target_poses[id].y = pose_control.pose[id].pose.y;
next_target_poses[id].theta = pose_control.pose[id].pose.theta;
return true;
}
// manifold->printState (direct_path.states[0], std::cout);
// manifold->printState (direct_path.states[1], std::cout);
//direct path is available, no need for planning
// if (direct_path.check ())
if (!doesIntersectObstacles (id, p_start, p_goal))
{
// std::cout << "direct path is AVAILABLE for robot " << id << std::endl;
if (solution_data_for_robots[id].size () > direct_path.states.size ())
{
for (uint32_t i = direct_path.states.size (); i < solution_data_for_robots[id].size (); i++)
manifold->freeState (solution_data_for_robots[id][i]);
solution_data_for_robots[id].resize (direct_path.states.size ());
}
else if (solution_data_for_robots[id].size () < direct_path.states.size ())
{
for (uint32_t i = solution_data_for_robots[id].size (); i < direct_path.states.size (); i++)
solution_data_for_robots[id].push_back (manifold->allocState ());
}
for (uint32_t i = 0; i < direct_path.states.size (); i++)
manifold->copyState (solution_data_for_robots[id][i], direct_path.states[i]);
manifold->freeState (tmp_start);
manifold->freeState (tmp_goal);
next_target_poses[id].x = pose_control.pose[id].pose.x;
next_target_poses[id].y = pose_control.pose[id].pose.y;
next_target_poses[id].theta = pose_control.pose[id].pose.theta;
// std::cout<<next_target_poses[id].x<<"\t"<<next_target_poses[id].y<<std::endl;
return true;
}
manifold->freeState (tmp_start);
manifold->freeState (tmp_goal);
// std::cout << "direct path is NOT AVAILABLE for robot " << id << ", doing planning" << std::endl;
//direct path is not available, DO PLAN if the earlier plan is invalidated!
//earlier plan is still valid
if (checkPlanForRobot (id))
return true;
else if (is_plan_done[id])
return false;
//earlier plan is not valid anymore, replan
planner_setup->clear ();
planner_setup->clearStartStates ();
ScopedState<RealVectorStateManifold> start_state (manifold);
ScopedState<RealVectorStateManifold> goal_state (manifold);
start_state->values[0] = team_state_[id].pose.x;
start_state->values[1] = team_state_[id].pose.y;
goal_state->values[0] = pose_control.pose[id].pose.x;
goal_state->values[1] = pose_control.pose[id].pose.y;
planner_setup->setStartAndGoalStates (start_state, goal_state);
planner_setup->getProblemDefinition ()->fixInvalidInputStates (ssl::config::ROBOT_BOUNDING_RADIUS / 2.0,
ssl::config::ROBOT_BOUNDING_RADIUS / 2.0, 100);
bool solved = planner_setup->solve (0.100);//100msec
if (solved)
{
planner_setup->simplifySolution ();
PathGeometric* path;
path = &planner_setup->getSolutionPath ();
// PathSimplifier p (planner_setup->getSpaceInformation ());
// p.reduceVertices (*path, 1000);
if (solution_data_for_robots[id].size () < path->states.size ())
{
for (unsigned int i = solution_data_for_robots[id].size (); i < path->states.size (); i++)
solution_data_for_robots[id].push_back (manifold->allocState ());
}
else if (solution_data_for_robots[id].size () > path->states.size ())
{
for (unsigned int i = path->states.size (); i < solution_data_for_robots[id].size (); i++)
manifold->freeState (solution_data_for_robots[id][i]);
//drop last elements that are already being freed
solution_data_for_robots[id].resize (path->states.size ());
}
for (unsigned int i = 0; i < path->states.size (); i++)
manifold->copyState (solution_data_for_robots[id][i], path->states[i]);
//leader-follower approach based segment enlargement
//pick next location
Point_2 curr_point (path->states[0]->as<RealVectorStateManifold::StateType> ()->values[0], path->states[0]->as<
RealVectorStateManifold::StateType> ()->values[1]);
for (uint32_t i = 1; i < path->states.size (); i++)
{
Point_2 next_point (path->states[i]->as<RealVectorStateManifold::StateType> ()->values[0], path->states[i]->as<
RealVectorStateManifold::StateType> ()->values[1]);
if (!doesIntersectObstacles (id, curr_point, next_point))
{
next_target_poses[id].x = path->states[i]->as<RealVectorStateManifold::StateType> ()->values[0];
next_target_poses[id].y = path->states[i]->as<RealVectorStateManifold::StateType> ()->values[1];
next_target_poses[id].theta = pose_control.pose[id].pose.theta;
}
else
break;
}
// next_target_poses[id].x = path->states[1]->as<RealVectorStateManifold::StateType> ()->values[0];
// next_target_poses[id].y = path->states[1]->as<RealVectorStateManifold::StateType> ()->values[1];
// planner_data_for_robots[id].clear ();
// planner_data_for_robots[id] = planner_setup->getPlannerData ();
}
return solved;
}
int main(int argc, char **argv)
{
ros::init (argc, argv, "move_group_tutorial");
ros::AsyncSpinner spinner(1);
spinner.start();
ros::NodeHandle node_handle("move_group");
// BEGIN_TUTORIAL
// Start
// ^^^^^
// Setting up to start using a planner is pretty easy. Planners are
// setup as plugins in MoveIt! and you can use the ROS pluginlib
// interface to load any planner that you want to use. Before we
// can load the planner, we need two objects, a RobotModel
// and a PlanningScene.
// We will start by instantiating a
// `RobotModelLoader`_
// object, which will look up
// the robot description on the ROS parameter server and construct a
// :moveit_core:`RobotModel` for us to use.
//
// .. _RobotModelLoader: http://docs.ros.org/api/moveit_ros_planning/html/classrobot__model__loader_1_1RobotModelLoader.html
robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
robot_model::RobotModelPtr robot_model = robot_model_loader.getModel();
// Using the :moveit_core:`RobotModel`, we can construct a
// :planning_scene:`PlanningScene` that maintains the state of
// the world (including the robot).
planning_scene::PlanningScenePtr planning_scene(new planning_scene::PlanningScene(robot_model));
// We will now construct a loader to load a planner, by name.
// Note that we are using the ROS pluginlib library here.
boost::scoped_ptr<pluginlib::ClassLoader<planning_interface::PlannerManager> > planner_plugin_loader;
planning_interface::PlannerManagerPtr planner_instance;
std::string planner_plugin_name;
// We will get the name of planning plugin we want to load
// from the ROS param server, and then load the planner
// making sure to catch all exceptions.
if (!node_handle.getParam("planning_plugin", planner_plugin_name))
ROS_FATAL_STREAM("Could not find planner plugin name");
try
{
planner_plugin_loader.reset(new pluginlib::ClassLoader<planning_interface::PlannerManager>("moveit_core", "planning_interface::PlannerManager"));
}
catch(pluginlib::PluginlibException& ex)
{
ROS_FATAL_STREAM("Exception while creating planning plugin loader " << ex.what());
}
try
{
planner_instance.reset(planner_plugin_loader->createUnmanagedInstance(planner_plugin_name));
if (!planner_instance->initialize(robot_model, node_handle.getNamespace()))
ROS_FATAL_STREAM("Could not initialize planner instance");
ROS_INFO_STREAM("Using planning interface '" << planner_instance->getDescription() << "'");
}
catch(pluginlib::PluginlibException& ex)
{
const std::vector<std::string> &classes = planner_plugin_loader->getDeclaredClasses();
std::stringstream ss;
for (std::size_t i = 0 ; i < classes.size() ; ++i)
ss << classes[i] << " ";
ROS_ERROR_STREAM("Exception while loading planner '" << planner_plugin_name << "': " << ex.what() << std::endl
<< "Available plugins: " << ss.str());
}
/* Sleep a little to allow time to startup rviz, etc. */
// ros::WallDuration sleep_time(15.0);
ros::WallDuration sleep_time(1);
sleep_time.sleep();
// Pose Goal
// ^^^^^^^^^
// We will now create a motion plan request for the right arm of the PR2
// specifying the desired pose of the end-effector as input.
planning_interface::MotionPlanRequest req;
planning_interface::MotionPlanResponse res;
geometry_msgs::PoseStamped pose;
pose.header.frame_id = "base";
pose.pose.position.x = 0.3;
pose.pose.position.y = 0.0;
pose.pose.position.z = 0.3;
pose.pose.orientation.w = 1.0;
// A tolerance of 0.01 m is specified in position
// and 0.01 radians in orientation
std::vector<double> tolerance_pose(3, 0.01);
std::vector<double> tolerance_angle(3, 0.01);
// We will create the request as a constraint using a helper function available
// from the
// `kinematic_constraints`_
// package.
//
// .. _kinematic_constraints: http://docs.ros.org/api/moveit_core/html/namespacekinematic__constraints.html#a88becba14be9ced36fefc7980271e132
req.group_name = "manipulator";
moveit_msgs::Constraints pose_goal = kinematic_constraints::constructGoalConstraints("tool0", pose, tolerance_pose, tolerance_angle);
req.goal_constraints.push_back(pose_goal);
// We now construct a planning context that encapsulate the scene,
// the request and the response. We call the planner using this
// planning context
planning_interface::PlanningContextPtr context = planner_instance->getPlanningContext(planning_scene, req, res.error_code_);
context->solve(res);
if(res.error_code_.val != res.error_code_.SUCCESS)
{
ROS_ERROR("Could not compute plan successfully: %d", (int) res.error_code_.val);
return 0;
}
// Visualize the result
// ^^^^^^^^^^^^^^^^^^^^
ros::Publisher display_publisher = node_handle.advertise<moveit_msgs::DisplayTrajectory>("/move_group/display_planned_path", 1, true);
moveit_msgs::DisplayTrajectory display_trajectory;
/* Visualize the trajectory */
ROS_INFO("Visualizing the trajectory");
moveit_msgs::MotionPlanResponse response;
res.getMessage(response);
display_trajectory.trajectory_start = response.trajectory_start;
display_trajectory.trajectory.push_back(response.trajectory);
display_publisher.publish(display_trajectory);
sleep_time.sleep();
// Joint Space Goals
// ^^^^^^^^^^^^^^^^^
/* First, set the state in the planning scene to the final state of the last plan */
robot_state::RobotState& robot_state = planning_scene->getCurrentStateNonConst();
planning_scene->setCurrentState(response.trajectory_start);
#if 0
const robot_state::JointModelGroup* joint_model_group = robot_state.getJointModelGroup("manipulator");
robot_state.setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions);
// Now, setup a joint space goal
robot_state::RobotState goal_state(robot_model);
std::vector<double> joint_values(7, 0.0);
joint_values[0] = -2.0;
joint_values[3] = -0.2;
joint_values[5] = -0.15;
goal_state.setJointGroupPositions(joint_model_group, joint_values);
moveit_msgs::Constraints joint_goal = kinematic_constraints::constructGoalConstraints(goal_state, joint_model_group);
req.goal_constraints.clear();
req.goal_constraints.push_back(joint_goal);
// Call the planner and visualize the trajectory
/* Re-construct the planning context */
context = planner_instance->getPlanningContext(planning_scene, req, res.error_code_);
/* Call the Planner */
context->solve(res);
/* Check that the planning was successful */
if(res.error_code_.val != res.error_code_.SUCCESS)
{
ROS_ERROR("Could not compute plan successfully");
return 0;
}
/* Visualize the trajectory */
ROS_INFO("Visualizing the trajectory");
res.getMessage(response);
display_trajectory.trajectory_start = response.trajectory_start;
display_trajectory.trajectory.push_back(response.trajectory);
/* Now you should see two planned trajectories in series*/
display_publisher.publish(display_trajectory);
/* We will add more goals. But first, set the state in the planning
scene to the final state of the last plan */
robot_state.setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions);
/* Now, we go back to the first goal*/
req.goal_constraints.clear();
req.goal_constraints.push_back(pose_goal);
context = planner_instance->getPlanningContext(planning_scene, req, res.error_code_);
context->solve(res);
res.getMessage(response);
display_trajectory.trajectory.push_back(response.trajectory);
display_publisher.publish(display_trajectory);
// Adding Path Constraints
// ^^^^^^^^^^^^^^^^^^^^^^^
// Let's add a new pose goal again. This time we will also add a path constraint to the motion.
/* Let's create a new pose goal */
pose.pose.position.x = 0.65;
pose.pose.position.y = -0.2;
pose.pose.position.z = -0.1;
moveit_msgs::Constraints pose_goal_2 = kinematic_constraints::constructGoalConstraints("tool0", pose, tolerance_pose, tolerance_angle);
/* First, set the state in the planning scene to the final state of the last plan */
robot_state.setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions);
/* Now, let's try to move to this new pose goal*/
req.goal_constraints.clear();
req.goal_constraints.push_back(pose_goal_2);
/* But, let's impose a path constraint on the motion.
Here, we are asking for the end-effector to stay level*/
geometry_msgs::QuaternionStamped quaternion;
quaternion.header.frame_id = "torso_lift_link";
quaternion.quaternion.w = 1.0;
req.path_constraints = kinematic_constraints::constructGoalConstraints("tool0", quaternion);
// Imposing path constraints requires the planner to reason in the space of possible positions of the end-effector
// (the workspace of the robot)
// because of this, we need to specify a bound for the allowed planning volume as well;
// Note: a default bound is automatically filled by the WorkspaceBounds request adapter (part of the OMPL pipeline,
// but that is not being used in this example).
// We use a bound that definitely includes the reachable space for the arm. This is fine because sampling is not done in this volume
// when planning for the arm; the bounds are only used to determine if the sampled configurations are valid.
req.workspace_parameters.min_corner.x = req.workspace_parameters.min_corner.y = req.workspace_parameters.min_corner.z = -2.0;
req.workspace_parameters.max_corner.x = req.workspace_parameters.max_corner.y = req.workspace_parameters.max_corner.z = 2.0;
// Call the planner and visualize all the plans created so far.
context = planner_instance->getPlanningContext(planning_scene, req, res.error_code_);
context->solve(res);
res.getMessage(response);
display_trajectory.trajectory.push_back(response.trajectory);
// Now you should see four planned trajectories in series
display_publisher.publish(display_trajectory);
#endif
//END_TUTORIAL
sleep_time.sleep();
ROS_INFO("Done");
planner_instance.reset();
return 0;
}
int main(int argc, char **argv)
{
ros::init (argc, argv, "motion_planning");
ros::AsyncSpinner spinner(1);
spinner.start();
ros::NodeHandle node_handle("/move_group");
// ros::NodeHandle node_handle("~");
/* SETUP A PLANNING SCENE*/
/* Load the robot model */
robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
/* Get a shared pointer to the model */
robot_model::RobotModelPtr robot_model = robot_model_loader.getModel();
/* Construct a planning scene - NOTE: this is for illustration purposes only.
The recommended way to construct a planning scene is to use the planning_scene_monitor
to construct it for you.*/
planning_scene::PlanningScenePtr planning_scene(new planning_scene::PlanningScene(robot_model));
/* SETUP THE PLANNER*/
boost::scoped_ptr<pluginlib::ClassLoader<planning_interface::PlannerManager> > planner_plugin_loader;
planning_interface::PlannerManagerPtr planner_instance;
std::string planner_plugin_name;
/* Get the name of the planner we want to use */
if (!node_handle.getParam("planning_plugin", planner_plugin_name))
ROS_FATAL_STREAM("Could not find planner plugin name");
/* Make sure to catch all exceptions */
try
{
planner_plugin_loader.reset(new pluginlib::ClassLoader<planning_interface::PlannerManager>("moveit_core", "planning_interface::PlannerManager"));
}
catch(pluginlib::PluginlibException& ex)
{
ROS_FATAL_STREAM("Exception while creating planning plugin loader " << ex.what());
}
try
{
planner_instance.reset(planner_plugin_loader->createUnmanagedInstance(planner_plugin_name));
if (!planner_instance->initialize(robot_model, node_handle.getNamespace()))
ROS_FATAL_STREAM("Could not initialize planner instance");
ROS_INFO_STREAM("Using planning interface '" << planner_instance->getDescription() << "'");
}
catch(pluginlib::PluginlibException& ex)
{
const std::vector<std::string> &classes = planner_plugin_loader->getDeclaredClasses();
std::stringstream ss;
for (std::size_t i = 0 ; i < classes.size() ; ++i)
ss << classes[i] << " ";
ROS_ERROR_STREAM("Exception while loading planner '" << planner_plugin_name << "': " << ex.what() << std::endl
<< "Available plugins: " << ss.str());
}
/* Sleep a little to allow time to startup rviz, etc. */
ros::WallDuration sleep_time(1.0);
sleep_time.sleep();
/* CREATE A MOTION PLAN REQUEST FOR THE RIGHT ARM OF THE PR2 */
/* We will ask the end-effector of the PR2 to go to a desired location */
planning_interface::MotionPlanRequest req;
planning_interface::MotionPlanResponse res;
/* A desired pose */
geometry_msgs::PoseStamped pose;
pose.header.frame_id = "base_link";
pose.pose.position.x = 0.3;
pose.pose.position.y = -0.3;
pose.pose.position.z = 0.7;
pose.pose.orientation.x = 0.62478;
pose.pose.orientation.y = 0.210184;
pose.pose.orientation.z = -0.7107 ;
pose.pose.orientation.w = 0.245722;
/* A desired tolerance */
std::vector<double> tolerance_pose(3, 0.1);
std::vector<double> tolerance_angle(3, 0.1);
// std::vector<double> tolerance_pose(3, 0.01);
// std::vector<double> tolerance_angle(3, 0.01);
ROS_INFO("marker4");
/*Create the request */
req.group_name = "manipulator";
moveit_msgs::Constraints pose_goal = kinematic_constraints::constructGoalConstraints("wrist_3_link", pose, tolerance_pose, tolerance_angle);
req.goal_constraints.push_back(pose_goal);
ROS_INFO("marker5");
/* Construct the planning context */
planning_interface::PlanningContextPtr context = planner_instance->getPlanningContext(planning_scene, req, res.error_code_);
ROS_INFO("marker6");
/* CALL THE PLANNER */
// context->solve(res);
// ROS_INFO("marker7");
/* Check that the planning was successful */
if(res.error_code_.val != res.error_code_.SUCCESS)
{
ROS_ERROR("Could not compute plan successfully");
return 0;
}
/* Visualize the generated plan */
/* Publisher for display */
ros::Publisher display_publisher = node_handle.advertise<moveit_msgs::DisplayTrajectory>("/move_group/display_planned_path", 1, true);
moveit_msgs::DisplayTrajectory display_trajectory;
/* Visualize the trajectory */
ROS_INFO("Visualizing the trajectory");
moveit_msgs::MotionPlanResponse response;
res.getMessage(response);
display_trajectory.trajectory_start = response.trajectory_start;
display_trajectory.trajectory.push_back(response.trajectory);
display_publisher.publish(display_trajectory);
sleep_time.sleep();
/* NOW TRY A JOINT SPACE GOAL */
/* First, set the state in the planning scene to the final state of the last plan */
robot_state::RobotState& robot_state = planning_scene->getCurrentStateNonConst();
planning_scene->setCurrentState(response.trajectory_start);
robot_state::JointStateGroup* joint_state_group = robot_state.getJointStateGroup("manipulator");
joint_state_group->setVariableValues(response.trajectory.joint_trajectory.points.back().positions);
/* Now, setup a joint space goal*/
robot_state::RobotState goal_state(robot_model);
robot_state::JointStateGroup* goal_group = goal_state.getJointStateGroup("manipulator");
std::vector<double> joint_values(7, 0.0);
// joint_values[0] = 2.0;
joint_values[2] = 1.6;
// joint_values[5] = -0.15;
goal_group->setVariableValues(joint_values);
moveit_msgs::Constraints joint_goal = kinematic_constraints::constructGoalConstraints(goal_group);
req.goal_constraints.clear();
req.goal_constraints.push_back(joint_goal);
/* Construct the planning context */
context = planner_instance->getPlanningContext(planning_scene, req, res.error_code_);
/* Call the Planner */
context->solve(res);
/* Check that the planning was successful */
if(res.error_code_.val != res.error_code_.SUCCESS)
{
ROS_ERROR("Could not compute plan successfully");
return 0;
}
/* Visualize the trajectory */
ROS_INFO("Visualizing the trajectory");
res.getMessage(response);
display_trajectory.trajectory_start = response.trajectory_start;
display_trajectory.trajectory.push_back(response.trajectory);
//Now you should see two planned trajectories in series
display_publisher.publish(display_trajectory);
/* Now, let's try to go back to the first goal*/
/* First, set the state in the planning scene to the final state of the last plan */
joint_state_group->setVariableValues(response.trajectory.joint_trajectory.points.back().positions);
/* Now, we go back to the first goal*/
req.goal_constraints.clear();
req.goal_constraints.push_back(pose_goal);
context = planner_instance->getPlanningContext(planning_scene, req, res.error_code_);
context->solve(res);
res.getMessage(response);
display_trajectory.trajectory.push_back(response.trajectory);
display_publisher.publish(display_trajectory);
/* Let's create a new pose goal */
pose.pose.position.x = 0.65;
pose.pose.position.y = -0.2;
pose.pose.position.z = -0.1;
moveit_msgs::Constraints pose_goal_2 = kinematic_constraints::constructGoalConstraints("wrist_3_link", pose, tolerance_pose, tolerance_angle);
/* First, set the state in the planning scene to the final state of the last plan */
joint_state_group->setVariableValues(response.trajectory.joint_trajectory.points.back().positions);
/* Now, let's try to move to this new pose goal*/
req.goal_constraints.clear();
req.goal_constraints.push_back(pose_goal_2);
/* But, let's impose a path constraint on the motion.
Here, we are asking for the end-effector to stay level*/
geometry_msgs::QuaternionStamped quaternion;
quaternion.header.frame_id = "base_link";
quaternion.quaternion.w = 1.0;
req.path_constraints = kinematic_constraints::constructGoalConstraints("wrist_3_link", quaternion);
// imposing path constraints requires the planner to reason in the space of possible positions of the end-effector
// (the workspace of the robot)
// because of this, we need to specify a bound for the allowed planning volume as well;
// Note: a default bound is automatically filled by the WorkspaceBounds request adapter (part of the OMPL pipeline,
// but that is not being used in this example).
// We use a bound that definitely includes the reachable space for the arm. This is fine because sampling is not done in this volume
// when planning for the arm; the bounds are only used to determine if the sampled configurations are valid.
req.workspace_parameters.min_corner.x = req.workspace_parameters.min_corner.y = -2.0;
req.workspace_parameters.min_corner.z = 0.2;
req.workspace_parameters.max_corner.x = req.workspace_parameters.max_corner.y = req.workspace_parameters.max_corner.z = 2.0;
context = planner_instance->getPlanningContext(planning_scene, req, res.error_code_);
context->solve(res);
res.getMessage(response);
display_trajectory.trajectory.push_back(response.trajectory);
//Now you should see four planned trajectories in series
display_publisher.publish(display_trajectory);
sleep_time.sleep();
ROS_INFO("Done");
planner_instance.reset();
return 0;
}
void FootstepPlanner::planCB(
const jsk_footstep_msgs::PlanFootstepsGoal::ConstPtr& goal)
{
boost::mutex::scoped_lock lock(mutex_);
latest_header_ = goal->goal_footstep.header;
JSK_ROS_INFO("planCB");
// check message sanity
if (goal->initial_footstep.footsteps.size() == 0) {
JSK_ROS_ERROR("no initial footstep is specified");
as_.setPreempted();
return;
}
if (goal->goal_footstep.footsteps.size() != 2) {
JSK_ROS_ERROR("Need to specify 2 goal footsteps");
as_.setPreempted();
return;
}
if (use_pointcloud_model_ && !pointcloud_model_) {
JSK_ROS_ERROR("No pointcloud model is yet available");
as_.setPreempted();
return;
}
//ros::WallDuration timeout(goal->timeout.expectedCycleTime().toSec());
ros::WallDuration timeout(10.0);
Eigen::Vector3f footstep_size(footstep_size_x_, footstep_size_y_, 0.000001);
////////////////////////////////////////////////////////////////////
// set start state
// 0 is always start
Eigen::Affine3f start_pose;
tf::poseMsgToEigen(goal->initial_footstep.footsteps[0].pose, start_pose);
FootstepState::Ptr start(FootstepState::fromROSMsg(
goal->initial_footstep.footsteps[0],
footstep_size,
Eigen::Vector3f(resolution_x_,
resolution_y_,
resolution_theta_)));
graph_->setStartState(start);
if (project_start_state_) {
if (!graph_->projectStart()) {
JSK_ROS_ERROR("Failed to project start state");
publishText(pub_text_,
"Failed to project start",
ERROR);
as_.setPreempted();
return;
}
}
////////////////////////////////////////////////////////////////////
// set goal state
jsk_footstep_msgs::Footstep left_goal, right_goal;
for (size_t i = 0; i < goal->goal_footstep.footsteps.size(); i++) {
FootstepState::Ptr goal_state(FootstepState::fromROSMsg(
goal->goal_footstep.footsteps[i],
footstep_size,
Eigen::Vector3f(resolution_x_,
resolution_y_,
resolution_theta_)));
if (goal_state->getLeg() == jsk_footstep_msgs::Footstep::LEFT) {
graph_->setLeftGoalState(goal_state);
left_goal = goal->goal_footstep.footsteps[i];
}
else if (goal_state->getLeg() == jsk_footstep_msgs::Footstep::RIGHT) {
graph_->setRightGoalState(goal_state);
right_goal = goal->goal_footstep.footsteps[i];
}
else {
JSK_ROS_ERROR("unknown goal leg");
as_.setPreempted();
return;
}
}
if (project_goal_state_) {
if (!graph_->projectGoal()) {
JSK_ROS_ERROR("Failed to project goal");
as_.setPreempted();
publishText(pub_text_,
"Failed to project goal",
ERROR);
return;
}
}
// set parameters
if (use_transition_limit_) {
graph_->setTransitionLimit(
TransitionLimitXYZRPY::Ptr(new TransitionLimitXYZRPY(
transition_limit_x_,
transition_limit_y_,
transition_limit_z_,
transition_limit_roll_,
transition_limit_pitch_,
transition_limit_yaw_)));
}
else {
graph_->setTransitionLimit(TransitionLimitXYZRPY::Ptr());
}
graph_->setLocalXMovement(local_move_x_);
graph_->setLocalYMovement(local_move_y_);
graph_->setLocalThetaMovement(local_move_theta_);
graph_->setLocalXMovementNum(local_move_x_num_);
graph_->setLocalYMovementNum(local_move_y_num_);
graph_->setLocalThetaMovementNum(local_move_theta_num_);
graph_->setPlaneEstimationMaxIterations(plane_estimation_max_iterations_);
graph_->setPlaneEstimationMinInliers(plane_estimation_min_inliers_);
graph_->setPlaneEstimationOutlierThreshold(plane_estimation_outlier_threshold_);
graph_->setSupportCheckXSampling(support_check_x_sampling_);
graph_->setSupportCheckYSampling(support_check_y_sampling_);
graph_->setSupportCheckVertexNeighborThreshold(support_check_vertex_neighbor_threshold_);
// Solver setup
FootstepAStarSolver<FootstepGraph> solver(graph_,
close_list_x_num_,
close_list_y_num_,
close_list_theta_num_,
profile_period_,
cost_weight_,
heuristic_weight_);
if (heuristic_ == "step_cost") {
solver.setHeuristic(boost::bind(&FootstepPlanner::stepCostHeuristic, this, _1, _2));
}
else if (heuristic_ == "zero") {
solver.setHeuristic(boost::bind(&FootstepPlanner::zeroHeuristic, this, _1, _2));
}
else if (heuristic_ == "straight") {
solver.setHeuristic(boost::bind(&FootstepPlanner::straightHeuristic, this, _1, _2));
}
else if (heuristic_ == "straight_rotation") {
solver.setHeuristic(boost::bind(&FootstepPlanner::straightRotationHeuristic, this, _1, _2));
}
else {
JSK_ROS_ERROR("Unknown heuristics");
as_.setPreempted();
return;
}
solver.setProfileFunction(boost::bind(&FootstepPlanner::profile, this, _1, _2));
ros::WallTime start_time = ros::WallTime::now();
std::vector<SolverNode<FootstepState, FootstepGraph>::Ptr> path = solver.solve(timeout);
ros::WallTime end_time = ros::WallTime::now();
double planning_duration = (end_time - start_time).toSec();
JSK_ROS_INFO_STREAM("took " << planning_duration << " sec");
JSK_ROS_INFO_STREAM("path: " << path.size());
if (path.size() == 0) {
JSK_ROS_ERROR("Failed to plan path");
publishText(pub_text_,
"Failed to plan",
ERROR);
as_.setPreempted();
return;
}
// Convert path to FootstepArray
jsk_footstep_msgs::FootstepArray ros_path;
ros_path.header = goal->goal_footstep.header;
for (size_t i = 0; i < path.size(); i++) {
ros_path.footsteps.push_back(*path[i]->getState()->toROSMsg());
}
// finalize path
if (path[path.size() - 1]->getState()->getLeg() == jsk_footstep_msgs::Footstep::LEFT) {
ros_path.footsteps.push_back(right_goal);
ros_path.footsteps.push_back(left_goal);
}
else if (path[path.size() - 1]->getState()->getLeg() == jsk_footstep_msgs::Footstep::RIGHT) {
ros_path.footsteps.push_back(left_goal);
ros_path.footsteps.push_back(right_goal);
}
result_.result = ros_path;
as_.setSucceeded(result_);
pcl::PointCloud<pcl::PointNormal> close_list_cloud, open_list_cloud;
solver.openListToPointCloud(open_list_cloud);
solver.closeListToPointCloud(close_list_cloud);
publishPointCloud(close_list_cloud, pub_close_list_, goal->goal_footstep.header);
publishPointCloud(open_list_cloud, pub_open_list_, goal->goal_footstep.header);
publishText(pub_text_,
(boost::format("Planning took %f sec\n%lu path\nopen list: %lu\nclose list:%lu")
% planning_duration % path.size()
% open_list_cloud.points.size()
% close_list_cloud.points.size()).str(),
OK);
}
TEST_F(Pr2SBPLPlannerTester, ManyPlan)
{
planning_scene::PlanningScenePtr ps(new planning_scene::PlanningScene());
ps->setActiveCollisionDetector(collision_detection::CollisionDetectorAllocatorHybridROS::create());
ps->configure(rml_->getURDF(), rml_->getSRDF());
ASSERT_TRUE(ps->isConfigured());
ps->getAllowedCollisionMatrixNonConst() = *acm_;
planning_models::RobotState *::JointStateGroup* start_jsg = ps->getCurrentState().getJointStateGroup("right_arm");
planning_models::RobotState *goal_state(ps->getCurrentState());
planning_models::RobotState *::JointStateGroup* goal_jsg = goal_state.getJointStateGroup("right_arm");
sbpl_interface::SBPLInterface sbpl_planner(ps->getRobotModel());
moveit_msgs::GetMotionPlan::Request mplan_req;
mplan_req.motion_plan_request.group_name = "right_arm";
mplan_req.motion_plan_request.num_planning_attempts = 5;
mplan_req.motion_plan_request.allowed_planning_time = ros::Duration(5.0);
const std::vector<std::string>& joint_names = ps->getRobotModel()->getJointModelGroup("right_arm")->getJointModelNames();
mplan_req.motion_plan_request.goal_constraints.resize(1);
mplan_req.motion_plan_request.goal_constraints[0].joint_constraints.resize(joint_names.size());
for(unsigned int i = 0; i < joint_names.size(); i++)
{
mplan_req.motion_plan_request.goal_constraints[0].joint_constraints[i].joint_name = joint_names[i];
mplan_req.motion_plan_request.goal_constraints[0].joint_constraints[i].position = 0.0;
mplan_req.motion_plan_request.goal_constraints[0].joint_constraints[i].tolerance_above = 0.001;
mplan_req.motion_plan_request.goal_constraints[0].joint_constraints[i].tolerance_below = 0.001;
mplan_req.motion_plan_request.goal_constraints[0].joint_constraints[i].weight = 1.0;
}
unsigned int comp_trials = 0;
unsigned int succ_trials = 0;
double max_planning_time = 0.0;
double total_planning_time = 0.0;
while(comp_trials < NUM_TRIALS) {
while(1) {
start_jsg->setToRandomValues();
goal_jsg->setToRandomValues();
std::vector<double> goal_vals;
goal_jsg->getGroupStateValues(goal_vals);
for(unsigned int i = 0; i < joint_names.size(); i++) {
mplan_req.motion_plan_request.goal_constraints[0].joint_constraints[i].position = goal_vals[i];
}
std::vector<double> start_vals;
start_jsg->getGroupStateValues(start_vals);
for(unsigned int i = 0; i < start_vals.size(); i++) {
std::cerr << "Start joint " << i << " val " << start_vals[i] << std::endl;
}
for(unsigned int i = 0; i < goal_vals.size(); i++) {
std::cerr << "Goal joint " << i << " val " << goal_vals[i] << std::endl;
}
moveit_msgs::GetMotionPlan::Response mplan_res;
if(sbpl_planner.solve(ps,
mplan_req,
mplan_res)) {
comp_trials++;
succ_trials++;
if(sbpl_planner.getLastPlanningStatistics().total_planning_time_.toSec() > max_planning_time) {
max_planning_time = sbpl_planner.getLastPlanningStatistics().total_planning_time_.toSec();
}
ASSERT_LT(sbpl_planner.getLastPlanningStatistics().total_planning_time_.toSec(), 5.0);
total_planning_time += sbpl_planner.getLastPlanningStatistics().total_planning_time_.toSec();
break;
} else {
if(mplan_res.error_code.val != moveit_msgs::MoveItErrorCodes::GOAL_IN_COLLISION &&
mplan_res.error_code.val != moveit_msgs::MoveItErrorCodes::START_STATE_IN_COLLISION) {
std::cerr << "Bad error code " << mplan_res.error_code.val << std::endl;
comp_trials++;
break;
} else {
//std::cerr << "Something in collision" << std::endl;
}
}
}
}
std::cerr << "Average planning time " << total_planning_time/(comp_trials*1.0) << " max " << max_planning_time << std::endl;
EXPECT_EQ(succ_trials, comp_trials);
//ASSERT_EQ(mplan_res.error_code.val, mplan_res.error_code.SUCCESS);
//EXPECT_GT(mplan_res.trajectory.joint_trajectory.points.size(), 0);
}
