-
Notifications
You must be signed in to change notification settings - Fork 0
/
map.py
128 lines (105 loc) · 3.98 KB
/
map.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
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
#!/usr/bin/env python
import rospy
from nav_msgs.msg import Odometry
from tf.transformations import euler_from_quaternion
import time
from math import cos, sin, radians
from geometry_msgs.msg import Twist
import numpy as np
from sensor_msgs.msg import LaserScan
import matplotlib.pyplot as plt
class my_turtlebot3_map:
def __init__(self):
self.x = 0
self.y = 0
self.theta = 0
rospy.init_node('turtlebot3_controller')
self.minimum_distance = float(input("Enter Minimum distance to maintain: "))
self.velocity_publisher = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
rospy.Subscriber("/odom", Odometry, self.call_back)
rospy.Subscriber('/scan', LaserScan, self.scan_call_back)
self.rate = rospy.Rate(25)
self.scan_ranges = None
def scan_call_back(self, msg):
self.scan_ranges = msg.ranges
def call_back(self, data):
self.x = round(data.pose.pose.position.x, 4)
self.y = round(data.pose.pose.position.y, 4)
orientation_q = data.pose.pose.orientation
orientation_list = [orientation_q.x, orientation_q.y, orientation_q.z, orientation_q.w]
roll, pitch, yaw_rad = euler_from_quaternion(orientation_list)
self.theta = yaw_rad
def move_minimum_distance(self):
vel_msg = Twist()
while self.scan_ranges[0] > self.minimum_distance:
vel_msg.linear.x = 0.1
vel_msg.angular.z = 0
self.velocity_publisher.publish(vel_msg)
self.rate.sleep()
vel_msg.linear.x = 0
self.velocity_publisher.publish(vel_msg)
self.rate.sleep()
while self.theta < radians(90):
vel_msg.angular.z = radians(20)
self.velocity_publisher.publish(vel_msg)
self.rate.sleep()
vel_msg.angular.z = 0
self.velocity_publisher.publish(vel_msg)
self.rate.sleep()
def rotate_goal(self, goal):
des_angle = np.argmin(self.scan_ranges)
error = des_angle - goal
vel_msg = Twist()
while abs(error) > 3:
vel_msg.angular.z = radians(error)
self.velocity_publisher.publish(vel_msg)
self.rate.sleep()
des_angle = np.argmin(self.scan_ranges)
error = des_angle - goal
vel_msg.angular.z = 0
self.velocity_publisher.publish(vel_msg)
self.rate.sleep()
def obj_coor(self, min_dis, ref_angle):
ref_angle = radians(ref_angle)
transformation_matrix = np.array([
[cos(self.theta), -sin(self.theta), self.x],
[sin(self.theta), cos(self.theta), self.y],
[0, 0, 1]
])
obs_robot_coordinates = np.array([min_dis * cos(ref_angle), min_dis * sin(ref_angle), 1]).reshape(3, 1)
obs_world = np.matmul(transformation_matrix, obs_robot_coordinates)
return obs_world[0], obs_world[1]
def map_object(self):
plt.ion()
fig, ax = plt.subplots()
self.rotate_goal(0)
self.move_minimum_distance()
vel_msg = Twist()
while not rospy.is_shutdown():
vel_msg.linear.x = 0.1
vel_msg.angular.z = 0
self.velocity_publisher.publish(vel_msg)
self.rate.sleep()
if self.scan_ranges[270] < 10:
obs_x_w, obs_y_w = self.obj_coor(self.scan_ranges[269], 270)
ax.plot(obs_x_w, obs_y_w, '.r')
plt.draw()
plt.xlim(-3, 3)
plt.ylim(-3, 3)
plt.pause(0.00001)
if abs(self.scan_ranges[269] - self.minimum_distance) > 0.1:
self.rotate_goal(270)
vel_msg.linear.x = 0
self.velocity_publisher.publish(vel_msg)
self.rate.sleep()
plt.savefig("map.png")
plt.ioff()
plt.show()
print("Figure saved")
if __name__ == '__main__':
try:
robo = my_turtlebot3_map()
time.sleep(1)
robo.map_object()
except rospy.ROSInterruptException:
pass