-
Notifications
You must be signed in to change notification settings - Fork 38
/
speeds_assignment.py
executable file
·125 lines (97 loc) · 3.91 KB
/
speeds_assignment.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
#!/usr/bin/env python
import rospy
import math
import time
from sensor_msgs.msg import Range
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist
from sonar_data_aggregator import SonarDataAggregator
from laser_data_aggregator import LaserDataAggregator
from navigation import Navigation
# Class for assigning the robot speeds
class RobotController:
# Constructor
def __init__(self):
# Debugging purposes
self.print_velocities = rospy.get_param('print_velocities')
# Where and when should you use this?
self.stop_robot = False
# Create the needed objects
self.sonar_aggregation = SonarDataAggregator()
self.laser_aggregation = LaserDataAggregator()
self.navigation = Navigation()
self.linear_velocity = 0
self.angular_velocity = 0
# Check if the robot moves with target or just wanders
self.move_with_target = rospy.get_param("calculate_target")
# The timer produces events for sending the speeds every 110 ms
rospy.Timer(rospy.Duration(0.11), self.publishSpeeds)
self.velocity_publisher = rospy.Publisher(\
rospy.get_param('speeds_pub_topic'), Twist,\
queue_size = 10)
# This function publishes the speeds and moves the robot
def publishSpeeds(self, event):
# Produce speeds
self.produceSpeeds()
# Create the commands message
twist = Twist()
twist.linear.x = self.linear_velocity
twist.linear.y = 0
twist.linear.z = 0
twist.angular.x = 0
twist.angular.y = 0
twist.angular.z = self.angular_velocity
# Send the command
self.velocity_publisher.publish(twist)
# Print the speeds for debuggind purposes
if self.print_velocities == True:
print "[L,R] = [" + str(twist.linear.x) + " , " + \
str(twist.angular.z) + "]"
# Produces speeds from the laser
def produceSpeedsLaser(self):
scan = self.laser_aggregation.laser_scan
linear = 0
angular = 0
############################### NOTE QUESTION ############################
# Check what laser_scan contains and create linear and angular speeds
# for obstacle avoidance
##########################################################################
return [linear, angular]
# Combines the speeds into one output using a motor schema approach
def produceSpeeds(self):
# Produce target if not existent
if self.move_with_target == True and \
self.navigation.target_exists == False:
# Create the commands message
twist = Twist()
twist.linear.x = 0
twist.linear.y = 0
twist.linear.z = 0
twist.angular.x = 0
twist.angular.y = 0
twist.angular.z = 0
# Send the command
self.velocity_publisher.publish(twist)
self.navigation.selectTarget()
# Get the submodule's speeds
[l_laser, a_laser] = self.produceSpeedsLaser()
# You must fill these
self.linear_velocity = 0
self.angular_velocity = 0
if self.move_with_target == True:
[l_goal, a_goal] = self.navigation.velocitiesToNextSubtarget()
############################### NOTE QUESTION ############################
# You must combine the two sets of speeds. You can use motor schema,
# subsumption of whatever suits your better.
##########################################################################
else:
############################### NOTE QUESTION ############################
# Implement obstacle avoidance here using the laser speeds.
# Hint: Subtract them from something constant
pass
##########################################################################
# Assistive functions
def stopRobot(self):
self.stop_robot = True
def resumeRobot(self):
self.stop_robot = False