Planning scene [closed]
Hi everyone, I'm not sure, If I'm doing the correct using the planning_environtment. Base on WorldInterface class in pyton, and other examples in Internet, I did my World Interface class (after the text).
What I really want is create a world (with some objects) and move the arm with ompl a and interactuate with the objects. For this I do (schematically):
Do I use the planning_enviorenment wrong? I cant obtain a good plan.
My problem is: when I request a plan, I obtain a code Error: -1.
Regards.
World_interface world;
...
arm_navigation_msgs::CollisionObject pieza1_object;
pieza1_object.id = "pieza1_pole";
pieza1_object.header.frame_id = "/odom_combined";
...
world.add_object(pieza1_object);
world.add_object(arrayObj[1]);
world.add_object(arrayObj[2]);
...
plan_request.motion_plan_request.goal_constraints.position_constraints[0].position.x = arrayObj[i].poses[0].position.x;
plan_request.motion_plan_request.goal_constraints.position_constraints[0].position.y = arrayObj[i].poses[0].position.y;
...
service_client.call(plan_request,plan_response);
if(plan_response.error_code.val != plan_response.error_code.SUCCESS) {
...
class World_interface {
private:
//Atributes:
ros::NodeHandle nh;
ros::Publisher collision_object_pub;
ros::Publisher attached_object_pub;
ros::ServiceClient get_planning_scene; //Returns the current state of the world.
ros::ServiceClient set_planning_scene_diff;
std::string world_frame; //(string): The frame in which objects are added to the collision map and also the frame in
//which planning is done. Usually this is /map, but if not using a map it will be /odom_combined. It is
//found by looking at the parent of the multi DOF joint in the robot state.
std::string robot_frame; //(string): The root frame of the robot's link tree. This has the same orientation as the
//world frame but moves with the robot. Usually it is /base_footprint. It is found by looking at the child
//of the multi DOF joint in the robot state.
std::string hands; // (dictionary): Hand descriptions for the robot indexed by 'left_arm' and 'right_arm'. For more
//information about hand descriptions, see hand_description.py
//Metodos:
void publish(arm_navigation_msgs::AttachedCollisionObject msg_obj, ros::Publisher publisher) {
arm_navigation_msgs::SetPlanningSceneDiff::Request set_planning_scene_diff_req;
arm_navigation_msgs::SetPlanningSceneDiff::Response set_planning_scene_diff_res;
ros::Duration(0.5).sleep();
if(publisher == attached_object_pub) {
publisher.publish(msg_obj);
}
else {
publisher.publish(msg_obj.object);
}
ros::Duration(0.5).sleep();
//Actualiza el environtment
if(!set_planning_scene_diff.call(set_planning_scene_diff_req, set_planning_scene_diff_res)) {
ROS_ERROR("Can't set planning scene");
}
}
public:
World_interface() {
//Servicios:
while(!ros::service::waitForService(GET_PLANNING_SCENE_NAME, ros::Duration(1.0)))
ROS_WARN("waiting for %s",GET_PLANNING_SCENE_NAME.c_str());
while(!ros::service::waitForService(SET_PLANNING_SCENE_DIFF_NAME, ros::Duration(1.0)))
ROS_WARN("waiting for %s",SET_PLANNING_SCENE_DIFF_NAME.c_str());
get_planning_scene = nh.serviceClient<arm_navigation_msgs::GetPlanningScene>(GET_PLANNING_SCENE_NAME);
set_planning_scene_diff = nh.serviceClient<arm_navigation_msgs::SetPlanningSceneDiff>(SET_PLANNING_SCENE_DIFF_NAME);
//Publicadores:
collision_object_pub = nh.advertise<arm_navigation_msgs::CollisionObject>("/collision_object", 10);
attached_object_pub = nh.advertise<arm_navigation_msgs::AttachedCollisionObject>("/attached_collision_object", 10);
world_frame = "odom_combined";
robot_frame = "/base_footprint";
hands = "right_arm";
ROS_INFO("World interface initialized");
}
~World_interface() {
//ros::shutdown();
}
void reset_collision_objects() {
//Removes all collision objects (but not attached objects!) from the collision map.
arm_navigation_msgs::AttachedCollisionObject obj;
obj.object = get_reset_object();
this->publish(obj, collision_object_pub);
ROS_INFO("Collision object reset");
}
void reset_attached_objects() {
//Removes all collision objects that are currently attached to the robot.
arm_navigation_msgs::AttachedCollisionObject obj;
obj.link_name = "all";
obj.object.header.frame_id = world_frame; //Frame referencia.
obj.object.header.stamp = ros::Time::now();
obj.object = get_reset_object();
this->publish(obj, attached_object_pub);
ROS_INFO("Attached object reset");
}
arm_navigation_msgs ...