Ask Your Question
0

get_trajectory_validity service not working

asked 2012-01-13 09:05:28 -0500

robbie gravatar image

updated 2012-01-13 09:06:06 -0500

I am trying to create a Python client for the service planning_scene_validity_server/get_trajectory_validity.

When I call the service I seem to only get back the default JointTrajectoryValidityResponse object. The returned object prints as:

Validity response error_code: 
  val: 0
trajectory_error_codes: []
contacts: []

ROS Version: Electric

OS: Ubuntu 11.10

My code is below:

import sys;
import roslib; roslib.load_manifest('fast_planning');
import rospy

from arm_navigation_msgs.srv import GetJointTrajectoryValidity
from arm_navigation_msgs.srv import GetJointTrajectoryValidityRequest
from arm_navigation_msgs.srv import GetJointTrajectoryValidityResponse
from arm_navigation_msgs.srv import SetPlanningSceneDiff
from trajectory_msgs.msg import JointTrajectory
from trajectory_msgs.msg import JointTrajectoryPoint
import numpy

SET_PLANNING_SCENE_DIFF_NAME = "/environment_server/set_planning_scene_diff";
#arg tray, type = trajectory_msgs/JointTrajectory
def get_full_trajectory_validity(traj):
    try:
        rospy.init_node("get_collision_free_ik_py");
        print "waiting for service"
        rospy.wait_for_service("planning_scene_validity_server/get_trajectory_validity");
        print "found service"
        validity_client = rospy.ServiceProxy("planning_scene_validity_server/get_trajectory_validity", GetJointTrajectoryValidity);
        set_planning_scene_diff_client = rospy.ServiceProxy(SET_PLANNING_SCENE_DIFF_NAME, SetPlanningSceneDiff);

        robot_state = set_planning_scene_diff_client().planning_scene.robot_state;
        validity_request = GetJointTrajectoryValidityRequest();
        #validity_request.robot_state = robot_state;

        #All sample points in the trajectory will be checked
        validity_request.check_full_trajectory = True;
        validity_request.check_collisions = True;
        validity_request.check_joint_limits
        validity_request.trajectory = traj
        validity_response = validity_client(validity_request);
        print "Validity response", validity_response
        print "Error code val", validity_response.error_code.val
        print "Error codes", validity_response.trajectory_error_codes

    except rospy.ROSInterruptException:
        print "program interrupted before completion"

if __name__ == '__main__':
    traj = JointTrajectory();

    traj.joint_names.append("r_shoulder_pan_joint")
    sample_points = [];
    start = 3;
    end = -3;
    for i in range(0, 100):
        point = JointTrajectoryPoint();
        point.positions.append(start+(i*.01*(start-end)));
        sample_points.append(point);

    traj.points = sample_points;
    get_full_trajectory_validity(traj)
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2012-01-13 13:37:41 -0500

egiljones gravatar image

The service function is definitely empty right now. I'll release a fix soon, but in the mean time if you need trajectory validity you'll have to use C++, as described in this tutorial:

getting trajectory validity

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

Stats

Asked: 2012-01-13 09:05:28 -0500

Seen: 194 times

Last updated: Jan 13 '12