ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

Defining trajectory_msgs/MultiDOFJointTrajectory in ROS

asked 2019-05-23 00:17:14 -0500

ddgenova gravatar image

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.

edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
0

answered 2020-10-23 20:17:11 -0500

First, you need to remove the last line of the constructor:

self.msg.points=2.

Also, to add the new points to the trajectory you need to use the append function:

self.msg.points.append(p) instead of self.msg.points(i) = p

edit flag offensive delete link more
1

answered 2019-05-23 02:50:55 -0500

gvdhoorn gravatar image

This is more a Python problem than anything else, but:

self.msg = MultiDOFJointTrajectory()
[..]
self.msg.points(i) = p

From the documentation on MultiDOFJointTrajectory (here) we see that points is an unbounded list of trajectory_msgs/MultiDOFJointTrajectoryPoint instances.

Those are lists in Python as well, so you'll need to use the regular ways to interact with list types in Python, such as [] and append(..), insert(..), etc.

In this case you'll probably want to use append(..):

self.msg.points.append(p)
edit flag offensive delete link more

Question Tools

1 follower

Stats

Asked: 2019-05-23 00:17:14 -0500

Seen: 1,278 times

Last updated: May 23 '19