ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

How to check from C++ code if moveit planning is successful?

asked 2020-02-22 23:01:42 -0500

Tawfiq Chowdhury gravatar image

I want to call a function from Moveit C++ to check if the the planner could plan to a pose, so that if the planning fails, I can call the Moveit pick function with another pose. What is the way to check from code if the planning has been successful or not?

I am looking for something similar

boolean plan=....here goes the function to check if the planning has been successful

if plan==true{
//Do nothing
}
else{
pick (with new pose)
}
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2020-02-22 23:19:20 -0500

Look at below example function, you provide a target goal and constructed movegroup interface of your manipulator.

/**
 * @brief moves robot tcp in joint space, a straight line following is not guarenteed
 *
 * @param robot_tcp_goal_in_joint_space
 * @param move_group_ptr_
 */
void RobotController::moveEndEffectortoGoalinJointSpace(
    geometry_msgs::Pose robot_tcp_goal_in_joint_space,
    moveit::planning_interface::MoveGroupInterface *move_group_ptr_) {
    // set pose target for tcp
    move_group_ptr_->setPoseTarget(robot_tcp_goal_in_joint_space);

    // Now, we call the planner to compute the plan
    moveit::planning_interface::MoveGroupInterface::Plan my_plan;

    // chechk wheter a sucessfull plan was found
    bool success = (move_group_ptr_->plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);

    // stream some info about this sucess state
    ROS_INFO_NAMED("Motion Plan for End-Effector %s", success ? "SUCCESSED" : "FAILED");

    // Move the manipulator according to Updated End-Effector goal pose
    move_group_ptr_->execute(my_plan);
}
edit flag offensive delete link more

Question Tools

1 follower

Stats

Asked: 2020-02-22 23:01:42 -0500

Seen: 433 times

Last updated: Feb 22 '20