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

# Trajectory Planning Algorithm Python ROS Program

I am currently trying to write a Python ROS program which can be executed as a ROS node (using rosrun) that implements the defs declared in a separate Python file arm.py (available at: https://github.com/nortega1/dvrk-ros/...). The program initially examines the current cartesian position of the arm. Subsequently, when provided with a series of points that the arm must pass through, the program calculates a polynomial equation and given a range of x values the program evaluates the equation to find the corresponding y values.

Within the arm.py file there is a publisher set_position_cartesian_pub that sets the Cartesian position of the arm as follows:

self.__set_position_cartesian_pub = rospy.Publisher(self.__full_ros_namespace + '/set_position_cartesian', Pose, latch = True, queue_size = 1)


However, I am unsure how to pass the x and y values (I'll calculate the z values at a later date) to the publisher in the python program that I am creating. This is what I have written thus far:

#!/usr/bin/env python

import rospy
from tf import transformations
from tf_conversions import posemath
from std_msgs.msg import String, Bool, Float32, Empty, Float64MultiArray

from geometry_msgs.msg import Pose, PoseStamped, Vector3, Quaternion, Wrench, WrenchStamped, TwistStamped

def callback(data):
rospy.loginfo(data.pose)

def currentPositionListener():
rospy.init_node('currentPositionListener', anonymous=True)
rospy.Subscriber('/dvrk/PSM1/position_cartesian_current', PoseStamped, callback)
rospy.spin()

def lagrange(f, x):

total = 0
n = len(f)
for i in range(n):
xi, yi = f[i]

def g(i, n):
g_tot = 1
for j in range(n):
if i == j:
continue
xj, yj = f[j]
g_tot *= (x - xj) / float(xi - xj)

return g_tot

total += yi * g(i, n)
return total

def newPositionListener():
rospy.Subscriber('/dvrk/PSM1/set_position_cartesian', PoseStamped, trajectoryMover)
rospy.spin()

def trajectoryMover(data):
points =[(0,0),(45,30),(23,10), (48,0)]
xlist = [i for i in range(100)]
ylist = [lagrange(points, xlist[i]) for i in range(100)]
for x, y in zip(xlist, ylist):
data.pose.x = x
data.pose.y = y
data.pose.z = 0

if __name__ == '__main__':
currentPositionListener()
newPositionListener()


Any help would be greatly appreciated!

edit retag close merge delete

## 1 Answer

Sort by ยป oldest newest most voted

Reffer to https://answers.ros.org/question/1073....

I think this will help you.

more

## Stats

Asked: 2019-06-19 09:24:53 -0600

Seen: 834 times

Last updated: Nov 06 '19