rqt_joint_trajectory_plot: joint_trajectory

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 import sys
00004 import actionlib
00005 import argparse
00006 import numpy as np
00007 import rospy
00008 from trajectory_msgs.msg import JointTrajectory,JointTrajectoryPoint
00009 from control_msgs.msg import FollowJointTrajectoryAction, FollowJointTrajectoryGoal
00010 
00011 class JointTrajectoryGenerator:
00012     def __init__(self, time=1.0, step=0.01, freq=1.0, joint_num=1, topic='/joint_trajectory', action='/joint_trajectroy_action'):
00013         self.time = time
00014         self.step = step
00015         self.freq = freq
00016         self.joint_names = ['joint_'+str(n) for n in range(joint_num)]
00017 
00018         self.pub = rospy.Publisher(topic, JointTrajectory, queue_size=10)
00019         self.client = actionlib.SimpleActionClient(
00020             action, FollowJointTrajectoryAction)
00021 
00022     def update(self):
00023         points = []
00024         tick = 0
00025         self.phase = np.random.uniform(np.pi)
00026         while tick <= self.time:
00027             x = 2*np.pi*self.freq*tick + self.phase
00028             positions = [np.sin(x)]*len(self.joint_names)
00029             velocities = [np.cos(x)]*len(self.joint_names)
00030             accelerations = [-np.sin(x)]*len(self.joint_names)
00031             points.append(JointTrajectoryPoint(positions=positions,
00032                                                velocities=velocities,
00033                                                accelerations=accelerations,
00034                                                time_from_start=rospy.Time.from_sec(tick)))
00035             tick += self.step
00036         msg = JointTrajectory(joint_names=self.joint_names, points=points)
00037         msg.header.stamp = rospy.Time.now()
00038         self.pub.publish(msg)
00039 
00040         # joint_trajectroy_action
00041         goal = FollowJointTrajectoryGoal()
00042         goal.trajectory = msg
00043         self.client.send_goal(goal)
00044 
00045 if __name__ == "__main__":
00046     rospy.init_node('joint_trajectory_generator')
00047 
00048     parser = argparse.ArgumentParser()
00049     parser.add_argument('--time-step', type=float, help='Time step between each trajectory point')
00050     parser.add_argument('--topic', type=str, default="/joint_trajectory", help='Topic name to be published')
00051     parser.add_argument('--action', type=str, default="/joint_trajectory_action", help='Action name to be called')
00052     parser.add_argument('--joint-num', type=int, default=1, help='Number of joints')
00053     sys.argv = rospy.myargv()
00054     args = parser.parse_args()
00055 
00056     generator = JointTrajectoryGenerator(joint_num=args.joint_num,
00057                                          topic=args.topic,
00058                                          action=args.action)
00059 
00060     rate = rospy.Rate(1)
00061     while not rospy.is_shutdown():
00062         generator.update()
00063         rate.sleep()