Defining trajectory_msgs/MultiDOFJointTrajectory in ROS
Hi All,
I am trying to write a code that generate trajectories in python. Currently, I am struggling with formatting the information so that it can be published in a trajectory_msgs/MultiDOFJointTrajectory. As an example here is some really quick code. Effectively, I want to know how to create a trajectory with Cartesian coordinates of x timesteps ahead. Currently I have it at 2, just to test it out.
import rospy
import tf
import math
import numpy as np
import std_msgs.msg
from geometry_msgs.msg import Transform, Quaternion, Point, Twist
from trajectory_msgs.msg import MultiDOFJointTrajectory, MultiDOFJointTrajectoryPoint
class Trajectory():
def __init__(self):h
self.msg = MultiDOFJointTrajectory()
self.header = std_msgs.msg.Header()
self.header.frame_id = 'frame'
self.quaternion = tf.transformations.quaternion_from_euler(0,0,0)
self.velocities = Twist()
self.accelerations = Twist()
self.msg.points = 2
def update(self):
for i in range(0,2):
self.header.stamp = rospy.Time()
self.msg.header = self.header
x = i+1
y = i+2
z = i+3
transforms = Transform(translation=Point(x,y,z), rotation=Quaternion(self.quaternion[0],self.quaternion[1],self.quaternion[2],self.quaternion[3]))
p = MultiDOFJointTrajectoryPoint([transforms], [self.velocities], [self.accelerations], rospy.Time(i))
self.msg.points(i) = p
return self.msg
if __name__ == '__main__':
rospy.init_node('Trajectory_Test', anonymous=False)
r = rospy.Rate(.25) #hz
rospy.loginfo('ProgramSTarted')
traj_pub = rospy.Publisher('/trajectory', MultiDOFJointTrajectory,queue_size=1)
do_it = Trajectory()
while not rospy.is_shutdown():
traj_pub.publish(do_it.update())
r.sleep()
At the Current moment my error is:
line 28 self.msg.points(i) = p
SyntaxError: can't assign to function call
I have tried it a few different ways, and I can't seem to get the formatting just right. Ideally I want to be able to initiialize a message object, and then update that whenever necessary.