Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- import rospy
- from threading import Thread
- from geometry_msgs.msg import Twist
- from sensor_msgs.msg import LaserScan
- from nav_msgs.msg import Odometry
- a = True
- x = True
- def callback(data):
- global a, velocity_publisher, vel_msg
- if(data.ranges[0] != float('inf') and a == True):
- if(data.ranges[0] >= 0.39):
- vel_msg.linear.x = 0.2
- velocity_publisher.publish(vel_msg)
- else:
- a = False
- def odomcallback(data1):
- global velocity_publisher, x, vel_msg
- if (data1.pose.pose.position.x > 0.000 and a == False):
- vel_msg.linear.x = -0.2
- elif(x == True and a == False):
- vel_msg.linear.x = 0.0
- x = False
- velocity_publisher.publish(vel_msg)
- if __name__ == '__main__':
- velocity_publisher = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
- rospy.init_node('listener', anonymous=True)
- vel_msg = Twist()
- rospy.Subscriber("/scan", LaserScan, callback)
- rospy.Subscriber('/odom',Odometry,odomcallback)
- rospy.spin()
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement