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
- import math
- a = True
- x = True
- b = False
- c = None
- d = False
- g = True
- def callback(data):
- global a, velocity_publisher, vel_msg, b
- 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:
- vel_msg.linear.x = 0
- velocity_publisher.publish(vel_msg)
- a = False
- b = True
- def odomcallback(data1):
- global velocity_publisher, x, vel_msg, b, c, d, g
- if(b == True):
- c = input()
- b = False
- t1 = +2.0 * (data1.pose.pose.orientation.w * data1.pose.pose.orientation.z + data1.pose.pose.orientation.x * data1.pose.pose.orientation.y)
- t2 = +1.0 - 2.0 * (data1.pose.pose.orientation.y ** 2 + data1.pose.pose.orientation.z**2)
- if(math.degrees(math.atan2(t1, t2)) < 173.0 and c != None and d == False):
- vel_msg.angular.z = 0.5
- velocity_publisher.publish(vel_msg)
- elif(math.degrees(math.atan2(t1, t2)) > 173.0 and d == False):
- vel_msg.angular.z = 0.0
- velocity_publisher.publish(vel_msg)
- d = True
- if (data1.pose.pose.position.x > 0.000 and a == False and c != None and d ==True):
- vel_msg.linear.x = 0.2
- elif(x == True and a == False and c != None and d == True):
- vel_msg.linear.x = 0.0
- x = False
- if (g == True and int(math.degrees(math.atan2(t1, t2))) != 0.0 and x == False):
- vel_msg.angular.z = 0.5
- velocity_publisher.publish(vel_msg)
- elif (g == True and x == False and int(math.degrees(math.atan2(t1, t2))) == 0):
- vel_msg.angular.z = 0.0
- velocity_publisher.publish(vel_msg)
- g = 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