how to use planning_scene_moniter get current PlanningScene in moveit?

asked 2017-11-26 20:19:14 -0500

cgdsss_ros gravatar image
     planning_scene_monitor_->requestPlanningSceneState(PLANNING_SCENE_SERVICE_);
    planning_scene_monitor::LockedPlanningSceneRW ls(planning_scene_monitor_);
    planning_scene::PlanningScenePtr monitor_planning_scene = planning_scene_monitor_->getPlanningScene();

    planning_scene::PlanningScenePtr planning_scene(new planning_scene::PlanningScene(robot_model_));
    planning_interface::MotionPlanRequest req;
    planning_interface::MotionPlanResponse res;

    robot_state::RobotState goal_state(robot_model_);
    const robot_state::RobotState& robot_state = planning_scene->getCurrentState();
    const robot_state::JointModelGroup* joint_model_group = robot_state.getJointModelGroup("arm");
    goal_state.setJointGroupPositions(joint_model_group, joints);
    moveit_msgs::Constraints joint_goal = kinematic_constraints::constructGoalConstraints(goal_state, joint_model_group);
    req.goal_constraints.clear();
    req.goal_constraints.push_back(joint_goal);
    req.group_name = "arm";

    planning_interface::PlanningContextPtr context =
        planner_instance_->getPlanningContext(monitor_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");
      return 0;
    }

enter code here

I want to plan arm path using planning_interface::PlanningContext Class, According to the tutorial,

Blockquote The PlanningScene class can be easily setup and configured using a RobotModel or a URDF and SRDF. This is, however, not the recommended way to instantiate a PlanningScene. The PlanningSceneMonitor is the recommended method to create and maintain the current planning scene (and is discussed in detail in the next tutorial) using data from the robot’s joints and the sensors on the robot. In this tutorial, we will instantiate a PlanningScene class directly, but this method of instantiation is only intended for illustration.

    planning_scene::PlanningScenePtr planning_scene(new planning_scene::PlanningScene(robot_model_));
    planning_interface::PlanningContextPtr context =
    planner_instance_->getPlanningContext(planning_scene, req, res.error_code_);
    context->solve(res);

this planning_scene instance can calculate a path, but Not from the current position or current scene.

[ERROR] [1511748061.275331061]: Found empty JointState message
[ INFO] [1511748061.275422869]: The timeout for planning must be positive (0.000000 specified). Assuming one second instead.
[ WARN] [1511748061.275478453]: It looks like the planning volume was not specified.
[ INFO] [1511748061.275837940]: No planner specified. Using default.
[ INFO] [1511748061.276242530]: RRTConnect: Starting planning with 1 states already in datastructure
[ INFO] [1511748061.288651292]: RRTConnect: Created 4 states (2 start + 2 goal)
[ INFO] [1511748061.288697757]: Solution found in 0.012655 seconds
[ INFO] [1511748061.289183296]: SimpleSetup: Path simplification took 0.000404 seconds and changed from 3 to 2 states
[ INFO] [1511748061.289351606]: Visualizing the trajectory

image description So, I want calculate a path using current planning scene.

planning_scene_monitor_->requestPlanningSceneState(PLANNING_SCENE_SERVICE_);
planning_scene_monitor::LockedPlanningSceneRW ls(planning_scene_monitor_);
planning_scene::PlanningScenePtr monitor_planning_scene = planning_scene_monitor_->getPlanningScene();
...........
planning_interface::PlanningContextPtr context =
planner_instance_->getPlanningContext(monitor_planning_scene, req, res.error_code_);
context->solve(res);

But it failed!

[ERROR] [1511748777.643164348]: Found empty JointState message
[ INFO] [1511748777.643256969]: The timeout for planning must be positive (0.000000 specified). Assuming one second instead.
[ WARN] [1511748777.643288569]: It looks like the planning volume was not specified.
[ INFO] [1511748777.643763072]: No planner specified. Using default.
[ WARN] [1511748777.644001150]: RRTConnect: Skipping invalid start state (invalid bounds)
[ERROR] [1511748777.644064953]: RRTConnect: Motion planning start tree could not be initialized!
[ INFO] [1511748777.644078287]: No solution found after 0.000104 seconds
[ WARN] [1511748777.653850768]: Goal sampling thread never did any work.
[ INFO] [1511748777.654009292]: Unable to solve the planning problem
[ERROR] [1511748777.654037117]: Could not compute plan successfully

I think there are too few tutorials on planning_scene_monitor and planning_scene. I don't know where the problem is. Please help me. I'll appreciate it!

edit retag flag offensive close merge delete