0
$\begingroup$

I have an existing ROS workspace which was built in C++. I am trying to implement a node built by The Construct in python which reads distance travelled from the /odom topic. I have successfully built a package within my existing work space containing their code and can run the node.

When I rosrun the node, I am receiving an error on the roscore saying:

[ERROR]: Client [/movement_detector_node] wants topic /odom to have datatype/md5sum [nav_msgs/Odometry], but our version has [geometry_msgs/PoseArray]. Dropping connection.

enter image description here

It looks like my existing work space uses Type: geometry_msgs/PoseArray messages to communicate.

Could I please get some help on the proper way to debug and fix this?

Node for distance collection below. All credit belongs to The Construct.

#!/usr/bin/env python

import rospy
import math
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Point
from std_msgs.msg import Float64

class MovementDetector(object):
    def __init__(self):
        self._mved_distance = Float64()
        self._mved_distance.data = 0.0
        self.get_init_position()

        self.distance_moved_pub = rospy.Publisher('/moved_distance', Float64, queue_size=1)
        rospy.Subscriber("/odom", Odometry, self.odom_callback)

    def get_init_position(self):
        data_odom = None
        while data_odom is None:
            try:
                data_odom = rospy.wait_for_message("/odom", Odometry, timeout=1)
            except:
                rospy.loginfo("Current odom not ready yet, retrying for setting up init pose")

        self._current_position = Point()
        self._current_position.x = data_odom.pose.pose.position.x
        self._current_position.y = data_odom.pose.pose.position.y
        self._current_position.z = data_odom.pose.pose.position.z

    def odom_callback(self,msg):
        NewPosition = msg.pose.pose.position
        self._mved_distance.data += self.calculate_distance(NewPosition, self._current_position)
        self.updatecurrent_positin(NewPosition)
        if self._mved_distance.data < 0.000001:
            aux = Float64()
            aux.data = 0.0
            self.distance_moved_pub.publish(aux)
        else:
            self.distance_moved_pub.publish(self._mved_distance)

    def updatecurrent_positin(self, new_position):
        self._current_position.x = new_position.x
        self._current_position.y = new_position.y
        self._current_position.z = new_position.z

    def calculate_distance(self, new_position, old_position):
        x2 = new_position.x
        x1 = old_position.x
        y2 = new_position.y
        y1 = old_position.y
        dist = math.hypot(x2 - x1, y2 - y1)
        return dist

    def publish_moved_distance(self):
        """
        Loops untils closed, publishing moved distance
        """
        rospy.spin()



        # spin() simply keeps python from exiting until this node is stopped
        rospy.spin()

if __name__ == '__main__':
    rospy.init_node('movement_detector_node', anonymous=True)
    movement_obj = MovementDetector()
    movement_obj.publish_moved_distance()

UPDATE:

I was able to fix this problem but now have an additional question. Basically the existing workspace was expecting the subscriber to use the geometry_msgs\PoseArray message. Once I imported that message and used it in the callback my error went away.

Looking at the geometry_msgs\PoseArray message, it's composed of position and orientation message. For the distance collection script to work, I need to only use the position message of geometry_msgs\PoseArray. I wrote a basic subscriber to practice isolating this data.

Pictures of both message types below. Could someone help me isolate the x, y, and z from the geometry_msgs\PoseArray message?

enter image description here

#!/usr/bin/env python

import rospy
import math
from nav_msgs.msg import Odometry
from geometry_msgs.msg import PoseArray
from std_msgs.msg import Float64

def callback(data):
    rospy.loginfo(rospy.get_caller_id() + "I heard %s", data)

def listener():

    # In ROS, nodes are uniquely named. If two nodes with the same
    # name are launched, the previous one is kicked off. The
    # anonymous=True flag means that rospy will choose a unique
    # name for our 'listener' node so that multiple listeners can
    # run simultaneously.
    rospy.init_node('listener', anonymous=True)

    rospy.Subscriber("odom", PoseArray, callback)

    # spin() simply keeps python from exiting until this node is stopped
    rospy.spin()

if __name__ == '__main__':
    listener()
$\endgroup$
1
$\begingroup$

As I understand your workspace send to the /odom topic geometry_msgs/PoseArray and you are trying to read it as nav_msgs/Odometry, As you can see they have totally different struct. I can imagine that you don't want to change the "The Construct" code, so my suggestion would be to create an intermediate topic in order to "translate" the poseArray to Odometry. This ros tutorial might help you.

It would be helpful if you post you workspace code too.

$\endgroup$
  • $\begingroup$ Thanks! This was helpful. I uploaded some additional information regarding progress made. Mind taking a look? $\endgroup$ – neerbasu Sep 26 at 19:42
  • $\begingroup$ That was my suggestion to have an intermediate node which listens to PoseArray and outputs just the odometry. Simply geting the position x,y,z and put it to your odometry position. $\endgroup$ – nionios Sep 27 at 11:18
  • $\begingroup$ Yes, this was based off your advice. Any idea how to isolate position data from PoseArray? $\endgroup$ – neerbasu Sep 27 at 20:23
  • $\begingroup$ you mean something like this? odom.pose.pose.position.x odom.pose.pose.position.y odom.pose.pose.position.z $\endgroup$ – nionios Sep 28 at 10:31
  • $\begingroup$ I will try this and get back to you. I always struggle when it comes to drilling into specific variables in ROS messages. Is there a good tutorial on how to do this? It would have taken me forever to figure out odom.pose.pose.position.x was the right call.. $\endgroup$ – neerbasu Sep 30 at 17:27
0
$\begingroup$

Could someone help me isolate the x, y, and z from the geometry_msgs/PoseArray message?

The code below is your solution as simple for the above question with geometry_msgs/PoseArray message:

import rospy
from geometry_msgs.msg import PoseArray

def callback(data):
    for pose in data.poses:
        print("X: {}".format(pose.position.x))
        print("Y: {}".format(pose.position.Y))
        print("Z: {}".format(pose.position.z))

def listener():
    rospy.init_node('listener', anonymous=True)
    rospy.Subscriber("odom", PoseArray, callback)
    rospy.spin()

if __name__ == '__main__':
    listener()
$\endgroup$
  • $\begingroup$ Hi, I have tried this solution but I am getting the following error: File "/home/nrjbs87/catkin_space/src/odom_movement_detector/src/test_odom_sub.py", line 7, in callback print("X: {}".format(data.poses.position.x)) AttributeError: 'list' object has no attribute 'position' $\endgroup$ – neerbasu Oct 7 at 14:00
  • $\begingroup$ Hello @neerbasu, I updated the answer. $\endgroup$ – Benyamin Jafari Oct 7 at 15:08
  • $\begingroup$ Brilliant! This has fixed the issue. I have one more request...how would I integrate this into the initial code I have posted. The one that takes each position and calculates distance traveled. $\endgroup$ – neerbasu Oct 7 at 18:00
  • $\begingroup$ To further elaborate on why I am struggling with this: the way you have isolated the readings is by using the PoseArray message type. In the example code provided above, they are using the Point message type for all of the calculation. So I am not sure how to convert the PoseArray to be used by the rest of the script. $\endgroup$ – neerbasu Oct 7 at 18:11

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.