Ask Your Question
0

Why is ROS Publisher not publishing?

asked 2019-06-24 05:49:55 -0600

691TReilly gravatar image

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)

The issue is that the publisher set_position_cartesian is not publishing the values of the newPose to the robot - can anyone figure out what the issue might be? I can confirm that the def lagrange correctly calculates the values of the x and y coordinates, which are printed out to the terminal via the command rospy.loginfo(newPose). Any help would be greatly appreciated as I've been trying to solve this issue for the last 2 days!

#! /usr/bin/python
import rospy
import sys
from std_msgs.msg import String, Bool, Float32
from geometry_msgs.msg import Pose
from geometry_msgs.msg import PoseStamped
from geometry_msgs.msg import Vector3
from geometry_msgs.msg import Quaternion
from geometry_msgs.msg import Wrench

class example_application:

def callback(self, data):
  self.position_cartesian_current = data.pose
  rospy.loginfo(data.pose)

def configure(self,robot_name):
    self._robot_name = 'PSM1'
    ros_namespace = '/dvrk/PSM1'
    rospy.Subscriber('/dvrk/PSM1/position_cartesian_current', PoseStamped, self.callback)
    self.set_position_cartesian = rospy.Publisher('/dvrk/PSM1/set_position_cartesian', Pose, latch=True, queue_size = 10)
    rospy.sleep(3)
    rospy.init_node('listener', anonymous=True)
    rospy.spin()

def lagrange(self, 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 trajectoryMover(self):
    newPose = Pose()
    points =[(0.0156561,0.123151),(0.00715134,0.0035123151),(0.001515177,0.002123151),(0.0071239751,0.09123150)]
    xlist = [i*0.001 for i in range(10)]
    ylist = [self.lagrange(points, xlist[i])*0.001 for i in range(10)]
    for x, y in zip(xlist, ylist):
        newPose.position.x = x
        newPose.position.y = y
        newPose.position.z = 0.001
        newPose.orientation.x = 0.001
        newPose.orientation.y = 0.001
        newPose.orientation.z = 0.005
        newPose.orientation.w = 0.002
        rospy.sleep(1)
        self.set_position_cartesian.publish(newPose)
        rospy.loginfo(newPose)
        rospy.spin()

def run(self):
    # self.home()
    self.trajectoryMover()

if __name__ == '__main__':
    try:
        if (len(sys.argv) != 2):
            print(sys.argv[0] + ' requires one argument, i.e. name of dVRK arm')
    else:
        application = example_application()
        application.configure(sys.argv[1])
        application.run()

except rospy.ROSInterruptException:
    pass
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2019-06-24 06:44:19 -0600

gvdhoorn gravatar image

updated 2019-06-24 06:45:07 -0600

def configure(self,robot_name):
    [..]
    self.set_position_cartesian = rospy.Publisher('/dvrk/PSM1/set_position_cartesian', Pose, latch=True, queue_size = 10)
    [..]
    rospy.spin()

rospy.spin() is a blocking call and will never end, until you stop the program.

So in essence you have an infinite wait / delay / sleep here at the end of configure(..).


Edit: and the same in trajectoryMover(..) btw.

edit flag offensive delete link more

Comments

I've removed the rospy.spin() commands from both defs, however the data is still not publishing. After running rostopic list -p in a separate terminal, the topic set_position_cartesian is available, however when I run $ rostopic echo /dvrk/PSM1/set_postion_cartesian the following message appears: WARNING: topic [/dvrk/PSM1/set_postion_cartesian] does not appear to be published yet What's causing this error?

691TReilly gravatar image691TReilly ( 2019-06-24 07:01:55 -0600 )edit
1

There is only a warning, but in any case: that is just rostopic warning you that you appear to have subscribed to a topic that doesn't see any messages published. As rostopic assumes you'd want to echo actual messages, it tries to be nice and let you know that it hasn't seen anything within a certain time period.

I've removed the rospy.spin() commands from both defs, however the data is still not publishing.

have you checked that your node is still running? With all rospy.spin()s gone, I suspect your script now essentially just exits after running through your list once.

You migt want to add a single rospy.spin() back at the end of your run() method.

Note: "defs" are actually called "methods" (when they take a self arg) or "functions" (if they don't). def is just a keyword.

gvdhoorn gravatar imagegvdhoorn ( 2019-06-24 07:36:20 -0600 )edit
1

PS: if you're just starting with all of this, I'd recommend getting some Python experience first before trying to learn ROS. Doing both at the same time will be confusing.

Try to understand asynchronous and callback based programming as well, as there is a lot of it going on in a typical ROS node.

gvdhoorn gravatar imagegvdhoorn ( 2019-06-24 07:36:48 -0600 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2019-06-24 05:49:55 -0600

Seen: 39 times

Last updated: Jun 24