0
$\begingroup$

I'm following the tutorial of MoveIt to add obstacles to the scene.

Everything goes well and I can see obstacles in Rviz. However, the planned path seems to ignore the presence of obstacles and go through them.

Here is the code snippet that is used to add obstacles:

moveit_msgs::PlanningScene planning_scene;
planning_scene.world.collision_objects.push_back(attached_object.object);
planning_scene.is_diff = true;
planning_scene_diff_publisher.publish(planning_scene);

I follow this tutorial to load the OMPL plugin.

Actually, I know this problem could be solved with the move group interface, but its member function setGoalOrientationTolerance can't set orientation tolerance for different directions. So I refer to this procedure to set the pose goal.

Or if there is any way to utilize the move group interface to set a pose goal with different tolerances for different axes, I can skip the above hassle.

Help would be appreciated! Thanks!

$\endgroup$

1 Answer 1

0
$\begingroup$

This is my solution, feel free to comment if there is a better way! Note that the code just lists the procedure, not out-of-the-box.

  planning_interface::MotionPlanRequest req;
  planning_interface::MotionPlanResponse res;

  geometry_msgs::PoseStamped pose;

  std::vector<double> tolerance_position;
  std::vector<double> tolerance_angle;

  moveit_msgs::Constraints pose_goal = kinematic_constraints::constructGoalConstraints("end-effector", pose, tolerance_position, tolerance_angle);

  req.group_name = PLANNING_GROUP;
  req.goal_constraints.push_back(pose_goal);

  // set start state
  start_state.setJointGroupPositions(joint_model_group, start_joint_values);

  moveit_msgs::RobotState start_msgs;
  moveit::core::robotStateToRobotStateMsg(start_state, start_msgs);

  req.start_state = start_msgs;

  moveit_msgs::MoveGroupGoal move_group_goal;
  move_group_goal.request = req;
  move_group_goal.planning_options.plan_only = true;
  move_group_goal.planning_options.look_around = false;
  move_group_goal.planning_options.replan = false;
  move_group_goal.planning_options.planning_scene_diff.is_diff = true;
  move_group_goal.planning_options.planning_scene_diff.robot_state.is_diff = true;

  actionlib::SimpleActionClient<moveit_msgs::MoveGroupAction>&
      move_action_client_custom = move_group_interface.getMoveGroupClient();

  move_action_client_custom.sendGoal(move_group_goal);

  if (!move_action_client_custom.waitForResult()) {
    ROS_INFO_STREAM("MoveGroup action returned early");
  }
  if (move_action_client_custom.getState() == actionlib::SimpleClientGoalState::SUCCEEDED) {
    ROS_INFO_STREAM("MoveGroup has finished the plan successfully and costed "
                    << move_action_client_custom.getResult()->planning_time << " seconds");
  } else {
    ROS_WARN_STREAM("Fail: " << move_action_client_custom.getState().toString()
                             << ": "
                             << move_action_client_custom.getState().getText());
  }
$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.