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.
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
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()
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
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
z from the
#!/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()