Visualizing robot poses in RVIZ

asked 2017-09-28 13:04:59 -0600

Younès gravatar image

updated 2017-09-29 07:10:34 -0600

I have this Python program that stores data in an object em_map :

em_map=TopologicalMap()    
pub_em=rospy.Publisher(topic_root+"/ExperienceMap/Map",TopologicalMap)
topo_node=TopoogicalNode()
for i in range(em.get_num_experiences()-1):
    topo_node.id=em.get_experience(i).id
    topo_node.pose.position.x=em.get_experience(i).x_m
    topo_node.pose.position.y=em.get_experience(i).y_m
    topo_node.pose.orientation.x=0
    topo_node.pose.orientation.y=0
    topo_node.pose.orientation.z=math.sin(em.get_experience(i).th_rad/2.0)
    topo_node.pose.orientation.w=math.cos(em.get_experience(i).th_rad/2.0)
    em_map.node.append(topo_node)
em_map.edge_count=em.get_num_links()

topo_edge=TopologicalEdge()
for i in range(em.get_num_links()):
    topo_edge.source_id=em.get_link(i).exp_from_id 
    topo_edge.destination_id=em.get_link(i).exp_to_id
    topo_edge.duration=rospy.Duration(em.get_link(i).delta_time_s)
    topo_edge.transform.translation.x=em.get_link(i).d*math.cos(em.get_link(i).heading_rad)
    topo_edge.transform.translation.y=em.get_link(i).d*math.sin(em.get_link(i).heading_rad)
    topo_edge.transform.rotation.x=0
    topo_edge.transform.rotation.y=0
    topo_edge.transform.rotation.z=math.sin(em.get_link(i).facing_rad/2.0)
    em_map.edge.append(topo_edge)
pub_em.publish(em_map)

TopologicalNode and TopologicalMap are defined in ROS messages
I want to visualize data of em_map in RVIZ. I see the topic /ExperienceMap/Map" in Rviz but the screen contains only squares. What should I do?

edit retag flag offensive close merge delete

Comments

Can you start by fixing the formatting of your code? It's pretty difficult to read. It may help to first have it correct in a text editor, copy & paste it here, highlight it, then click the 101010 button.

jayess gravatar imagejayess ( 2017-09-28 14:39:32 -0600 )edit

I have done it

Younès gravatar imageYounès ( 2017-09-29 07:10:48 -0600 )edit