ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange |
1 | initial version |
Here is an updated version..
import rospy from geometry_msgs.msg import Twist from sensor_msgs.msg import LaserScan
class wall_follow():
def __init__(self,r1=0.0,r2=0.0,threshold=5.0,i=0):
self.r1=r1
self.r2=r2
self.threshold=threshold
self.i=i
rospy.init_node('wall_follow', anonymous=True)
rospy.loginfo("Press Ctrl+C to Exit")
rospy.on_shutdown(self.shutdown)
self.cmd_vel = rospy.Publisher('cmd_vel_mux/input/navi', Twist, queue_size=50)
self.sub=rospy.Subscriber('scan',LaserScan,self.scan_callback)
rate = rospy.Rate(.5)
while not rospy.is_shutdown():
rospy.loginfo("%d",self.i)
self.i=self.i+1
move_cmd=Twist()
rospy.loginfo("range_580 = %f range_500 = %f", self.r1,self.r2)
if self.r2-self.r1 > self.threshold:
move_cmd.angular.z=1.0
move_cmd.linear.x=0.0
elif self.r2-self.r1 < -1*self.threshold:
move_cmd.angular.z=-1.0
move_cmd.linear.x=0.0
else:
move_cmd.angular.z=0.0
move_cmd.linear.x=0.3
self.cmd_vel.publish(move_cmd)
rate.sleep()
def scan_callback(self,msg):
rospy.loginfo("%d",self.i)
self.r1=msg.ranges[580]
self.r2=msg.ranges[500]
rospy.spin()
def shutdown(self):
rospy.loginfo("Stopping TurtleBot")
self.cmd_vel.publish(Twist())
# Give TurtleBot time to stop
rospy.sleep(1)
if __name__ == '__main__': try: wall_follow() except: rospy.loginfo("End of the trip for TurtleBot")
As you can see changed the position of the Subscriber callback outside, i tried placing inside as well still the algo is not working. I am getting the following result
[INFO] [WallTime: 1477906023.974810] Press Ctrl+C to Exit [INFO] [WallTime: 1477906023.985877] 0 [INFO] [WallTime: 1477906024.005692] range_580 = 0.000000 range_500 = 0.000000 [INFO] [WallTime: 1477906024.113967] 1 [ERROR] [WallTime: 1477906024.116318] bad callback: <bound method="" wall_follow.scan_callback="" of="" <__main__.wall_follow="" instance="" at="" 0x7fac758c9998="">> Traceback (most recent call last): File "/opt/ros/indigo/lib/python2.7/dist-packages/rospy/topics.py", line 720, in _invoke_callback cb(msg) File "wall_follower.py", line 41, in scan_callback self.r1=msg.ranges[580] IndexError: tuple index out of range
[INFO] [WallTime: 1477906024.213830] 1 [ERROR] [WallTime: 1477906024.215866] bad callback: <bound method="" wall_follow.scan_callback="" of="" <__main__.wall_follow="" instance="" at="" 0x7fac758c9998="">> Traceback (most recent call last): File "/opt/ros/indigo/lib/python2.7/dist-packages/rospy/topics.py", line 720, in _invoke_callback cb(msg) File "wall_follower.py", line 41, in scan_callback self.r1=msg.ranges[580] IndexError: tuple index out of range
the value of ranges is not changing!! I checked the hokuyo model specs (it can scan upto 240_deg with each step of about 3.52_deg) so the index 580 should be within its limit.
Please Help.
Also, if you could advise / share any other approach to this problem, it would be very nice.