I want one robot to follow another. I wrote this code so r2 can follow r1 by changing its goal with position of r1. r2 is going to r1/odom but when i start to move r1 more frequently, r2 of course is getting lost and is doing too much planning. What should I do to make r2 follow r1 as I control r1 without too much planning from r2?
#!/usr/bin/env python
# license removed for brevity
import rospy
from nav_msgs.msg import Odometry
from std_msgs.msg import String, Header
from geometry_msgs.msg import Twist, Point, PoseStamped, Pose
goal = Point()
def newOdom(msg):
goal.x = round(msg.pose.pose.position.x + 0.1)
goal.y = round(msg.pose.pose.position.y + 0.1)
def talker():
pub = rospy.Publisher('/r2/move_base_simple/goal', PoseStamped, queue_size = 1)
sub = rospy.Subscriber("/r1/odom", Odometry, newOdom)
rospy.init_node('follow_r1')
rate = rospy.Rate(3) # 10hz
while not rospy.is_shutdown():
odom = PoseStamped()
odom.header.frame_id = "/map"
odom.pose.position.x = goal.x
odom.pose.position.y = goal.y
odom.pose.orientation.w = 1.0
pub.publish(odom)
rate.sleep()
if __name__ == '__main__':
try:
talker()
except rospy.ROSInterruptException:
pass
↧