Skip to content
Snippets Groups Projects

Compare revisions

Changes are shown as if the source revision was being merged into the target revision. Learn more about comparing revisions.

Source

Select target project
No results found

Target

Select target project
  • cdf/cdf-2020-2021/modules/coordination-card/ros-coordination-card
1 result
Show changes
Commits on Source (62)
Hide whitespace changes
Inline Side-by-side
Showing
with 789 additions and 43 deletions
.gitignore 0 → 100644
View file @ 18b16210
.vscode/
.vscode/settings.json 0 → 100644
View file @ 18b16210
<<<<<<< HEAD
{}
=======
{
"ros.distro": "galactic",
"python.autoComplete.extraPaths": [
"/opt/ros/galactic/lib/python3.8/site-packages"
]
}
>>>>>>> bdceb05... Add clamps srv
actions_manager/src/tasks/goto.py
View file @ 18b16210
......@@ -5,9 +5,8 @@ from utils import compute_signed_angle, compare_distance, compare_theta
class Goto(Task):
def __init__(self, robot, state, target_pos, backward):
def __init__(self, robot, target_pos, backward):
self.robot = robot
self.state = state
self.target_pos = target_pos
self.backward = backward
......
actions_manager/src/tasks/multiclamp_grab.py
View file @ 18b16210
......@@ -10,6 +10,7 @@ class MulticlampGrab(Task):
def run(self):
# todo open all
for cup_idx in self.cup_indexes:
self.robot.open_multiclamp_servo(cup_idx)
......
actions_manager/src/tasks/swing_windsocks.py
View file @ 18b16210
......@@ -22,9 +22,9 @@ class SwingWindsocks(Task):
# Raise up the arm
# Wait for the arm to be completely up
self.robot.rotate_to(-pi)
self.robot.lower_windsocks_arm()
sleep(0.5)
self.robot.rotate_to(-pi)
self.robot.forward(TRAVEL_DISTANCE)
self.robot.raise_windsocks_arm()
sleep(0.5)
coordinator/launch/homologation.launch
View file @ 18b16210
......@@ -12,7 +12,7 @@
<node machine="lagalere" name="mccd" pkg="rosserial_python" type="serial_node.py" output="screen">
<param name="port" type="string" value="$(arg mccd_port)"/>
<param name="baud" type="int" value="$(arg mccd_baud)"/>
<param name="sampling_period" type="int" value="100"/>
<param name="sampling_period" type="int" value="50"/>
<param name="speed_profile/linear_speed_acceleration_max" type="double" value="0.8"/>
<param name="speed_profile/linear_speed_deceleration_max" type="double" value="0.8"/>
<param name="speed_profile/angular_speed_acceleration_max" type="double" value="3.14"/>
......@@ -26,6 +26,11 @@
<param name="baud" value="$(arg ctrl_panel_baud)" />
</node>
<!-- <node machine="lagalere" name="clamp" pkg="rosserial_python" type="serial_node.py" output="screen">
<param name="port" value="/dev/clamp" />
<param name="baud" value="57600" />
</node> -->
<node machine="lagalere" name="rplidarNode" pkg="rplidar_ros" type="rplidarNode" respawn="true" output="screen">
<param name="serial_port" type="string" value="/dev/ttyUSB0"/>
<param name="serial_baudrate" type="int" value="115200"/>
......
coordinator/launch/match.launch 0 → 100644
View file @ 18b16210
<launch>
<machine name="lagalere" address="lagalere.local" env-loader="/home/lagalere/ros_ws/env_loader.sh" default="false" user="lagalere" />
<machine name="local" address="localhost" default="true" />
<arg name="mccd_port" default="/dev/mccd"/>
<arg name="mccd_baud" default="115200"/>
<arg name="ctrl_panel_port" default="/dev/ctrl_panel"/>
<arg name="ctrl_panel_baud" default="57600"/>
<arg name="enable_raw" default="false"/>
<arg name="enable_imv" default="false"/>
<arg name="camera_id" default="0"/>
<arg name="camera_frame_id" default="raspicam"/>
<arg name="camera_name" default="picam"/>
<!-- namespace for camera input -->
<arg name="camera" default="/raspicam_node"/>
<arg name="image" default="image"/>
<arg name="transport" default="compressed"/>
<arg name="fiducial_len" default="0.14"/>
<arg name="dictionary" default="0"/>
<arg name="do_pose_estimation" default="true"/>
<!-- If vis_msgs set to true, pose estimation will be published with ROS standard vision_msgs -->
<arg name="vis_msgs" default="false"/>
<arg name="ignore_fiducials" default="" />
<arg name="fiducial_len_override" default="" />
<arg name="verbose" default="false"/>
<node machine="lagalere" name="mccd" pkg="rosserial_python" type="serial_node.py">
<param name="port" type="string" value="$(arg mccd_port)"/>
<param name="baud" type="int" value="$(arg mccd_baud)"/>
<param name="sampling_period" type="int" value="200"/>
<param name="speed_profile/linear_speed_acceleration_max" type="double" value="0.8"/>
<param name="speed_profile/linear_speed_deceleration_max" type="double" value="0.8"/>
<param name="speed_profile/angular_speed_acceleration_max" type="double" value="3.14"/>
<param name="speed_profile/angular_speed_deceleration_max" type="double" value="3.14"/>
<rosparam param="left_motor_gains">[3.285, 64.40766, 0.0]</rosparam>
<rosparam param="right_motor_gains">[3.240, 66.48427, 0.0]</rosparam>
</node>
<node machine="lagalere" name="ctrl_panel" pkg="rosserial_python" type="serial_node.py">
<param name="port" value="$(arg ctrl_panel_port)" />
<param name="baud" value="$(arg ctrl_panel_baud)" />
</node>
<node machine="lagalere" name="clamp" pkg="rosserial_python" type="serial_node.py" output="screen">
<param name="port" value="/dev/clamp" />
<param name="baud" value="57600" />
</node>
<node machine="lagalere" name="rplidarNode" pkg="rplidar_ros" type="rplidarNode" respawn="true">
<param name="serial_port" type="string" value="/dev/ttyUSB0"/>
<param name="serial_baudrate" type="int" value="115200"/>
<param name="frame_id" type="string" value="laser"/>
<param name="inverted" type="bool" value="false"/>
<param name="angle_compensate" type="bool" value="true"/>
</node>
<node machine="lagalere" type="raspicam_node" pkg="raspicam_node" name="raspicam_node" output="screen">
<param name="private_topics" value="true"/>
<param name="camera_frame_id" value="$(arg camera_frame_id)"/>
<param name="enable_raw" value="$(arg enable_raw)"/>
<param name="enable_imv" value="$(arg enable_imv)"/>
<param name="camera_id" value="$(arg camera_id)"/>
<param name="camera_info_url" value="package://raspicam_node/camera_info/camerav2_1280x960.yaml"/>
<param name="camera_name" value="$(arg camera_name)"/>
<param name="width" value="1280"/>
<param name="height" value="960"/>
<param name="framerate" value="10"/>
<param name="exposure_mode" value="antishake"/>
<param name="shutter_speed" value="0"/>
</node>
<node machine="lagalere" pkg="aruco_detect" name="aruco_detect"
type="aruco_detect" output="screen" respawn="false">
<param name="image_transport" value="$(arg transport)"/>
<param name="publish_images" value="false" />
<param name="fiducial_len" value="$(arg fiducial_len)"/>
<param name="dictionary" value="$(arg dictionary)"/>
<param name="do_pose_estimation" value="$(arg do_pose_estimation)"/>
<param name="vis_msgs" value="$(arg vis_msgs)"/>
<param name="ignore_fiducials" value="$(arg ignore_fiducials)"/>
<param name="fiducial_len_override" value="$(arg fiducial_len_override)"/>
<param name="verbose" value="$(arg verbose)"/>
<remap from="camera/compressed"
to="$(arg camera)/$(arg image)/$(arg transport)"/>
<remap from="camera_info" to="$(arg camera)/camera_info"/>
</node>
<node machine="lagalere" name="match" pkg="coordinator" type="match.py" output="screen"/>
</launch>
coordinator/nodes/homologation.py
View file @ 18b16210
......@@ -8,10 +8,10 @@ from mccd_msgs.msg import Odometry
from sensor_msgs.msg import LaserScan as Scan
from std_srvs.srv import SetBool
from std_msgs.msg import Bool
from math import radians, degrees, pi
from math import radians, degrees, pi, sqrt
emergency_distance = 0.22 # in meters
emergency_distance = 0.40 # in meters
emergency_min_count = 2
......@@ -20,23 +20,24 @@ class CoordinatorNode():
def __init__(self):
self.change_pwm_service = rospy.ServiceProxy(
'/mccd/enable_pwm', SetBool)
self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
self.pull_cord_status_sub = rospy.Subscriber(
'/pull_cord_status', Bool, self.pull_cord_status_callback)
self.pull_cord_status = False
self.odom_sub = rospy.Subscriber(
"/mccd/odom", Odometry, self.odom_callback)
self.flag_state_pub = rospy.Publisher(
"/flag_state", Bool, queue_size=1)
self.linear_speed = 0
self.theta = 0
self.x = 0
self.y = 0
self.lidar_subscriber = rospy.Subscriber(
'scan', Scan, self.scan_callback)
self.exec_thread = threading.Thread(target=self.run)
self.exec_thread.start()
self.scan = None
print("Coordinator initialized.")
......@@ -44,6 +45,7 @@ class CoordinatorNode():
def odom_callback(self, odom):
self.linear_speed = odom.linear_speed
self.theta = odom.theta
self.x = odom.x
self.y = odom.y
def change_pwm(self, enable):
......@@ -75,14 +77,16 @@ class CoordinatorNode():
angle = self.scan.angle_min + i * self.scan.angle_increment
if distance < emergency_distance:
counter = counter + 1
# Stop if we detected something in the front and we are going forward
if linear_speed > 0 and (radians(-180) < angle < radians(-45-90) or radians(45+90) < angle < radians(180)):
counter = counter + 1
# Stop if we detected something in the back and we are going backward
elif linear_speed < 0 and radians(-45) < angle < radians(45):
counter = counter + 1
# if linear_speed > 0 and (radians(-180) < angle < radians(-60-90) or radians(60+90) < angle < radians(180)):
# counter = counter + 1
# # Stop if we detected something in the back and we are going backward
# elif linear_speed < 0 and radians(-60) < angle < radians(60):
# counter = counter + 1
if counter >= emergency_min_count:
# rospy.loginfo("Emergency stop")
linear_speed_cmd = 0
angular_speed_cmd = 0
......@@ -94,52 +98,61 @@ class CoordinatorNode():
def pull_cord_status_callback(self, data):
self.pull_cord_status = data.data
def move(self, target_distance, forward):
start_x = self.x
start_y = self.y
epsilon = 0.01
distance = 0
while abs(target_distance - distance) >= epsilon:
# rospy.loginfo("Distance: {}".format(distance))
self.set_speed(0.1 if forward else -0.1, 0)
rospy.sleep(0.02)
distance = sqrt((self.x - start_x) * (self.x - start_x) +
(self.y - start_y) * (self.y - start_y))
def run(self):
self.change_pwm(True)
while self.pull_cord_status != True:
self.set_speed(0, 0)
time.sleep(0.05)
time.sleep(0.02)
start = rospy.get_time()
rospy.loginfo("Starting !")
# Roule pendant 3 secondes
while rospy.get_time() - start <= 20:
rospy.loginfo("Timer: {}".format(rospy.get_time() - start))
self.set_speed(0.1, 0)
rospy.sleep(0.05)
self.change_pwm(True)
rospy.loginfo("Rotating like hell!")
start = rospy.get_rostime().secs
while abs(self.theta - pi) >= 10e-3:
rospy.loginfo("Theta: {}".format(degrees(self.theta)))
self.set_speed(0, 0.5)
rospy.sleep(0.05)
rospy.loginfo("Starting !")
rospy.loginfo("Going forward boss!")
rospy.loginfo("Going forward")
self.move(1, forward=True)
rospy.loginfo("Finished forward")
while abs(self.y - 0.05) >= 0.001:
rospy.loginfo("Y: {}".format(self.y))
self.set_speed(0.1, 0)
rospy.sleep(0.05)
rospy.loginfo("Going backward")
self.move(0.5, forward=False)
rospy.loginfo("Finished backward")
rospy.loginfo("Fuck this shit, I'm done!")
while True:
if rospy.get_rostime().secs - start >= 96:
flag_state = Bool()
flag_state.data = True
self.flag_state_pub.publish(flag_state)
self.set_speed(0, 0)
time.sleep(0.02)
self.send_vel(0, 0)
self.set_speed(0, 0)
self.change_pwm(False)
# self.send_vel(0, 0)
# time.sleep(0.2)
# self.change_pwm(False)
if __name__ == '__main__':
try:
rospy.init_node("coordinator")
CoordinatorNode()
coordinator = CoordinatorNode()
coordinator.run()
rospy.spin()
except rospy.ROSInterruptException:
pass
coordinator/nodes/match.py 0 → 100755
View file @ 18b16210
#!/usr/bin/env python3
import sys
import signal
import threading
import time
import rospy
from mccd_msgs.msg import CmdVel
from mccd_msgs.msg import Odometry
from colors_msgs.msg import Colors
from std_msgs.msg import Int8, Empty
from sensor_msgs.msg import LaserScan as Scan
from std_srvs.srv import SetBool
from std_msgs.msg import Bool, Int32
from fiducial_msgs.msg import FiducialTransformArray
from math import radians, degrees, pi, sqrt, atan2, sin, cos, asin
emergency_distance = 0.40 # in meters
emergency_min_count = 2
lidar_checking_range = 90
send_interval = 0.1
angle_correction_factor = 0.92345679
distance_correction_offset = 0.028
start_slope_distance_threshold = 0.03
end_slope_distance_threshold = 0.20
max_lin_speed = 0.35
min_lin_speed = 0.05
class CoordinatorNode():
def __init__(self):
self.aruco_sub = rospy.Subscriber(
"/fiducial_transforms", FiducialTransformArray, self.aruco_cb)
self.change_pwm_service = rospy.ServiceProxy(
'/mccd/enable_pwm', SetBool)
self.cmd_vel_pub = rospy.Publisher(
'/mccd/cmd_vel', CmdVel, queue_size=5)
self.pull_cord_status_sub = rospy.Subscriber(
'/pull_cord_status', Bool, self.pull_cord_status_callback)
self.pull_cord_status = False
self.odom_sub = rospy.Subscriber(
"/mccd/odom", Odometry, self.odom_callback)
self.flag_state_pub = rospy.Publisher(
"/flag_state", Bool, queue_size=5)
self.arm_state_pub = rospy.Publisher(
"/arm_state", Bool, queue_size=5)
self.estimated_score_pub = rospy.Publisher(
"/estimated_score", Int32, queue_size=1)
self.colors_sub = rospy.Subscriber(
"/color_sensors", Colors, self.colors_cb)
self.fdc_sub = rospy.Subscriber("/limit_switch", Int8, self.fdc_cb)
self.team_sub = rospy.Subscriber("/team", Bool, self.team_cb)
self.reset_error_pub = rospy.Publisher(
"/mccd/reset_error", Empty, queue_size=1)
self.clamps_pub = rospy.Publisher(
'/clamp/cmd_backclamp', Int8, queue_size=1)
self.linear_speed = 0
self.theta = 0
self.x = 0
self.y = 0
self.disable_lidar = False
self.left_fdc_pressed = 0
self.right_fdc_pressed = 0
self.color_left = None
self.color_right = None
self.lidar_subscriber = rospy.Subscriber(
'scan', Scan, self.scan_callback)
self.scan = None
self.team = None
self.weathercock_info = None
self.enable_aruco_check = True
print("Coordinator initialized.")
def euler_from_quaternion(self, x, y, z, w):
"""
Convert a quaternion into euler angles (roll, pitch, yaw)
roll is rotation around x in radians (counterclockwise)
pitch is rotation around y in radians (counterclockwise)
yaw is rotation around z in radians (counterclockwise)
"""
t0 = +2.0 * (w * x + y * z)
t1 = +1.0 - 2.0 * (x * x + y * y)
roll_x = atan2(t0, t1)
t2 = +2.0 * (w * y - z * x)
t2 = +1.0 if t2 > +1.0 else t2
t2 = -1.0 if t2 < -1.0 else t2
pitch_y = asin(t2)
t3 = +2.0 * (w * z + x * y)
t4 = +1.0 - 2.0 * (y * y + z * z)
yaw_z = atan2(t3, t4)
return roll_x, pitch_y, yaw_z # in radians
def aruco_cb(self, data):
# if self.enable_aruco_check:
# return
if (data.transforms is None):
return
for aruco in data.transforms:
if (aruco.fiducial_id == 17):
rot = aruco.transform.rotation
(x, y, z) = self.euler_from_quaternion(
rot.x, rot.y, rot.z, rot.w)
if (y > 0):
self.weathercock_info = "Nord"
else:
self.weathercock_info = "Sud"
print(self.weathercock_info)
def colors_cb(self, data):
self.color_left = data.color_left
self.color_right = data.color_right
def team_cb(self, msg):
self.team = msg.data
def fdc_cb(self, msg):
self.left_fdc_pressed = msg.data & 0b1
self.right_fdc_pressed = (msg.data >> 0b1) & 0b1
pass
def odom_callback(self, odom):
self.linear_speed = odom.linear_speed
self.theta = odom.theta
self.x = odom.x
self.y = odom.y
def change_pwm(self, enable):
print("Waiting for service '/mccd/enable_pwm'")
rospy.wait_for_service('/mccd/enable_pwm')
try:
res = self.change_pwm_service(enable)
return res
except rospy.ServiceException as e:
print("Service call failed: %s" % e)
def reset_error(self):
msg = Empty()
self.reset_error_pub.publish(msg)
def scan_callback(self, scan):
self.scan = scan
def set_speed(self, linear_speed, angular_speed):
linear_speed_cmd = linear_speed
angular_speed_cmd = angular_speed
# First check if we can go in the wanted direction (ie. not obstacle in front/back if going forward/backward)
if self.scan is None:
linear_speed_cmd = 0
angular_speed_cmd = 0
else:
counter = 0
for i, distance in enumerate(self.scan.ranges):
angle = self.scan.angle_min + i * self.scan.angle_increment
if distance < emergency_distance:
# Stop if we detected something in the front and we are going forward
if linear_speed > 0 and (radians(-180) < angle < radians(-lidar_checking_range // 2 - 90) or radians(lidar_checking_range // 2 + 90) < angle < radians(180)):
counter = counter + 1
# Stop if we detected something in the back and we are going backward
elif linear_speed < 0 and radians(-lidar_checking_range // 2) < angle < radians(lidar_checking_range // 2):
counter = counter + 1
if counter >= emergency_min_count and not self.disable_lidar:
linear_speed_cmd = 0
angular_speed_cmd = 0
cmd_vel_msg = CmdVel()
cmd_vel_msg.linear_velocity = linear_speed_cmd
cmd_vel_msg.angular_velocity = angular_speed_cmd
self.cmd_vel_pub.publish(cmd_vel_msg)
def pull_cord_status_callback(self, data):
self.pull_cord_status = data.data
def move(self, target_distance, forward):
target_distance = target_distance - distance_correction_offset
distance_accum = 0
last_x = self.x
last_y = self.y
while distance_accum < target_distance:
start_slope_speed = min_lin_speed + distance_accum / \
start_slope_distance_threshold * \
(max_lin_speed - min_lin_speed)
end_slope_speed = min_lin_speed + \
(target_distance - distance_accum) / \
end_slope_distance_threshold * (max_lin_speed - min_lin_speed)
speed = max(min_lin_speed, min(max_lin_speed,
min(start_slope_speed, end_slope_speed)))
self.set_speed(speed if forward else -speed, 0)
distance_accum = distance_accum + sqrt((self.x - last_x) * (self.x - last_x) +
(self.y - last_y) * (self.y - last_y))
last_x = self.x
last_y = self.y
time.sleep(send_interval)
def shortest_angle(self, from_angle, to_angle):
return atan2(sin(to_angle - from_angle), cos(to_angle - from_angle))
def turn(self, theta, speed=0.5):
theta = theta * angle_correction_factor
last_theta = self.theta
theta_accum = 0 # always positive
while theta_accum < abs(theta):
dTheta = self.shortest_angle(last_theta, self.theta)
theta_accum = theta_accum + abs(dTheta)
if abs(theta_accum - abs(theta)) <= radians(10):
self.set_speed(0, 0.25 if theta > 0 else -0.25)
else:
self.set_speed(0, speed if theta > 0 else -speed)
last_theta = self.theta
time.sleep(send_interval)
def brake_for_n_seconds(self, secs):
start = rospy.get_rostime().secs
while rospy.get_rostime().secs - start < secs:
self.set_speed(0, 0)
time.sleep(send_interval)
def open_clamp(self, left):
clamp_msg = Int8()
if left:
clamp_msg.data = 21
else:
clamp_msg.data = 11
self.clamps_pub.publish(clamp_msg)
time.sleep(0.01)
self.clamps_pub.publish(clamp_msg)
def close_clamp(self, left):
clamp_msg = Int8()
if left:
clamp_msg.data = 20
else:
clamp_msg.data = 10
self.clamps_pub.publish(clamp_msg)
time.sleep(0.01)
self.clamps_pub.publish(clamp_msg)
def raise_clamps(self):
clamp_msg = Int8()
clamp_msg.data = 1
self.clamps_pub.publish(clamp_msg)
time.sleep(0.01)
self.clamps_pub.publish(clamp_msg)
def store_clamps(self):
clamp_msg = Int8()
clamp_msg.data = 0
self.clamps_pub.publish(clamp_msg)
time.sleep(0.01)
self.clamps_pub.publish(clamp_msg)
def lower_flag(self):
msg = Bool()
msg.data = False
self.flag_state_pub.publish(msg)
time.sleep(0.1)
self.flag_state_pub.publish(msg)
def raise_flag(self):
msg = Bool()
msg.data = True
self.flag_state_pub.publish(msg)
time.sleep(0.01)
self.flag_state_pub.publish(msg)
def lower_arm(self):
msg = Bool()
msg.data = True
self.arm_state_pub.publish(msg)
time.sleep(0.01)
self.arm_state_pub.publish(msg)
def raise_arm(self):
msg = Bool()
msg.data = False
self.arm_state_pub.publish(msg)
time.sleep(0.01)
self.arm_state_pub.publish(msg)
def hit_wall(self, forward=True, timeout=5):
start_fdc = rospy.get_rostime().secs
while not (self.left_fdc_pressed and self.right_fdc_pressed):
self.set_speed(0.2 if forward else -0.2, 0)
time.sleep(send_interval)
if rospy.get_rostime().secs - start_fdc > timeout:
return False
self.reset_error()
self.brake_for_n_seconds(0.2)
return True
def run(self):
self.raise_arm()
self.lower_flag()
self.store_clamps()
self.reset_error()
self.set_speed(0, 0)
self.change_pwm(True)
rospy.loginfo("Ready")
while self.pull_cord_status != True:
self.set_speed(0, 0)
time.sleep(send_interval)
rospy.loginfo("Tirette retirée")
start = rospy.get_rostime().secs
hit_ok = True
if self.team: # yellow
self.move(0.5, forward=True)
self.turn(radians(-90))
self.move(0.25, forward=True)
self.turn(radians(88))
self.move(0.35, forward=False)
rospy.loginfo("Ecocup poussées")
self.move(0.15, forward=True)
self.turn(radians(-90))
self.hit_wall()
rospy.loginfo("Premier mur touché")
self.move(0.07, forward=False)
self.turn(radians(-90))
hit_ok = self.hit_wall()
if hit_ok:
rospy.loginfo("Second mur touché")
self.move(0.06, forward=False)
rospy.loginfo("Activation du phare")
self.set_speed(0, 0)
time.sleep(send_interval)
self.lower_arm()
self.brake_for_n_seconds(1)
self.raise_arm()
self.brake_for_n_seconds(1)
rospy.loginfo("Phare activé")
self.disable_lidar = True
# Recule
self.move(0.55, forward=False)
self.move(0.10, forward=True)
self.turn(radians(-90))
self.disable_lidar = False
# Retourne vers le port côté gauche
self.move(0.82, forward=True)
self.turn(radians(-90))
# Pousse l'autre écocup
self.move(0.2, forward=False)
# Part du chenal gauche et va se calibrer contre le premier mur
self.move(0.15, forward=True)
self.turn(radians(90))
self.hit_wall(timeout=999)
# Calibration sur le second mur
self.move(0.06, forward=False)
self.turn(radians(92))
self.hit_wall()
# Tourne, puis baisse le bras, puis avance pour faire basculer les manches à air
self.move(0.06, forward=False)
self.turn(radians(189))
self.lower_arm()
self.brake_for_n_seconds(2)
self.move(0.41, forward=True)
# Tourne pour faire basculer la deuxième
self.turn(radians(-90))
self.raise_arm()
self.brake_for_n_seconds(1)
# Se retourne pour se recalibrer sur le mur
self.move(0.05, forward=True)
self.turn(radians(-180), 1.0)
self.hit_wall()
# Repart et va lire la valeur de la girouette
self.move(0.6, forward=False)
self.turn(radians(30))
start_girouette = rospy.get_time()
count_north = 0
count_south = 0
while rospy.get_time() - start_girouette < 2:
if self.weathercock_info == "Nord":
count_north = count_north + 1
elif self.weathercock_info == "Sud":
count_south = count_south + 1
self.set_speed(0, 0)
time.sleep(send_interval)
# Prend la décision de la zone de mouillage
self.turn(radians(-30))
if count_north > count_south:
self.move(0.60, forward=False)
self.turn(radians(-90))
self.move(0.2, forward=False)
else:
self.set_speed(0, 0)
self.reset_error()
self.change_pwm(False)
else: # blue
self.move(0.5, forward=True)
self.turn(radians(90))
self.move(0.28, forward=True)
self.turn(radians(-88))
self.move(0.35, forward=False)
rospy.loginfo("Ecocup poussées")
self.move(0.15, forward=True)
self.turn(radians(90))
self.hit_wall()
rospy.loginfo("Premier mur touché")
self.move(0.07, forward=False)
self.turn(radians(90))
hit_ok = self.hit_wall()
if hit_ok:
rospy.loginfo("Second mur touché")
self.move(0.06, forward=False)
self.turn(radians(-190))
rospy.loginfo("Activation du phare")
self.set_speed(0, 0)
time.sleep(send_interval)
self.lower_arm()
self.brake_for_n_seconds(2)
self.raise_arm()
self.brake_for_n_seconds(1)
rospy.loginfo("Phare activé")
self.disable_lidar = True
# Recule
self.move(0.55, forward=True)
self.move(0.10, forward=False)
self.turn(radians(-90))
self.disable_lidar = False
# Retourne vers le port côté gauche
self.move(0.82, forward=True)
self.turn(radians(-90))
# Pousse l'autre écocup
self.move(0.38, forward=True)
# Part du chenal gauche et va se calibrer contre le premier mur
self.move(0.15, forward=False)
self.turn(radians(90))
self.hit_wall(timeout=999)
# Calibration sur le second mur
self.move(0.06, forward=False)
self.turn(radians(-92))
self.hit_wall()
# Tourne, puis baisse le bras, puis avance pour faire basculer les manches à air
self.move(0.06, forward=False)
self.turn(radians(-90))
self.lower_arm()
self.brake_for_n_seconds(1)
self.turn(radians(90))
self.move(0.41, forward=False)
self.raise_arm()
self.brake_for_n_seconds(1)
self.turn(radians(90))
# Se recalibre sur le mur
self.hit_wall()
# Repart et va lire la valeur de la girouette
self.move(0.6, forward=False)
self.turn(radians(-30))
start_girouette = rospy.get_time()
count_north = 0
count_south = 0
while rospy.get_time() - start_girouette < 2:
if self.weathercock_info == "Nord":
count_north = count_north + 1
elif self.weathercock_info == "Sud":
count_south = count_south + 1
self.set_speed(0, 0)
time.sleep(send_interval)
# Prend la décision de la zone de mouillage
self.turn(radians(30))
if count_north > count_south:
self.move(0.60, forward=False)
self.turn(radians(90))
self.move(0.2, forward=False)
else:
self.set_speed(0, 0)
self.reset_error()
self.change_pwm(False)
score_msg = Int32()
score_msg.data = 66
flag_state = Bool()
flag_state.data = True
while True:
if rospy.get_rostime().secs - start > 95:
self.flag_state_pub.publish(flag_state)
self.estimated_score_pub.publish(score_msg)
self.set_speed(0, 0)
time.sleep(send_interval)
self.change_pwm(False)
def signal_handler(sig, frame):
print('Goodbye!')
sys.exit(0)
if __name__ == '__main__':
signal.signal(signal.SIGINT, signal_handler)
try:
rospy.init_node("match")
coordinator = CoordinatorNode()
coordinator.run()
except rospy.ROSInterruptException:
pass
homologation/launch/homologation.launch 0 → 100644
View file @ 18b16210
<launch>
<arg name="mccd_port" default="/dev/mccd"/>
<arg name="mccd_baud" default="115200"/>
<include file="$(find motors_control_card)/launch/mccd.launch">
<arg name="port" value="$(arg mccd_port)" />
<arg name="baud" value="$(arg mccd_baud)" />
</include>
<node name="front_laser" pkg="laser_filters" type="scan_to_scan_filter_chain" output="screen">
<remap from="scan" to="front_scan" />
<rosparam command="load" file="$(find homologation)/config/front_laser.yaml" />
</node>
<node name="rear_laser" pkg="laser_filters" type="scan_to_scan_filter_chain" output="screen">
<remap from="scan" to="rear_scan" />
<rosparam command="load" file="$(find homologation)/config/rear_laser.yaml" />
</node>
</launch>
velocity_controller/package.xml
View file @ 18b16210
......@@ -57,7 +57,7 @@
<build_export_depend>rplidar_ros</build_export_depend>
<exec_depend>geometry_msgs</exec_depend>
<exec_depend>rplidar_ros</exec_depend>
<run_depend>sensor_msgs</run_depend>
<exec_depend>sensor_msgs</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
......