Robotics StackExchange | Archived questions

Visualizing robot poses in RVIZ

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?

Asked by Younès on 2017-09-28 13:04:59 UTC

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.

Asked by jayess on 2017-09-28 14:39:32 UTC

I have done it

Asked by Younès on 2017-09-29 07:10:48 UTC

Answers