diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000000000000000000000000000000000000..1d74e21965c4f858f5f818a270e64e1bfad7d843
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1 @@
+.vscode/
diff --git a/.vscode/settings.json b/.vscode/settings.json
new file mode 100644
index 0000000000000000000000000000000000000000..4fa938b26fce46ac9eab7db76124dab3a7a925fa
--- /dev/null
+++ b/.vscode/settings.json
@@ -0,0 +1,10 @@
+<<<<<<< HEAD
+{}
+=======
+{
+ "ros.distro": "galactic",
+ "python.autoComplete.extraPaths": [
+ "/opt/ros/galactic/lib/python3.8/site-packages"
+ ]
+}
+>>>>>>> bdceb05... Add clamps srv
diff --git a/actions_manager/CMakeLists.txt b/actions_manager/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..b185ebec185335f06d2d333e72e9d25b56de4ef7
--- /dev/null
+++ b/actions_manager/CMakeLists.txt
@@ -0,0 +1,199 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(actions_manager)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+ colors_msgs
+ ctrlpan_msgs
+ std_msgs
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+## * add a build_depend tag for "message_generation"
+## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+## but can be declared for certainty nonetheless:
+## * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+## * add "message_generation" and every package in MSG_DEP_SET to
+## find_package(catkin REQUIRED COMPONENTS ...)
+## * add "message_runtime" and every package in MSG_DEP_SET to
+## catkin_package(CATKIN_DEPENDS ...)
+## * uncomment the add_*_files sections below as needed
+## and list every .msg/.srv/.action file to be processed
+## * uncomment the generate_messages entry below
+## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+# FILES
+# Message1.msg
+# Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+# FILES
+# Service1.srv
+# Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+# FILES
+# Action1.action
+# Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+# DEPENDENCIES
+# colors_msgs# common_msgs# ctrlpan_msgs# std_msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+## * add "dynamic_reconfigure" to
+## find_package(catkin REQUIRED COMPONENTS ...)
+## * uncomment the "generate_dynamic_reconfigure_options" section below
+## and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+# cfg/DynReconf1.cfg
+# cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+# INCLUDE_DIRS include
+# LIBRARIES actions_manager
+# CATKIN_DEPENDS colors_msgs common_msgs ctrlpan_msgs std_msgs
+# DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+ ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+# src/${PROJECT_NAME}/actions_manager.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+# add_executable(${PROJECT_NAME}_node src/actions_manager_node.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+# ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+# scripts/my_python_script
+# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
+# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+# FILES_MATCHING PATTERN "*.h"
+# PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+# # myfile1
+# # myfile2
+# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_actions_manager.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)
diff --git a/actions_manager/nodes/actions_manager.py b/actions_manager/nodes/actions_manager.py
new file mode 100755
index 0000000000000000000000000000000000000000..f1306abca2a3d099080555c04b6ca19f765f34af
--- /dev/null
+++ b/actions_manager/nodes/actions_manager.py
@@ -0,0 +1,14 @@
+#!/usr/bin/env python3
+
+import rospy
+from src.robot import Robot
+
+if __name__ == '__main__':
+
+ rospy.init_node("actions_manager")
+
+ try:
+ robot = Robot()
+ rospy.spin()
+ except rospy.ROSInterruptException:
+ pass
diff --git a/actions_manager/package.xml b/actions_manager/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..7d002bbb29a159a9f073dcca9da97104e12ce386
--- /dev/null
+++ b/actions_manager/package.xml
@@ -0,0 +1,68 @@
+<?xml version="1.0"?>
+<package format="2">
+ <name>actions_manager</name>
+ <version>0.0.0</version>
+ <description>The actions_manager package</description>
+
+ <!-- One maintainer tag required, multiple allowed, one person per tag -->
+ <!-- Example: -->
+ <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+ <maintainer email="thomas@todo.todo">thomas</maintainer>
+
+
+ <!-- One license tag required, multiple allowed, one license per tag -->
+ <!-- Commonly used license strings: -->
+ <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+ <license>TODO</license>
+
+
+ <!-- Url tags are optional, but multiple are allowed, one per tag -->
+ <!-- Optional attribute type can be: website, bugtracker, or repository -->
+ <!-- Example: -->
+ <!-- <url type="website">http://wiki.ros.org/actions_manager</url> -->
+
+
+ <!-- Author tags are optional, multiple are allowed, one per tag -->
+ <!-- Authors do not have to be maintainers, but could be -->
+ <!-- Example: -->
+ <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+ <!-- The *depend tags are used to specify dependencies -->
+ <!-- Dependencies can be catkin packages or system dependencies -->
+ <!-- Examples: -->
+ <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+ <!-- <depend>roscpp</depend> -->
+ <!-- Note that this is equivalent to the following: -->
+ <!-- <build_depend>roscpp</build_depend> -->
+ <!-- <exec_depend>roscpp</exec_depend> -->
+ <!-- Use build_depend for packages you need at compile time: -->
+ <!-- <build_depend>message_generation</build_depend> -->
+ <!-- Use build_export_depend for packages you need in order to build against this package: -->
+ <!-- <build_export_depend>message_generation</build_export_depend> -->
+ <!-- Use buildtool_depend for build tool packages: -->
+ <!-- <buildtool_depend>catkin</buildtool_depend> -->
+ <!-- Use exec_depend for packages you need at runtime: -->
+ <!-- <exec_depend>message_runtime</exec_depend> -->
+ <!-- Use test_depend for packages you need only for testing: -->
+ <!-- <test_depend>gtest</test_depend> -->
+ <!-- Use doc_depend for packages you need only for building documentation: -->
+ <!-- <doc_depend>doxygen</doc_depend> -->
+ <buildtool_depend>catkin</buildtool_depend>
+ <build_depend>colors_msgs</build_depend>
+ <build_depend>ctrlpan_msgs</build_depend>
+ <build_depend>std_msgs</build_depend>
+ <build_export_depend>colors_msgs</build_export_depend>
+ <build_export_depend>ctrlpan_msgs</build_export_depend>
+ <build_export_depend>std_msgs</build_export_depend>
+ <exec_depend>colors_msgs</exec_depend>
+ <exec_depend>ctrlpan_msgs</exec_depend>
+ <exec_depend>std_msgs</exec_depend>
+
+
+ <!-- The export tag contains other, unspecified, tags -->
+ <export>
+ <!-- Other tools can request additional information be placed here -->
+
+ </export>
+</package>
diff --git a/actions_manager/src/action.py b/actions_manager/src/action.py
new file mode 100644
index 0000000000000000000000000000000000000000..b471833404f54f75b65b2d14b705148b44dcf7b3
--- /dev/null
+++ b/actions_manager/src/action.py
@@ -0,0 +1,7 @@
+import queue
+
+
+class Action():
+
+ def __init__(self):
+ self.task_queue = queue.Queue()
diff --git a/actions_manager/src/position.py b/actions_manager/src/position.py
new file mode 100644
index 0000000000000000000000000000000000000000..7d08753c731bace5b7830c716b0fd80e917f768a
--- /dev/null
+++ b/actions_manager/src/position.py
@@ -0,0 +1,19 @@
+from math import sqrt
+
+
+def Position():
+
+ def __init__(self, x, y, theta):
+ self.x = x
+ self.y = y
+ self.theta = theta
+
+ def distance_sq(self, x, y):
+ dx = self.x - x
+ dy = self.y - y
+ return dx * dx + dy * dy
+
+ def distance(self, x, y):
+ dx = self.x - x
+ dy = self.y - y
+ return sqrt(dx * dx + dy * dy)
diff --git a/actions_manager/src/robot.py b/actions_manager/src/robot.py
new file mode 100644
index 0000000000000000000000000000000000000000..49b28113ad45319864508731c5a456f93569efff
--- /dev/null
+++ b/actions_manager/src/robot.py
@@ -0,0 +1,115 @@
+from time import sleep
+from position import Position
+from math import pi
+from utils import compare_distance, compare_theta
+import rospy
+from geometry_msgs.msg import Twist
+from std_srvs.srv import SetBool
+from colors_msgs.msg import Colors
+
+LINEAR_SPEED = 0.2
+ROTATION_SPEED = 3.14 # rad.s-1
+
+SLEEP_INTERVAL = 0.1
+
+
+class Robot():
+
+ def __init__(self):
+ self.position = Position(0, 0, 0)
+ self.left_color = None
+ self.right_color = None
+
+ # TODO send twist to a multiplexer (velocity control node) for the navigation (containing LiDAR emergency stop)
+ self.cmd_vel_pub = rospy.Publisher(
+ "/actions_manager/cmd_vel", Twist, queue_size=5)
+ self.activate_windsocks_arm_srv = rospy.ServiceProxy(
+ "/ctrl_panel/arm", SetBool)
+ self.color_sensor_sub = rospy.Subscriber(
+ "/color_sensors", Colors, self.color_sensor_cb)
+
+ def color_sensor_cb(self, data):
+ self.left_color = data.color_left
+ self.right_color = data.color_right
+
+ def rotate_to(self, theta):
+
+ theta = theta % (2 * pi)
+
+ start_theta = self.position.theta
+
+ dTheta = (theta - start_theta) % (2 * pi)
+
+ # Optimize the rotation by taking the shortest angle position and inverting rotation direction if needed
+ clockwise = False
+ if dTheta >= pi:
+ theta = (2 * pi - theta) % (2 * pi)
+ clockwise = True
+ else: # 0 < theta < pi
+ clockwise = False
+
+ while not compare_theta(theta, self.position.theta):
+ self.send_vel(
+ 0, -ROTATION_SPEED if clockwise else ROTATION_SPEED)
+ self.sleep(SLEEP_INTERVAL)
+
+ def lower_windsocks_arm(self):
+ self.activate_windsocks_arm_srv(True)
+
+ def raise_windsocks_arm(self):
+ self.activate_windsocks_arm_srv(False)
+
+ def lower_multiclamp_arm(self):
+ pass
+
+ def raise_multiclamp_arm(self):
+ pass
+
+ def close_multiclamp_servo(self, servo_number):
+ pass
+
+ def open_multiclamp_servo(self, servo_number):
+ pass
+
+ def lower_monoclamp(self, left):
+ pass
+
+ def raise_monoclamp(self, left):
+ pass
+
+ def open_monoclamp(self, left):
+ pass
+
+ def close_monoclamp(self, left):
+ pass
+
+ def send_vel(self, lin_vel, ang_vel):
+ twist = Twist()
+ twist.linear.x = lin_vel
+ twist.linear.y = 0
+ twist.linear.z = 0
+ twist.angular.x = 0
+ twist.angular.y = 0
+ twist.angular.z = ang_vel
+ self.cmd_vel_pub.publish(twist)
+
+ # /!\ /!\ /!\ /!\ /!\
+ # TODO: slow down when getting close to the objective
+
+ def forward(self, distance):
+
+ start_position = self.position
+
+ # We check that we approximately run the distance we wanted, if not, we continue to go forward
+ while not compare_distance(start_position, self.position, distance):
+ self.send_vel(LINEAR_SPEED, 0)
+ sleep(SLEEP_INTERVAL)
+
+ def backward(self, distance):
+
+ start_position = self.position
+
+ # We check that we approximately run the distance we wanted, if not, we continue to go forward
+ while not compare_distance(start_position, self.position, distance):
+ self.send_vel(-LINEAR_SPEED, 0)
+ sleep(SLEEP_INTERVAL)
diff --git a/actions_manager/src/tasks/follow_line.py b/actions_manager/src/tasks/follow_line.py
new file mode 100644
index 0000000000000000000000000000000000000000..e7ba5c76ac197fd47f841fbf83ef70d81ccf0f16
--- /dev/null
+++ b/actions_manager/src/tasks/follow_line.py
@@ -0,0 +1,29 @@
+from time import sleep
+from task import Task
+
+
+class FollowLine(Task):
+
+ def __init__(self, robot):
+ self.robot = robot
+
+ def run(self):
+
+ ang_vel = 0
+ lin_vel = 0.1
+
+ update_interval = 0.03
+
+ # TODO add an end condition (eg. touching the wall)
+ while True:
+ if (self.robot.left_color == 1):
+ ang_vel = ang_vel + 0.001
+
+ if (self.robot.right_color == 1):
+ ang_vel = ang_vel - 0.001
+
+ if (self.robot.right_color == 2 and self.color.left_color == 2):
+ ang_vel = 0
+
+ self.robot.send_vel(lin_vel, ang_vel)
+ sleep(update_interval)
diff --git a/actions_manager/src/tasks/goto.py b/actions_manager/src/tasks/goto.py
new file mode 100644
index 0000000000000000000000000000000000000000..258727a0d8c2427fd1e19eaae27e0038401d4368
--- /dev/null
+++ b/actions_manager/src/tasks/goto.py
@@ -0,0 +1,28 @@
+from task import Task
+from math import pi
+from utils import compute_signed_angle, compare_distance, compare_theta
+
+
+class Goto(Task):
+
+ def __init__(self, robot, target_pos, backward):
+ self.robot = robot
+ self.target_pos = target_pos
+ self.backward = backward
+
+ def run(self):
+
+ if self.backward:
+
+ angle_to_target = compute_signed_angle(
+ self.robot.position, self.target_pos)
+ angle_to_target = (angle_to_target + pi) % (2 * pi)
+ self.robot.rotate_to(angle_to_target)
+ self.robot.backward(self.robot.position.distance(self.target_pos))
+ self.robot.rotate_to(self.target_pos.theta)
+ else:
+ angle_to_target = compute_signed_angle(
+ self.robot.position, self.target_pos)
+ self.robot.rotate_to(angle_to_target)
+ self.robot.forward(self.robot.position.distance(self.target_pos))
+ self.robot.rotate_to(self.target_pos.theta)
diff --git a/actions_manager/src/tasks/monoclamp_grab.py b/actions_manager/src/tasks/monoclamp_grab.py
new file mode 100644
index 0000000000000000000000000000000000000000..b79a59c923708ee9878ad2a9f08671311dac96e6
--- /dev/null
+++ b/actions_manager/src/tasks/monoclamp_grab.py
@@ -0,0 +1,20 @@
+from time import sleep
+from task import Task
+
+
+class MonoclampGrab(Task):
+
+ def __init__(self, robot, left):
+ self.robot = robot
+ self.left = left
+
+ def run(self):
+
+ self.robot.open_monoclamp(self.left)
+ sleep(0.5)
+ self.robot.lower_monoclamp(self.left)
+ sleep(1)
+ self.robot.close_monoclamp(self.left)
+ sleep(0.5)
+ self.robot.raise_monoclamp(self.left)
+ sleep(1)
diff --git a/actions_manager/src/tasks/monoclamp_place.py b/actions_manager/src/tasks/monoclamp_place.py
new file mode 100644
index 0000000000000000000000000000000000000000..2c18ce72d5b8b307e7c173ef904a4f5d10b6651d
--- /dev/null
+++ b/actions_manager/src/tasks/monoclamp_place.py
@@ -0,0 +1,20 @@
+from time import sleep
+from task import Task
+
+
+class MonoclampPlace(Task):
+
+ def __init__(self, robot, left):
+ self.robot = robot
+ self.left = left
+
+ def run(self):
+
+ self.robot.lower_monoclamp(self.left)
+ sleep(1)
+ self.robot.open_monoclamp(self.left)
+ sleep(0.5)
+ self.robot.raise_monoclamp(self.left)
+ sleep(1)
+ self.robot.close_monoclamp(self.left)
+ sleep(0.5)
diff --git a/actions_manager/src/tasks/multiclamp_grab.py b/actions_manager/src/tasks/multiclamp_grab.py
new file mode 100644
index 0000000000000000000000000000000000000000..ceba1e0309b4c55e31570a5e3901472c07320737
--- /dev/null
+++ b/actions_manager/src/tasks/multiclamp_grab.py
@@ -0,0 +1,24 @@
+from time import sleep
+from task import Task
+
+
+class MulticlampGrab(Task):
+
+ def __init__(self, robot, cup_indexes={1, 2, 3, 4, 5}):
+ self.robot = robot
+ self.cup_indexes = cup_indexes
+
+ def run(self):
+
+ # todo open all
+ for cup_idx in self.cup_indexes:
+ self.robot.open_multiclamp_servo(cup_idx)
+
+ self.robot.lower_multiclamp_arm()
+ sleep(1) # wait for the multiclamp to go down
+
+ for cup_idx in self.cup_indexes:
+ self.robot.close_multiclamp_servo(cup_idx)
+
+ self.robot.raise_multiclamp_arm()
+ sleep(1) # wait for the multiclamp to go up
diff --git a/actions_manager/src/tasks/multiclamp_place.py b/actions_manager/src/tasks/multiclamp_place.py
new file mode 100644
index 0000000000000000000000000000000000000000..cd7716da5457cee9db2ee3ef0914b9df279ab4ae
--- /dev/null
+++ b/actions_manager/src/tasks/multiclamp_place.py
@@ -0,0 +1,23 @@
+from time import sleep
+from task import Task
+
+
+class MulticlampPlace(Task):
+
+ def __init__(self, robot, cup_indexes={1, 2, 3, 4, 5}):
+ self.robot = robot
+ self.cup_indexes = cup_indexes
+
+ def run(self):
+
+ self.robot.lower_multiclamp_arm()
+ sleep(1) # wait for the multiclamp to go down
+
+ for cup_idx in self.cup_indexes:
+ self.robot.open_multiclamp_servo(cup_idx)
+
+ self.robot.raise_multiclamp_arm()
+ sleep(1) # wait for the multiclamp to go up
+
+ for cup_idx in self.cup_indexes:
+ self.robot.close_multiclamp_servo(cup_idx)
diff --git a/actions_manager/src/tasks/swing_windsocks.py b/actions_manager/src/tasks/swing_windsocks.py
new file mode 100644
index 0000000000000000000000000000000000000000..a073ff26b0023cb1cc153dd20a20652905e6a008
--- /dev/null
+++ b/actions_manager/src/tasks/swing_windsocks.py
@@ -0,0 +1,30 @@
+from time import sleep
+from task import Task
+from utils import compare_distance, compare_theta
+from math import pi
+
+TRAVEL_DISTANCE = 0.5 # meters
+
+
+class SwingWindsocks(Task):
+
+ def __init__(self, robot, state):
+ self.robot = robot
+ self.state = state
+ self.start_pos = None
+
+ def run(self):
+ self.start_pos = self.robot.position
+
+ # Lower the arm and wait for it to complete
+ # Ensure we are at the right rotation
+ # Go forward TRAVEL_DISTANCE meters
+ # 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.forward(TRAVEL_DISTANCE)
+ self.robot.raise_windsocks_arm()
+ sleep(0.5)
diff --git a/actions_manager/src/tasks/task.py b/actions_manager/src/tasks/task.py
new file mode 100644
index 0000000000000000000000000000000000000000..2ce1ca40987e6b7dd731ab2f6ac517f4e339c391
--- /dev/null
+++ b/actions_manager/src/tasks/task.py
@@ -0,0 +1,23 @@
+from abc import abstractmethod
+
+
+class Task():
+ """
+ A task is a small and atomic action to perform.
+
+ For instance : goto a position, grab an object, ...
+ """
+
+ def estimatePointsAndTime(self):
+ """
+ return a tuple (points, time) due to the task
+ This estimation is done for a specific state (robot position, ..)
+ """
+ return (0, 0)
+
+ @abstractmethod
+ def run(self):
+ """
+ Blocking call to process the task
+ """
+ pass
diff --git a/actions_manager/src/utils.py b/actions_manager/src/utils.py
new file mode 100644
index 0000000000000000000000000000000000000000..49411c52cc93ec49e6a457ac324272230beb2bc1
--- /dev/null
+++ b/actions_manager/src/utils.py
@@ -0,0 +1,32 @@
+from math import atan2, pi
+
+EPSILON_POSITION = 0.01 # +/- 1cm
+EPSILON_ROTATION = 0.017 # +/- 1°
+
+
+def compute_signed_angle(from_pos, to_pos):
+ """
+ Return the angle of the vector going from from_pos to to_pos. The angle is included in the interval [0; 2*pi]
+ """
+ vx = to_pos.x - from_pos.x
+ vy = to_pos.y - from_pos.y
+ signed_angle = atan2(vy, vx)
+ # Wrap between [0; 2pi]
+ signed_angle = signed_angle % (2 * pi)
+ return signed_angle
+
+
+def compare_distance(pos_a, pos_b, distance=0):
+ """
+ Return true if position A and position B are at a given distance
+ This internally uses EPSILON_POSITION to take imprecisions into account
+ """
+ return pos_a.distance_sq(pos_b) - distance * distance <= EPSILON_POSITION * EPSILON_POSITION
+
+
+def compare_theta(theta_a, theta_b):
+ """
+ Return true if theta A and theta B are close enough.
+ This internally uses EPSILON_POSITION to take imprecisions into account
+ """
+ return abs(theta_a - theta_b) <= EPSILON_ROTATION
diff --git a/homologation/.gitignore b/coordinator/.gitignore
similarity index 100%
rename from homologation/.gitignore
rename to coordinator/.gitignore
diff --git a/homologation/CMakeLists.txt b/coordinator/CMakeLists.txt
similarity index 99%
rename from homologation/CMakeLists.txt
rename to coordinator/CMakeLists.txt
index d2fad16773968a2b53db4a44a680cb71815d9e31..59e0b2d5fb2de2fcc3e3bfcc6f058d86fb507fc9 100644
--- a/homologation/CMakeLists.txt
+++ b/coordinator/CMakeLists.txt
@@ -1,5 +1,5 @@
cmake_minimum_required(VERSION 3.0.2)
-project(homologation)
+project(coordinator)
## Compile as C++11, supported in ROS Kinetic and newer
# add_compile_options(-std=c++11)
diff --git a/homologation/config/front_laser.yaml b/coordinator/config/front_laser.yaml
similarity index 100%
rename from homologation/config/front_laser.yaml
rename to coordinator/config/front_laser.yaml
diff --git a/homologation/config/rear_laser.yaml b/coordinator/config/rear_laser.yaml
similarity index 100%
rename from homologation/config/rear_laser.yaml
rename to coordinator/config/rear_laser.yaml
diff --git a/coordinator/launch/homologation.launch b/coordinator/launch/homologation.launch
new file mode 100644
index 0000000000000000000000000000000000000000..0b9b06655c6972a8de5ea75cc8eff8e112bc7e12
--- /dev/null
+++ b/coordinator/launch/homologation.launch
@@ -0,0 +1,44 @@
+<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"/>
+
+ <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="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"/>
+ <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" output="screen">
+ <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" output="screen">
+ <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="local" name="homologation" pkg="coordinator" type="homologation.py" output="screen"/>
+
+</launch>
\ No newline at end of file
diff --git a/coordinator/launch/match.launch b/coordinator/launch/match.launch
new file mode 100644
index 0000000000000000000000000000000000000000..bff7bc3555805c2e9ffa8e72f5bc1adaf202f713
--- /dev/null
+++ b/coordinator/launch/match.launch
@@ -0,0 +1,98 @@
+<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>
diff --git a/coordinator/nodes/homologation.py b/coordinator/nodes/homologation.py
new file mode 100755
index 0000000000000000000000000000000000000000..0fa076c9fb6050c279f7b2e9846fe5dd998b02e6
--- /dev/null
+++ b/coordinator/nodes/homologation.py
@@ -0,0 +1,158 @@
+#!/usr/bin/env python3
+
+import threading
+import time
+import rospy
+from geometry_msgs.msg import Twist
+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, sqrt
+
+
+emergency_distance = 0.40 # in meters
+emergency_min_count = 2
+
+
+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=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.scan = None
+
+ print("Coordinator initialized.")
+
+ 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 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:
+ 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(-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
+
+ twist = Twist()
+ twist.linear.x = linear_speed_cmd
+ twist.angular.z = angular_speed_cmd
+ self.cmd_vel_pub.publish(twist)
+
+ 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.02)
+
+ self.change_pwm(True)
+
+ start = rospy.get_rostime().secs
+
+ rospy.loginfo("Starting !")
+
+ rospy.loginfo("Going forward")
+ self.move(1, forward=True)
+ rospy.loginfo("Finished forward")
+
+ rospy.loginfo("Going backward")
+ self.move(0.5, forward=False)
+ rospy.loginfo("Finished backward")
+
+ 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.set_speed(0, 0)
+ self.change_pwm(False)
+
+
+if __name__ == '__main__':
+ try:
+ rospy.init_node("coordinator")
+ coordinator = CoordinatorNode()
+ coordinator.run()
+ rospy.spin()
+ except rospy.ROSInterruptException:
+ pass
diff --git a/coordinator/nodes/line_follower.py b/coordinator/nodes/line_follower.py
new file mode 100755
index 0000000000000000000000000000000000000000..ef46a9c8da5e7bb82180ff644d9d359396794e06
--- /dev/null
+++ b/coordinator/nodes/line_follower.py
@@ -0,0 +1,95 @@
+#!/usr/bin/env python3
+
+import threading
+import time
+import rospy
+from geometry_msgs.msg import Twist, Vector3
+from std_srvs.srv import SetBool
+
+
+class HSVColor():
+ def __init__(self, h, s, v):
+ self.h = h
+ self.s = s
+ self.v = v
+
+
+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.left_color_sub = rospy.Subscriber(
+ 'color_sensor/left', Vector3, self.left_color_cb)
+ self.right_color_sub = rospy.Subscriber(
+ 'color_sensor/right', Vector3, self.right_color_cb)
+ self.exec_thread = threading.Thread(target=self.run)
+ self.exec_thread.start()
+
+ print("Coordinator initialized.")
+
+ 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 send_vel(self, linear_vel, angular_vel):
+ twist = Twist()
+ twist.linear.x = linear_vel
+ twist.linear.y = 0
+ twist.linear.z = 0
+ twist.angular.x = 0
+ twist.angular.y = 0
+ twist.angular.z = angular_vel
+ self.cmd_vel_pub.publish(twist)
+
+ def left_color_cb(self, hsv):
+ self.last_left_color = self.left_color
+ self.left_color = HSVColor(hsv.x, hsv.y, hsv.z)
+
+ def right_color_cb(self, hsv):
+ self.last_right_color = self.right_color
+ self.right_color = HSVColor(hsv.x, hsv.y, hsv.z)
+
+ def line_follow_update(self):
+ # TODO Line follow logic
+
+ # Left sees white
+ if self.left_color.s > 0.96:
+ rospy.loginfo("left sees white")
+
+ self.send_vel(0.1, 0)
+ time.sleep(0.1)
+
+ def run(self):
+
+ rospy.loginfo("Waiting for 2s before starting...")
+ time.sleep(2.0)
+
+ self.change_pwm(True)
+ self.send_vel(0, 0)
+
+ start_time = rospy.get_rostime()
+
+ # Roule pendant 10 secondes
+ while rospy.get_rostime().secs - start_time.secs <= 10:
+ self.line_follow_update()
+
+ self.send_vel(0, 0)
+ time.sleep(0.2)
+ self.change_pwm(False)
+
+
+if __name__ == '__main__':
+ try:
+ rospy.init_node("line_follower")
+ CoordinatorNode()
+ rospy.spin()
+ except rospy.ROSInterruptException:
+ pass
diff --git a/coordinator/nodes/match.py b/coordinator/nodes/match.py
new file mode 100755
index 0000000000000000000000000000000000000000..8dc26edce7a6073b9374858d8c0dd28c45cdc982
--- /dev/null
+++ b/coordinator/nodes/match.py
@@ -0,0 +1,598 @@
+#!/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
diff --git a/homologation/package.xml b/coordinator/package.xml
similarity index 94%
rename from homologation/package.xml
rename to coordinator/package.xml
index 3920eaa3baacc0f800229375633f1fb0c0b0a773..b1951db9f2805c1d627e73c39c6b602f17935176 100644
--- a/homologation/package.xml
+++ b/coordinator/package.xml
@@ -1,8 +1,8 @@
<?xml version="1.0"?>
<package format="2">
- <name>homologation</name>
+ <name>coordinator</name>
<version>0.1.0</version>
- <description>Homologation stack package</description>
+ <description>Coordinator stack package</description>
<maintainer email="charles.javerliat@gmail.com">Charles JAVERLIAT</maintainer>
<license>GPLv3</license>
diff --git a/homologation/scripts/homologation.py b/homologation/scripts/homologation.py
deleted file mode 100644
index 78840b04ec99ee3c4688a407c8d2e186e302db53..0000000000000000000000000000000000000000
--- a/homologation/scripts/homologation.py
+++ /dev/null
@@ -1,100 +0,0 @@
-#!/usr/bin/env python
-
-import threading
-import time
-import rospy
-from geometry_msgs.msg import Twist
-from sensor_msgs.msg import LaserScan as Scan
-from std_srvs.srv import SetBool, SetBoolResponse
-from std_msgs.msg import Bool
-
-
-emergency_distance = 0.45 # in meters
-emergency_min_count = 2
-
-
-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.pull_cord_status_sub = rospy.Subscriber(
- '/pull_cord_status', Bool, self.pull_cord_status_callback)
- self.pull_cord_status = False
-
- self.emergency_count = 0
-
- # TODO(cjaverliat): subscribe to front_scan and rear_scan instead and check on this
- self.lidar_subscriber = rospy.Subscriber(
- 'scan', Scan, self.scan_callback)
-
- self.exec_thread = threading.Thread(target=self.run)
- self.exec_thread.start()
-
- print("Coordinator initialized.")
-
- 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 scan_callback(self, scan):
- if (min(scan.ranges) <= emergency_distance):
- self.emergency_count = self.emergency_count + 1
- if (self.emergency_count >= emergency_min_count):
- self.emergency_stop = True
- else:
- self.emergency_count = 0
- self.emergency_stop = False
-
- def pull_cord_status_callback(self, data):
- self.pull_cord_status = data.data
-
- def send_vel(self, linear_vel, angular_vel):
- twist = Twist()
- twist.linear.x = linear_vel
- twist.linear.y = 0
- twist.linear.z = 0
- twist.angular.x = 0
- twist.angular.y = 0
- twist.angular.z = angular_vel
- self.cmd_vel_pub.publish(twist)
-
- def run(self):
-
- self.change_pwm(True)
-
- while self.pull_cord_status != True:
- self.send_vel(0, 0)
- time.sleep(0.3)
-
- start_time = rospy.get_rostime()
-
- # Roule pendant 3 secondes
- while rospy.get_rostime().secs - start_time.secs <= 3:
-
- if self.emergency_stop:
- self.send_vel(0, 0)
- else:
- self.send_vel(0.1, 0)
-
- time.sleep(0.1)
-
- self.send_vel(0, 0)
- time.sleep(0.2)
- self.change_pwm(False)
-
-
-if __name__ == '__main__':
- try:
- rospy.init_node("coordinator")
- CoordinatorNode()
- rospy.spin()
- except rospy.ROSInterruptException:
- pass
diff --git a/velocity_controller/CMakeLists.txt b/velocity_controller/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..0bf44cbbf372f5205912f096141c73d2c76be48c
--- /dev/null
+++ b/velocity_controller/CMakeLists.txt
@@ -0,0 +1,206 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(velocity_controller)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+ geometry_msgs
+ sensor_msgs
+ rplidar_ros
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+## * add a build_depend tag for "message_generation"
+## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+## but can be declared for certainty nonetheless:
+## * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+## * add "message_generation" and every package in MSG_DEP_SET to
+## find_package(catkin REQUIRED COMPONENTS ...)
+## * add "message_runtime" and every package in MSG_DEP_SET to
+## catkin_package(CATKIN_DEPENDS ...)
+## * uncomment the add_*_files sections below as needed
+## and list every .msg/.srv/.action file to be processed
+## * uncomment the generate_messages entry below
+## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+# FILES
+# Message1.msg
+# Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+# FILES
+# Service1.srv
+# Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+# FILES
+# Action1.action
+# Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+# DEPENDENCIES
+# geometry_msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+## * add "dynamic_reconfigure" to
+## find_package(catkin REQUIRED COMPONENTS ...)
+## * uncomment the "generate_dynamic_reconfigure_options" section below
+## and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+# cfg/DynReconf1.cfg
+# cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+# INCLUDE_DIRS include
+# LIBRARIES velocity_controller
+# CATKIN_DEPENDS geometry_msgs rplidar_ros
+# DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+ ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+# src/${PROJECT_NAME}/velocity_controller.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+# add_executable(${PROJECT_NAME}_node src/velocity_controller_node.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+# ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# catkin_install_python(PROGRAMS
+# scripts/my_python_script
+# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
+# install(TARGETS ${PROJECT_NAME}_node
+# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark libraries for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+# install(TARGETS ${PROJECT_NAME}
+# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+# FILES_MATCHING PATTERN "*.h"
+# PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+# # myfile1
+# # myfile2
+# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_velocity_controller.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)
diff --git a/velocity_controller/nodes/velocity_controller.py b/velocity_controller/nodes/velocity_controller.py
new file mode 100644
index 0000000000000000000000000000000000000000..1b6a3f01c0af4d233fff816cad2191a9540f09b5
--- /dev/null
+++ b/velocity_controller/nodes/velocity_controller.py
@@ -0,0 +1,71 @@
+#!/usr/bin/env python3
+
+import rospy
+from geometry_msgs.msg import Twist
+from sensor_msgs.msg import LaserScan
+from math import radians
+import rospy
+
+EMERGENCY_DISTANCE = 0.22 # in meters
+EMERGENCY_POINTS_COUNT_THRESHOLD = 2
+
+
+class VelocityController():
+
+ def __init__(self):
+ self.cmd_vel_pub = rospy.Publisher("/cmd_vel", Twist, queue_size=5)
+ self.am_cmd_vel_sub = rospy.Subscriber(
+ "/actions_manager/cmd_vel", Twist, self.am_cmd_vel_cb)
+ self.scan_sub = rospy.Subscriber("/scan", LaserScan, self.scan_cb)
+ self.scan = None
+
+ def scan_cb(self, scan):
+ self.scan = scan
+
+ def am_cmd_vel_cb(self, cmd_vel):
+ """
+ Receives the cmd_vel from the Action Manager and zeroes it if something is detecting with the LiDAR
+ """
+ linear_speed = cmd_vel.linear.x
+ angular_speed = cmd_vel.angular.z
+
+ 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(-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 counter >= EMERGENCY_POINTS_COUNT_THRESHOLD:
+ linear_speed_cmd = 0
+ angular_speed_cmd = 0
+
+ twist = Twist()
+ twist.linear.x = linear_speed_cmd
+ twist.angular.z = angular_speed_cmd
+ self.cmd_vel_pub.publish(twist)
+
+
+if __name__ == '__main__':
+
+ rospy.init_node("velocity_controller")
+
+ try:
+ VelocityController()
+ rospy.spin()
+ except rospy.ROSInterruptException:
+ pass
diff --git a/velocity_controller/package.xml b/velocity_controller/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..a04320af95757e55d7e7114c61596d58337237c1
--- /dev/null
+++ b/velocity_controller/package.xml
@@ -0,0 +1,68 @@
+<?xml version="1.0"?>
+<package format="2">
+ <name>velocity_controller</name>
+ <version>0.1.0</version>
+ <description>The velocity_controller package</description>
+
+ <!-- One maintainer tag required, multiple allowed, one person per tag -->
+ <!-- Example: -->
+ <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+ <maintainer email="charles@todo.todo">charles</maintainer>
+
+
+ <!-- One license tag required, multiple allowed, one license per tag -->
+ <!-- Commonly used license strings: -->
+ <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+ <license>GPLv3</license>
+
+
+ <!-- Url tags are optional, but multiple are allowed, one per tag -->
+ <!-- Optional attribute type can be: website, bugtracker, or repository -->
+ <!-- Example: -->
+ <!-- <url type="website">http://wiki.ros.org/velocity_controller</url> -->
+
+
+ <!-- Author tags are optional, multiple are allowed, one per tag -->
+ <!-- Authors do not have to be maintainers, but could be -->
+ <!-- Example: -->
+ <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+ <author >Charles JAVERLIAT</author>
+
+
+ <!-- The *depend tags are used to specify dependencies -->
+ <!-- Dependencies can be catkin packages or system dependencies -->
+ <!-- Examples: -->
+ <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+ <!-- <depend>roscpp</depend> -->
+ <!-- Note that this is equivalent to the following: -->
+ <!-- <build_depend>roscpp</build_depend> -->
+ <!-- <exec_depend>roscpp</exec_depend> -->
+ <!-- Use build_depend for packages you need at compile time: -->
+ <!-- <build_depend>message_generation</build_depend> -->
+ <!-- Use build_export_depend for packages you need in order to build against this package: -->
+ <!-- <build_export_depend>message_generation</build_export_depend> -->
+ <!-- Use buildtool_depend for build tool packages: -->
+ <!-- <buildtool_depend>catkin</buildtool_depend> -->
+ <!-- Use exec_depend for packages you need at runtime: -->
+ <!-- <exec_depend>message_runtime</exec_depend> -->
+ <!-- Use test_depend for packages you need only for testing: -->
+ <!-- <test_depend>gtest</test_depend> -->
+ <!-- Use doc_depend for packages you need only for building documentation: -->
+ <!-- <doc_depend>doxygen</doc_depend> -->
+ <buildtool_depend>catkin</buildtool_depend>
+ <build_depend>geometry_msgs</build_depend>
+ <build_depend>rplidar_ros</build_depend>
+ <build_depend>sensor_msgs</build_depend>
+ <build_export_depend>geometry_msgs</build_export_depend>
+ <build_export_depend>rplidar_ros</build_export_depend>
+ <exec_depend>geometry_msgs</exec_depend>
+ <exec_depend>rplidar_ros</exec_depend>
+ <exec_depend>sensor_msgs</exec_depend>
+
+
+ <!-- The export tag contains other, unspecified, tags -->
+ <export>
+ <!-- Other tools can request additional information be placed here -->
+
+ </export>
+</package>