An error for publishing nav_msgs/Odometry
In a simulator, I am trying to write a python script which subscribe a navmsgs/Odometry topic to add a frame between odom and basefootprint. It then also publishes another nav_msgs/Odometry topic under frame odom. The callback function for the subscriber is here:
#!/usr/bin/env python
import roslib
roslib.load_manifest('simulator')
import rospy, tf
from geometry_msgs.msg import Point, Quaternion, PoseStamped
from nav_msgs.msg import Odometry
import xml.dom.minidom
def newDataAvailable(robotPose):
robotPosition=float(robotPose.pose.pose.position.x) -x, float(robotPose.pose.pose.position.y) -y,0.0
robotOrientation=robotPose.pose.pose.orientation.x, robotPose.pose.pose.orientation.y, robotPose.pose.pose.orientation.z, robotPose.pose.pose.orientation.w
robotVelocity=(robotPose.twist.twist.linear.x,robotPose.twist.twist.linear.y,robotPose.twist.twist.linear.z)
currentTime = rospy.Time.now()
br.sendTransform(robotPosition, robotOrientation, currentTime, "base_footprint", "odom")
odom = Odometry()
odom.header.stamp = rospy.Time.now()
odom.header.frame_id = parentFrameId
odom.child_frame_id = frameId
odom.pose.pose.position = robotPosition
print odom.pose.pose.position
odom.pose.pose.orientation = robotOrientation
print "print orientation"
print odom.pose.pose.orientation
odom.twist.twist.linear=Point(*robotVelocity)
print odom.twist
odomPublisher.publish(odom)
def main():
rospy.init_node('simulator_robot_tf_broadcaster')
global frameId, parentFrameId, br, x, y, odomPublisher
frameId = rospy.get_param("frame", "base_footprint")
parentFrameId = rospy.get_param("parent_frame", "odom")
robotgroundtruthTopic = "/robot_pose"
rospy.Subscriber(robotgroundtruthTopic, Odometry, newDataAvailable)
odomPublisher=rospy.Publisher("/base_pose_ground_truth", Odometry, 10)
scene_file_param=rospy.get_param("/scene_file", "scene.xml")
dom = xml.dom.minidom.parse(scene_file_param)
root = dom.documentElement
itemlist = dom.getElementsByTagName('agent')
for i in itemlist:
pn = i.getAttribute("type")
if pn=="2": # robot:2
print "I got the robot's initial postion"
x = i.getAttribute("x")
#print x
y = i.getAttribute("y")
#print y
br = tf.TransformBroadcaster()
x=float(x)
y=float(y)
print x,y
rospy.spin()
if __name__ == '__main__':
main()
When I run it, an error comes out:
File "/opt/ros/hydro/lib/python2.7/dist-packages/nav_msgs/msg/_Odometry.py", line 161, in serialize
buff.write(_struct_7d.pack(_x.pose.pose.position.x, _x.pose.pose.position.y, _x.pose.pose.position.z, _x.pose.pose.orientation.x, _x.pose.pose.orientation.y, _x.pose.pose.orientation.z, _x.pose.pose.orientation.w))
AttributeError: 'tuple' object has no attribute 'x'
All print functions work fine. However, no message outputs from topic "/baseposeground_truth"
EDIT1: I modified some places of code as:
robotPosition=robotPose.pose.pose.position.x - x, robotPose.pose.pose.position.y-y, 0.0
robotOrientation=(robotPose.pose.pose.orientation.x, robotPose.pose.pose.orientation.y, robotPose.pose.pose.orientation.z, robotPose.pose.pose.orientation.w)
robotVelocity=(robotPose.twist.twist.linear.x,robotPose.twist.twist.linear.y,robotPose.twist.twist.linear.z)
currentTime = rospy.Time.now()
br.sendTransform(robotPosition, robotOrientation, currentTime, "base_footprint", "odom")
odom = Odometry()
odom.header.stamp = rospy.Time.now()
odom.header.frame_id = parentFrameId
odom.child_frame_id = frameId
odom.pose.pose.position = Point(*robotPosition)
odom.pose.pose.orientation =Quaternion(*robotOrientation)
odom.twist.twist.linear=Point(*robotVelocity)
odomPublisher.publish(odom)
Then above error(simplified) disappears. Message output from topic "/baseposegroundtruth" also but in a lower speed comparing to topic "/robotpose"( why?)
However, as it is mentioned before, the above node adds a frame between "odom" and "base_footprint". Then this node is connected to fakelocalization. But, the link between map and odom doesn't constructed by fakelocalization successfully.
And an warning comes out:
[WARN] [WallTime: 1430749249.205509] Inbound TCP/IP connection failed: 'int' object has no attribute 'peer_subscribe'
Please tell me the reason why. Thank you!
Asked by scopus on 2015-05-04 08:22:40 UTC
Comments
It looks like there's an answer for this on http://answers.ros.org/question/208532/add-a-tf-frame-in-a-simulator/
Asked by ahendrix on 2017-04-12 15:37:05 UTC