-
Notifications
You must be signed in to change notification settings - Fork 0
/
self task1.py
81 lines (59 loc) · 2.27 KB
/
self task1.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Twist
from sensor_msgs.msg import LaserScan
from nav_msgs.msg import Odometry
from tf.transformations import euler_from_quaternion
import math
import numpy as np
pose = [1,2,3]
def euclidean_distance(goal_pose):
return math.sqrt(math.pow((goal_pose.pose.pose.position.x - pose[0]), 2) + math.pow((goal_pose.pose.pose.position.y - pose[1]), 2))
def steering_angle(goal_pose):
return abs(math.atan2(goal_pose.pose.pose.position.y , goal_pose.pose.pose.position.x) - pose[2])
def linear_vel(goal_pose, constant=1.5):
return constant * euclidean_distance(goal_pose)
def angular_vel(goal_pose, constant=6):
return constant * (steering_angle(goal_pose))
def odom_callback(data):
global pose
pose = data
x = data.pose.pose.orientation.x;
y = data.pose.pose.orientation.y;
z = data.pose.pose.orientation.z;
w = data.pose.pose.orientation.w;
pose = [data.pose.pose.position.x, data.pose.pose.position.y, euler_from_quaternion([x,y,z,w])[2]]
def control_loop():
rospy.init_node('ebot_controller', anonymous=True)
pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
#rospy.Subscriber('/ebot/laser/scan', LaserScan, laser_callback)
rospy.Subscriber('/odom', Odometry, odom_callback)
rate = rospy.Rate(10)
global pose
pose[0] = Odometry()
pose[1] = Odometry()
pose[2] = Odometry()
vel_msg = Twist()
goal_pose = Odometry()
goal_pose.pose.pose.position.x = input('x')
goal_pose.pose.pose.position.y = input('y')
distance_tolerance = 0.1
angle_curr = 0.01
while steering_angle(goal_pose)>=angle_curr :
vel_msg.angular.z = angular_vel(goal_pose)
pub.publish(vel_msg)
rospy.loginfo('posew: {}'.format(steering_angle(goal_pose)))
rate.sleep()
while euclidean_distance(goal_pose) >= distance_tolerance:
vel_msg.linear.x = linear_vel(goal_pose)
pub.publish(vel_msg)
rospy.loginfo('euclidean_distance: {}'.format(euclidean_distance(goal_pose)))
rate.sleep()
vel_msg.linear.x = 0
vel_msg.angular.z = 0
pub.publish(vel_msg)
if __name__ == '__main__':
try:
control_loop()
except rospy.ROSInterruptException:
pass