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 planningscenediffpublisher = nodehandle.advertise("planningscene", 1); while (planningscenediffpublisher.getNumSubscribers() < 1) { ros::WallDuration sleept(0.5); sleept.sleep(); }

// Service ros::ServiceClient planningscenediffclient = nodehandle.serviceClient("applyplanningscene"); planningscenediff_client.waitForExistence();

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

// Adding object ROSINFO("Creating BOX object in the scene:"); moveitmsgs::PlanningScene planningscenemsg;

planningscenemsg.isdiff = true; planningscenemsg.robotstate.is_diff = true;

moveitmsgs::CollisionObject collisionobject; collisionobject.header.frameid = "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;

geometrymsgs::Pose boxpose; boxpose.orientation.w = 1.0; boxpose.position.x = 0.2; boxpose.position.y = 0.1; boxpose.position.z = 1;

collisionobject.primitives.pushback(primitive); collisionobject.primitiveposes.pushback(boxpose);

//add the collision object into the world collisionobject.operation = collisionobject.ADD; ROSINFO("Add the object into the world"); planningscenemsg.world.collisionobjects.pushback(collisionobject);

srv.request.scene = planningscenemsg; planningscenediff_client.call(srv);

planningscenediffpublisher.publish(planningscene_msg);

sleep_time.sleep();

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

robotmodelloader::RobotModelLoader robotmodelloader("robotdescription"); robotmodel::RobotModelPtr robotmodel = robotmodelloader.getModel(); planningscene::PlanningScenePtr planning(new planningscene::PlanningScene(robotmodel)); ros::WallDuration sleeptime(10.0); sleeptime.sleep();

boost::sharedptrtf::TransformListener tf(new tf::TransformListener(ros::Duration(10.0))); planningscenemonitor::PlanningSceneMonitorPtr psm(new planningscenemonitor::PlanningSceneMonitor("robotdescription", tf));

psm->requestPlanningSceneState("getplanningscene");

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

planningpipeline::PlanningPipelinePtr planningpipeline(new planningpipeline::PlanningPipeline(robotmodel, nodehandle, "planningplugin", "request_adapters"));

[......···]

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

the problem is that when launch the node which publishes the topic "planningscene" (having already launched rosrun movegroup.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,