-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathobs.py
114 lines (87 loc) · 3.19 KB
/
obs.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
#! /usr/bin/env python
import rospy
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Point, Twist
from nav_msgs.msg import Odometry
from tf.transformations import euler_from_quaternion
from math import atan2
pub = None
x=0.0
y=0.0
theta=0.0
def newOdom(msgo):
global x
global y
global theta
x=msgo.pose.pose.position.x
y=msgo.pose.pose.position.y
rot_q=msg.pose.pose.orientation
(roll, pitch, theta)=euler_from_quaternion([rot_q.x, rot_q.y, rot_q.z, rot_q.w])
speed=Twist()
def callback_laser(msg):
# 120 degrees into 3 regions
regions = {
'right': min(min(msg.ranges[0:2]), 10),
'front': min(min(msg.ranges[3:5]), 10),
'left': min(min(msg.ranges[6:9]), 10),
}
take_action(regions)
def take_action(regions):
goal=Point()
goal.x=0.7
goal.y=0.4
threshold_dist = 1.5
linear_speed = 0.6
angular_speed = 1
msg = Twist()
linear_x = 0
angular_z = 0
state_description = ''
if regions['front'] > threshold_dist and regions['left'] > threshold_dist and regions['right'] > threshold_dist:
state_description = 'case 1 - no obstacle'
linear_x = linear_speed
angular_z = 0
elif regions['front'] < threshold_dist and regions['left'] < threshold_dist and regions['right'] < threshold_dist:
state_description = 'case 7 - front and left and right'
linear_x = -linear_speed
angular_z = angular_speed # Increase this angular speed for avoiding obstacle faster
elif regions['front'] < threshold_dist and regions['left'] > threshold_dist and regions['right'] > threshold_dist:
state_description = 'case 2 - front'
linear_x = 0
angular_z = angular_speed
elif regions['front'] > threshold_dist and regions['left'] > threshold_dist and regions['right'] < threshold_dist:
state_description = 'case 3 - right'
linear_x = 0
angular_z = -angular_speed
elif regions['front'] > threshold_dist and regions['left'] < threshold_dist and regions['right'] > threshold_dist:
state_description = 'case 4 - left'
linear_x = 0
angular_z = angular_speed
elif regions['front'] < threshold_dist and regions['left'] > threshold_dist and regions['right'] < threshold_dist:
state_description = 'case 5 - front and right'
linear_x = 0
angular_z = -angular_speed
elif regions['front'] < threshold_dist and regions['left'] < threshold_dist and regions['right'] > threshold_dist:
state_description = 'case 6 - front and left'
linear_x = 0
angular_z = angular_speed
elif regions['front'] > threshold_dist and regions['left'] < threshold_dist and regions['right'] < threshold_dist:
state_description = 'case 8 - left and right'
linear_x = linear_speed
angular_z = 0
else:
state_description = 'unknown case'
rospy.loginfo(regions)
rospy.loginfo(state_description)
msg.linear.x = linear_x
msg.angular.z = angular_z
pub.publish(msg)
def main():
global pub
rospy.init_node('reading_laser')
pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
sub = rospy.Subscriber('/robot/laser/scan', LaserScan, callback_laser)
sub=rospy.Subscriber('/odometry/filtered', Odometry, newOdom)
rospy.spin()
if __name__ == '__main__':
main()