Trajectory goes across the obstacle. Problems using PlanningSCeneMonitor and planning_scene topic?

edit

Hello, :)

I'm trying to generate a trajectory to a scene which shows an object placed along the trajectory Generated without the Same Subject. I wrote because codes.

1) in the first code I Created the subject and added to the scene, publishing MESSAGE on "planning_scene" and using ApplyPlanningScene service:

// Advertise the required topic ros::Publisher planning_scene_diff_publisher = node_handle.advertise<moveit_msgs::planningscene>("planning_scene", 1); while (planning_scene_diff_publisher.getNumSubscribers() < 1) { ros::WallDuration sleep_t(0.5); sleep_t.sleep(); }

// Service ros::ServiceClient planning_scene_diff_client = node_handle.serviceClient<moveit_msgs::applyplanningscene>("apply_planning_scene"); planning_scene_diff_client.waitForExistence();

// and send the diffs to the planning scene via a service call: moveit_msgs::ApplyPlanningScene srv;

// Adding object ROS_INFO("Creating BOX object in the scene:"); moveit_msgs::PlanningScene planning_scene_msg;

planning_scene_msg.is_diff = true; planning_scene_msg.robot_state.is_diff = true;

moveit_msgs::CollisionObject collision_object; collision_object.header.frame_id = "world"; collision_object.id = "box1"; //the id is used to identify the obj

shape_msgs::SolidPrimitive primitive; primitive.type = primitive.BOX; primitive.dimensions.resize(3); primitive.dimensions[0] = 0.1; primitive.dimensions[1] = 0.4; primitive.dimensions[2] = 0.1;

geometry_msgs::Pose box_pose; box_pose.orientation.w = 1.0; box_pose.position.x = 0.2; box_pose.position.y = 0.1; box_pose.position.z = 1;

collision_object.primitives.push_back(primitive); collision_object.primitive_poses.push_back(box_pose);

//add the collision object into the world collision_object.operation = collision_object.ADD; ROS_INFO("Add the object into the world"); planning_scene_msg.world.collision_objects.push_back(collision_object);

srv.request.scene = planning_scene_msg; planning_scene_diff_client.call(srv);

planning_scene_diff_publisher.publish(planning_scene_msg);

sleep_time.sleep();

2) in another code that must acquire the scene and generate the trajectory whereas the presence of new object:

robot_model_loader::RobotModelLoader robot_model_loader("robot_description"); robot_model::RobotModelPtr robot_model = robot_model_loader.getModel(); planning_scene::PlanningScenePtr planning(new planning_scene::PlanningScene(robot_model)); ros::WallDuration sleep_time(10.0); sleep_time.sleep();

boost::shared_ptr<tf::transformlistener> tf(new tf::TransformListener(ros::Duration(10.0))); planning_scene_monitor::PlanningSceneMonitorPtr psm(new planning_scene_monitor::PlanningSceneMonitor("robot_description", tf));

psm->requestPlanningSceneState("get_planning_scene");

planning_scene_monitor::LockedPlanningSceneRW ps(psm); ps->getCurrentStateNonConst().update(); planning_scene::PlanningScenePtr plans = ps->diff(); plans->decoupleParent();

planning_pipeline::PlanningPipelinePtr planning_pipeline(new planning_pipeline::PlanningPipeline(robot_model, node_handle, "planning_plugin", "request_adapters"));

[......···]

planning_pipeline->generatePlan(plans, req, res);

the problem is that when launch the node which publishes the topic "planning_scene" (having already launched rosrun <package> move_group.launch) RViz the scene is changed, but the trajectory that is generated does not see the object that was added, but through it and then wrong. the procedure is correct or have I done something wrong?

thank you to those who will say,