stop_joint_trajectory.py

Go to the documentation of this file.

#!/usr/bin/env python
# -*- coding: utf-8 -*-
#spins off a thread to listen for joint_states messages
#and provides the same information (or subsets of) as a service

import roslib
roslib.load_manifest('coverage_tools')
import rospy
from coverage_tools.srv import *
from sensor_msgs.msg import JointState
from pr2_controllers_msgs.msg import *
from trajectory_msgs.msg import *
import threading
import actionlib

from coverage_tools.srv import ReturnJointStates
import time
import sys

def get_joint_states(joint_names):
    rospy.wait_for_service("return_joint_states")
    try:
        s = rospy.ServiceProxy("return_joint_states", ReturnJointStates)
        resp = s(joint_names)
    except rospy.ServiceException, e:
        print "error when calling return_joint_states: %s"%e
        sys.exit(1)
    for (ind, joint_name) in enumerate(joint_names):
        if(not resp.found[ind]):
            print "joint %s not found!"%joint_name
    return resp.position

def stop_joint_trajectory():
    client = actionlib.SimpleActionClient("r_arm_controller/joint_trajectory_action", pr2_controllers_msgs.msg.JointTrajectoryAction)
    client.wait_for_server()
    joint_names= ["r_shoulder_pan_joint", "r_shoulder_lift_joint","r_upper_arm_roll_joint","r_elbow_flex_joint","r_forearm_roll_joint","r_wrist_flex_joint","r_wrist_roll_joint"]

    goal = pr2_controllers_msgs.msg.JointTrajectoryGoal()
    goal.trajectory.joint_names = joint_names
        
    traj_point = trajectory_msgs.msg.JointTrajectoryPoint()
    traj_point.time_from_start = rospy.Duration.from_sec(1.0)       
    for i in range(0,7):
        traj_point.velocities.append(0)
    pos_list = get_joint_states(joint_names)
    print list(pos_list)
    traj_point.positions = list(pos_list)
    
    print "Calling client to stop trajectory"
    client.send_goal(goal)
    client.wait_for_result()
    return client.get_result()  # A FibonacciResult

#run the server
if __name__ == "__main__":

    rospy.init_node('trajectory_stopper')    
    print "trying to stop trajectory"
    print stop_joint_trajectory()
    rospy.spin()