/odom topics not matching ERROR

Question

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.

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?

#!/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()

Answer 1

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.

Answer 2

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()