Plan out of vision

Basic description

Services for planning out of vision are providing by the node plan_out_of_vision in the clopema_arm_navigation package.
There are two services provided
  • /plan_out_of_vision/plan
    Type of the service is clopema_arm_navigation/ClopemaPlanOutOfVision.srv
    The input is configuration for the planner, containing the following information:
    #The following variables are used to determine which arm can move and which arm will be enabled in 'camera-vision' collision checking. 
    bool r1_arm
    bool r2_arm
    
    #The following variables enables/disables collision checking for the camera-vision.
    bool r1_kinect - if set to true the result pose of the robot will not be seen from camera mounted on first arm
    bool r2_kinect - if set to true the result pose of the robot will not be seen from camera mounted on second arm
    

The output is the planned trajectory which can be merged with other planners output (see More points for information how to merge trajectories).

  • /plan_out_of_vision/visualize_vision
    Type of the service is std_srvs/Empty.srv.
    This service can be used to publish 'camera-vision' to the rviz - the 'camera-vision' objects are published on topic state_validity_markers_array with duration set to 2.0 seconds.

Planner in work

Youtube video
To launch all nodes, producing similar output as in the video, type:

roslaunch clopema_planning_tutorials plan_out_of_vision_gui.launch

To display menu for planing out of vision add Interactive Marker (listening on topic 'vision_controls/update') to the rviz. To enable interaction click on Interact button in the top left corner of the rviz.

Example

The following code will plan and execute the planned trajectory on the robot. The code is almost same as the code executed in video after "Plan dual" button is pressed.

    actionlib::SimpleActionClient<control_msgs::FollowJointTrajectoryAction> move("/clopema_controller/follow_joint_trajectory", true);
    move.waitForServer();

//Set the planner (what will be check for the collisions, what will move in resulting trajectory)
    clopema_arm_navigation::ClopemaPlanOutOfVision p;
    p.request.r1_arm = true;
    p.request.r2_arm = true;
    p.request.r1_kinect = true;
    p.request.r2_kinect = true;

    if (!ros::service::call("/plan_out_of_vision/plan", p)) {
        ROS_ERROR("Can't call plan_out_of_vision service");
        return ;
    }

    if (p.response.error_code.val != p.response.error_code.SUCCESS) {
        ROS_ERROR_STREAM("Can't plan out of vision, error code: " << p.response.error_code.val);
        return ;
    } else {
        ROS_INFO_STREAM("Successfully planned");
    }
//Planning completed - sent trajectory to the robot

//Create goal and send it to the controller
    control_msgs::FollowJointTrajectoryGoal goal;
    goal.trajectory = p.response.joint_trajectory;
    move.sendGoal(goal);

    bool finished_within_time = false;
    finished_within_time = move.waitForResult(ros::Duration(45.0));

    if (!finished_within_time) {
        move.cancelGoal();
        ROS_INFO("Timed out achieving goal A");
    } else {
        actionlib::SimpleClientGoalState state = move.getState();
        bool success = (state == actionlib::SimpleClientGoalState::SUCCEEDED);
        if (success)
            ROS_INFO("Action finished: %s", state.toString().c_str());
        else {
            ROS_INFO("Action failed: %s", state.toString().c_str());
            ROS_WARN("Addition information: %s", state.text_.c_str());
        }
    }