Ask Your Question
1

how to use planning_scene_moniter get current PlanningScene in moveit?

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

cgdsss_ros gravatar image

updated 2020-11-27 00:11:04 -0600

fvd 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,

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

Comments

It seems like you tried to show an image, but it is broken. What are you trying to do or plan? What code in your post are you executing and which parts are copied from a tutorial for illustration?

fvd gravatar image fvd  ( 2020-11-27 00:12:11 -0600 )edit

2 Answers

Sort by » oldest newest most voted
0

answered 2020-11-27 00:12:46 -0600

fvd gravatar image

In the last output, your start state seems to be collision with something, but it is hard to know what is going on without seeing your robot/scene and the code that you are running.

edit flag offensive delete link more
0

answered 2020-11-25 18:19:17 -0600

updated 2020-11-25 18:19:33 -0600

Not sure if this addresses your issue:

If you want to grab the current motion plan request, do some preprocessing on it, the feed the plan request into a planner and return the plan response, I would recommend using planning adapter. That is the native way for pre-process & post-process it.

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2017-11-26 20:19:14 -0600

Seen: 492 times

Last updated: Nov 27 '20