test_get_base_pos_changed_plan.py

Go to the documentation of this file.

#! /usr/bin/env python

PKG = 'planning_environment'

import roslib; roslib.load_manifest(PKG)
import rospy
import planning_environment_msgs.srv
import sys
import unittest
import math

import sensor_msgs.msg
import mapping_msgs.msg
from mapping_msgs.msg import CollisionObject
from mapping_msgs.msg import AttachedCollisionObject
from motion_planning_msgs.msg import CollisionOperation, ArmNavigationErrorCodes, AllowedContactSpecification, RobotState, DisplayTrajectory, MotionPlanRequest, JointConstraint
from motion_planning_msgs.srv import GetMotionPlanRequest, GetMotionPlanResponse, GetMotionPlan
from geometric_shapes_msgs.msg import Shape
from geometry_msgs.msg import Pose, PointStamped
from planning_environment_msgs.srv import GetRobotState, GetStateValidity, GetRobotStateRequest, GetStateValidityRequest
from tf import TransformListener

default_prefix = "/environment_server"

class TestGetStateValidity(unittest.TestCase):
    
    def setUp(self):

        rospy.init_node('test_get_base_state_validity')

        self.tf = TransformListener()
        
        self.obj_pub = rospy.Publisher('collision_object',CollisionObject,latch=True)
        self.trajectory_pub = rospy.Publisher('display_path', DisplayTrajectory)

        rospy.wait_for_service('/ompl_planning/plan_kinematic_path')
        self.motion_planner = rospy.ServiceProxy('/ompl_planning/plan_kinematic_path', GetMotionPlan)

        obj1 = CollisionObject()

        obj1.header.stamp = rospy.Time.now()
        obj1.header.frame_id = 'odom_combined'
        obj1.id = 'pole'
        obj1.operation.operation = mapping_msgs.msg.CollisionObjectOperation.ADD
        obj1.shapes = [Shape() for _ in range(1)]
        obj1.shapes[0].type = Shape.BOX
        obj1.shapes[0].dimensions = [float() for _ in range(3)]
        obj1.shapes[0].dimensions[0] = .1
        obj1.shapes[0].dimensions[1] = .1
        obj1.shapes[0].dimensions[2] = 1.0
        obj1.poses = [Pose() for _ in range(1)]
        obj1.poses[0].position.x = 4.55
        obj1.poses[0].position.y = -.5
        obj1.poses[0].position.z = .75
        obj1.poses[0].orientation.x = 0
        obj1.poses[0].orientation.y = 0
        obj1.poses[0].orientation.z = 0
        obj1.poses[0].orientation.w = 1
    
        self.obj_pub.publish(obj1)
        
        rospy.sleep(5.)

    def test_get_base_state_motion_plan(self):

        req = GetMotionPlanRequest()
        req.motion_plan_request.group_name = "right_arm"
        req.motion_plan_request.num_planning_attempts = 1
        req.motion_plan_request.allowed_planning_time = rospy.Duration(5.0)
        req.motion_plan_request.planner_id = ""

        joint_names = ['%s_%s' % ('r', j) for j in ['shoulder_pan_joint', 'shoulder_lift_joint', 'upper_arm_roll_joint', 'elbow_flex_joint', 'forearm_roll_joint', 'wrist_flex_joint', 'wrist_roll_joint']]

        req.motion_plan_request.goal_constraints.joint_constraints = [JointConstraint() for i in range(len(joint_names))]
        for i in range(len(joint_names)):
            req.motion_plan_request.goal_constraints.joint_constraints[i].joint_name = joint_names[i]
            req.motion_plan_request.goal_constraints.joint_constraints[i].position = 0.0
            req.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_above = 0.08
            req.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_below = 0.08

        req.motion_plan_request.goal_constraints.joint_constraints[0].position = -2.0
        req.motion_plan_request.goal_constraints.joint_constraints[3].position = -0.2
        req.motion_plan_request.goal_constraints.joint_constraints[5].position = -0.2

        #should be in collision
        req.motion_plan_request.start_state.multi_dof_joint_state.joint_names.append("base_joint")
        req.motion_plan_request.start_state.multi_dof_joint_state.frame_ids.append("odom_combined")
        req.motion_plan_request.start_state.multi_dof_joint_state.child_frame_ids.append("base_footprint")
        pose = Pose()
        pose.position.x = 4.0
        pose.orientation.w = 1.0
        req.motion_plan_request.start_state.multi_dof_joint_state.poses.append(pose)

        res = self.motion_planner.call(req)
        
        print len(res.trajectory.joint_trajectory.joint_names)

        disp = DisplayTrajectory()
        disp.model_id = "right_arm"
        disp.trajectory.joint_trajectory = res.trajectory.joint_trajectory
        disp.robot_state = res.robot_state
        self.trajectory_pub.publish(disp)


if __name__ == '__main__':
    import rostest
    rostest.unitrun('test_get_state_validity', 'test_get_state_validity', TestGetStateValidity, sys.argv)