Ask Your Question
0

Tracking Robot position using Marker

asked 2019-10-13 06:06:06 -0600

BV_Pradeep gravatar image

Hi All,

I am using ROS Kinetic on Ubuntu 16.04. I am working on a RVIZ Marker program to track "\AMCL_Pose" parameter and visualize in RVIZ. The code I have written is as follows

"""
The following program is used to display the real pose positions taken by the
robot in rviz.
References:
1.https://www.programcreek.com/python/example/88812/visualization_msgs.msg.Marker
"""

import rospy
from visualization_msgs.msg import Marker
from geometry_msgs.msg import PoseWithCovarianceStamped,Point

def callback(data):
    add_point = Point()
    add_point.x = data.pose.pose.position.x
    add_point.y = data.pose.pose.position.y
    add_point.z = 0
    rospy.loginfo('Publishing Marker Point')
    marker.points.append(add_point)
    # Publish the Marker
    pub_point.publish(marker)
    rospy.sleep(5)


marker = Marker()
marker.header.frame_id = "/map"
marker.type = marker.LINE_STRIP
marker.action = marker.ADD

# marker scale
marker.scale.x = 0.03
marker.scale.y = 0.03
marker.scale.z = 0.03

# marker color
marker.color.a = 1.0
marker.color.r = 1.0
marker.color.g = 0.0
marker.color.b = 0.0

# marker orientaiton
marker.pose.orientation.x = 0.0
marker.pose.orientation.y = 0.0
marker.pose.orientation.z = 0.0
marker.pose.orientation.w = 1.0
"""
# marker position
marker.pose.position.x = 0.0
marker.pose.position.y = 0.0
marker.pose.position.z = 0.0
"""
# marker line points
marker.points = []
rospy.loginfo('Marker created')

rospy.init_node('position_tracker')

pub_point = rospy.Publisher('realpoints_marker', Marker, queue_size=1)
print "Publisher created...."

rospy.Subscriber("/amcl_pose",PoseWithCovarianceStamped, callback)
print "Subcriber created...."
rospy.spin()

I get the following result image description

The issue is that the length and orientation of the line are not proper. The robot has moved over 20 meters but in rviz it showing only 5 meters.

How to resolve this issue ?

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2019-10-13 06:45:44 -0600

bekirbostanci gravatar image

your callback function should be a loop because you are publish your data only 1 however you want to update every step

edit flag offensive delete link more

Comments

@bekirbostanci The callback function will loop over automatically as many times the amcl data is being published by the publisher.

BV_Pradeep gravatar imageBV_Pradeep ( 2019-10-14 23:24:00 -0600 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2019-10-13 06:06:06 -0600

Seen: 36 times

Last updated: Oct 13