├── crazychoir ├── crazychoir │ ├── __init__.py │ ├── gui │ │ ├── __init__.py │ │ ├── gui │ │ │ ├── __init__.py │ │ │ ├── res │ │ │ │ └── icons │ │ │ │ │ ├── icons8-save-64.png │ │ │ │ │ ├── icons8-clear-58.png │ │ │ │ │ ├── icons8-delete-64.png │ │ │ │ │ ├── icons8-directory-100.png │ │ │ │ │ └── icons8-rimuovere-100.png │ │ │ ├── custom_dialogs.py │ │ │ ├── rosWidget.py │ │ │ └── splineWidget.py │ │ ├── controller │ │ │ ├── __init__.py │ │ │ ├── utils.py │ │ │ └── ros.py │ │ ├── requirements.txt │ │ └── settings.yaml │ ├── integrator │ │ ├── __init__.py │ │ └── crazyflie_integrator.py │ ├── filtering │ │ ├── __init__.py │ │ ├── single_stage_predictor.py │ │ └── estimator.py │ ├── utils │ │ ├── images │ │ │ └── casy_logo.png │ │ ├── __init__.py │ │ └── frequency_plotter.py │ ├── crazyflie_communication │ │ ├── __init__.py │ │ ├── sender_strategy.py │ │ ├── vel_sender.py │ │ ├── fpqr_sender.py │ │ └── frpy_sender.py │ ├── planner │ │ ├── trajectory_handler │ │ │ ├── __init__.py │ │ │ └── trajectory_handler.py │ │ ├── __init__.py │ │ ├── trajectory_strategy.py │ │ ├── position_reference.py │ │ ├── acceleration_reference.py │ │ ├── fullstate_reference.py │ │ └── planner.py │ ├── guidance │ │ ├── __init__.py │ │ ├── distributed_control.py │ │ ├── goto_guidance.py │ │ ├── simple_guidance.py │ │ └── aggregative.py │ ├── radio_handler │ │ ├── __init__.py │ │ ├── radio_handler_vel_lighthouse.py │ │ ├── radio_handler_xyz_lighthouse.py │ │ ├── radio_handler_fpqr.py │ │ ├── radio_handler_frpy.py │ │ ├── radio_handler_xyz.py │ │ └── radio_handler_vel.py │ ├── webots_plugin │ │ ├── __init__.py │ │ ├── real_pose_ctrl.py │ │ ├── motor_ctrl_vel.py │ │ └── motor_ctrl_fpqr.py │ └── controller │ │ ├── __init__.py │ │ ├── geometric_attitude_ctrl.py │ │ └── velocity_ctrl.py ├── resource │ └── crazychoir ├── setup.cfg ├── package.xml ├── setup.py └── test │ ├── test_copyright.py │ ├── test_pep257.py │ └── test_flake8.py ├── crazychoir_examples ├── resource │ ├── crazychoir_examples │ ├── crazyflie_fpqr.urdf │ ├── crazyflie_vel.urdf │ ├── crazyflie_xyz.urdf │ ├── real_pose_vicon.urdf │ ├── real_pose_lighthouse.urdf │ └── rvizconf.rviz ├── crazychoir_examples │ ├── __init__.py │ ├── goto_webots │ │ ├── __init__.py │ │ ├── trajectory.py │ │ ├── guidance.py │ │ ├── controller.py │ │ └── gui.py │ ├── collision_webots │ │ ├── __init__.py │ │ ├── closest_robot_getter.py │ │ ├── trajectory.py │ │ ├── guidance.py │ │ ├── trajectory_vel.py │ │ ├── gui.py │ │ ├── controller_vel.py │ │ ├── collision.py │ │ └── controller.py │ ├── formation_rviz │ │ ├── __init__.py │ │ ├── rviz.py │ │ ├── integrator.py │ │ ├── guidance.py │ │ └── controller.py │ ├── formation_vicon │ │ ├── __init__.py │ │ ├── radio.py │ │ ├── guidance.py │ │ ├── trajectory.py │ │ ├── gui.py │ │ └── controller.py │ ├── formation_webots │ │ ├── __init__.py │ │ ├── trajectory.py │ │ ├── guidance.py │ │ ├── controller_leaders.py │ │ ├── controller_followers.py │ │ └── gui.py │ ├── tracking_vicon │ │ ├── __init__.py │ │ ├── radio.py │ │ ├── guidance.py │ │ ├── trajectory.py │ │ ├── gui.py │ │ └── controller.py │ ├── tracking_webots │ │ ├── __init__.py │ │ ├── guidance.py │ │ ├── trajectory.py │ │ ├── controller.py │ │ └── gui.py │ ├── task_assignment_vicon │ │ ├── __init__.py │ │ ├── radio.py │ │ ├── simple_guidance.py │ │ ├── planner.py │ │ ├── guidance.py │ │ ├── gui.py │ │ └── table.py │ ├── task_assignment_webots │ │ ├── __init__.py │ │ ├── simple_guidance.py │ │ ├── planner.py │ │ ├── guidance.py │ │ ├── gui.py │ │ └── table.py │ ├── aggregative_lighthouse │ │ ├── radio.py │ │ ├── guidance_intruder.py │ │ ├── trajectory.py │ │ ├── simple_guidance.py │ │ ├── gui.py │ │ ├── controller.py │ │ ├── guidance_aggregative.py │ │ └── guidance_aggregative_moving.py │ └── aggregative_webots │ │ ├── guidance_intruder.py │ │ ├── trajectory.py │ │ ├── simple_guidance.py │ │ ├── gui.py │ │ ├── controller.py │ │ ├── guidance_aggregative.py │ │ └── guidance_aggregative_moving.py ├── worlds │ ├── meshes │ │ ├── 4_motors.stl │ │ ├── battery.stl │ │ ├── ccw_prop.stl │ │ ├── cf_body.stl │ │ ├── cw_prop.stl │ │ ├── 2_pinheaders.stl │ │ ├── 4_motormounts.stl │ │ ├── cf2_assembly.stl │ │ └── battery_holder.stl │ ├── obj_sphere.wbt │ ├── obj_target.wbt │ ├── obj_real_pose.wbt │ ├── empty_world.wbt │ ├── world_generator.py │ ├── obj_crazyflie.wbt │ └── obj_crazyflie_red.wbt ├── setup.cfg ├── package.xml ├── test │ ├── test_flake8.py │ ├── test_copyright.py │ └── test_pep257.py ├── setup.py └── launch │ ├── tracking_vicon.launch.py │ ├── formation_rviz.launch.py │ ├── tracking_webots.launch.py │ ├── task_assignment_vicon.launch.py │ └── aggregative_lighthouse.launch.py ├── crazychoir_interfaces ├── crazychoir_interfaces │ └── __init__.py ├── msg │ └── FullState.msg ├── setup.py ├── CMakeLists.txt └── package.xml ├── maeci_logo.png ├── docker ├── docker_img.png ├── requirements.txt ├── createContainer ├── buildImage └── final_setup.sh ├── requirements.txt ├── .gitmodules ├── .gitignore └── README.md /crazychoir/crazychoir/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /crazychoir/resource/crazychoir: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /crazychoir/crazychoir/gui/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /crazychoir_examples/resource/crazychoir_examples: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /crazychoir_examples/crazychoir_examples/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /crazychoir_interfaces/crazychoir_interfaces/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /crazychoir_examples/crazychoir_examples/goto_webots/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /crazychoir_examples/crazychoir_examples/collision_webots/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /crazychoir_examples/crazychoir_examples/formation_rviz/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /crazychoir_examples/crazychoir_examples/formation_vicon/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /crazychoir_examples/crazychoir_examples/formation_webots/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /crazychoir_examples/crazychoir_examples/tracking_vicon/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /crazychoir_examples/crazychoir_examples/tracking_webots/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /crazychoir_examples/crazychoir_examples/task_assignment_vicon/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /crazychoir/crazychoir/gui/gui/__init__.py: -------------------------------------------------------------------------------- 1 | from .mainWindow import MainWindow -------------------------------------------------------------------------------- /crazychoir_examples/crazychoir_examples/task_assignment_webots/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /maeci_logo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OPT4SMART/crazychoir/HEAD/maeci_logo.png -------------------------------------------------------------------------------- /crazychoir/crazychoir/integrator/__init__.py: -------------------------------------------------------------------------------- 1 | from .crazyflie_integrator import CrazyflieIntegrator -------------------------------------------------------------------------------- /docker/docker_img.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OPT4SMART/crazychoir/HEAD/docker/docker_img.png -------------------------------------------------------------------------------- /crazychoir/crazychoir/gui/controller/__init__.py: -------------------------------------------------------------------------------- 1 | from .settings import SettingsParser 2 | from .ros import ControlPanel -------------------------------------------------------------------------------- /crazychoir_interfaces/msg/FullState.msg: -------------------------------------------------------------------------------- 1 | geometry_msgs/Pose pose 2 | geometry_msgs/Twist vel 3 | geometry_msgs/Accel acc -------------------------------------------------------------------------------- /crazychoir/setup.cfg: -------------------------------------------------------------------------------- 1 | [develop] 2 | script_dir=$base/lib/crazychoir 3 | [install] 4 | install_scripts=$base/lib/crazychoir 5 | -------------------------------------------------------------------------------- /crazychoir/crazychoir/filtering/__init__.py: -------------------------------------------------------------------------------- 1 | from .estimator import Estimator 2 | from .single_stage_predictor import SingleStagePredictor -------------------------------------------------------------------------------- /crazychoir_examples/worlds/meshes/4_motors.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OPT4SMART/crazychoir/HEAD/crazychoir_examples/worlds/meshes/4_motors.stl -------------------------------------------------------------------------------- /crazychoir_examples/worlds/meshes/battery.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OPT4SMART/crazychoir/HEAD/crazychoir_examples/worlds/meshes/battery.stl -------------------------------------------------------------------------------- /crazychoir_examples/worlds/meshes/ccw_prop.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OPT4SMART/crazychoir/HEAD/crazychoir_examples/worlds/meshes/ccw_prop.stl -------------------------------------------------------------------------------- /crazychoir_examples/worlds/meshes/cf_body.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OPT4SMART/crazychoir/HEAD/crazychoir_examples/worlds/meshes/cf_body.stl -------------------------------------------------------------------------------- /crazychoir_examples/worlds/meshes/cw_prop.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OPT4SMART/crazychoir/HEAD/crazychoir_examples/worlds/meshes/cw_prop.stl -------------------------------------------------------------------------------- /crazychoir/crazychoir/gui/requirements.txt: -------------------------------------------------------------------------------- 1 | PyQt5~=5.15.7 2 | matplotlib~=3.6.1 3 | numpy~=1.23.3 4 | scipy~=1.10.0 5 | PyYAML~=6.0 6 | qt-material 7 | -------------------------------------------------------------------------------- /crazychoir/crazychoir/utils/images/casy_logo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OPT4SMART/crazychoir/HEAD/crazychoir/crazychoir/utils/images/casy_logo.png -------------------------------------------------------------------------------- /crazychoir_examples/setup.cfg: -------------------------------------------------------------------------------- 1 | [develop] 2 | script_dir=$base/lib/crazychoir_examples 3 | [install] 4 | install_scripts=$base/lib/crazychoir_examples 5 | -------------------------------------------------------------------------------- /crazychoir_examples/worlds/meshes/2_pinheaders.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OPT4SMART/crazychoir/HEAD/crazychoir_examples/worlds/meshes/2_pinheaders.stl -------------------------------------------------------------------------------- /crazychoir_examples/worlds/meshes/4_motormounts.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OPT4SMART/crazychoir/HEAD/crazychoir_examples/worlds/meshes/4_motormounts.stl -------------------------------------------------------------------------------- /crazychoir_examples/worlds/meshes/cf2_assembly.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OPT4SMART/crazychoir/HEAD/crazychoir_examples/worlds/meshes/cf2_assembly.stl -------------------------------------------------------------------------------- /crazychoir_examples/worlds/meshes/battery_holder.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OPT4SMART/crazychoir/HEAD/crazychoir_examples/worlds/meshes/battery_holder.stl -------------------------------------------------------------------------------- /crazychoir/crazychoir/gui/gui/res/icons/icons8-save-64.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OPT4SMART/crazychoir/HEAD/crazychoir/crazychoir/gui/gui/res/icons/icons8-save-64.png -------------------------------------------------------------------------------- /crazychoir/crazychoir/gui/gui/res/icons/icons8-clear-58.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OPT4SMART/crazychoir/HEAD/crazychoir/crazychoir/gui/gui/res/icons/icons8-clear-58.png -------------------------------------------------------------------------------- /crazychoir/crazychoir/gui/gui/res/icons/icons8-delete-64.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OPT4SMART/crazychoir/HEAD/crazychoir/crazychoir/gui/gui/res/icons/icons8-delete-64.png -------------------------------------------------------------------------------- /crazychoir/crazychoir/gui/gui/res/icons/icons8-directory-100.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OPT4SMART/crazychoir/HEAD/crazychoir/crazychoir/gui/gui/res/icons/icons8-directory-100.png -------------------------------------------------------------------------------- /crazychoir/crazychoir/gui/gui/res/icons/icons8-rimuovere-100.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OPT4SMART/crazychoir/HEAD/crazychoir/crazychoir/gui/gui/res/icons/icons8-rimuovere-100.png -------------------------------------------------------------------------------- /crazychoir/crazychoir/gui/settings.yaml: -------------------------------------------------------------------------------- 1 | area_settings: 2 | paint_square_size: 3 | - 2 4 | - 2 5 | data_storage: 6 | polynomials_directory: saves/poly 7 | trajectory_directory: saves/trajectory 8 | -------------------------------------------------------------------------------- /crazychoir/crazychoir/crazyflie_communication/__init__.py: -------------------------------------------------------------------------------- 1 | from .sender_strategy import SenderStrategy 2 | from .fpqr_sender import FPQRSender 3 | from .frpy_sender import FRPYSender 4 | from .vel_sender import VelSender -------------------------------------------------------------------------------- /crazychoir_examples/resource/crazyflie_fpqr.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /crazychoir_examples/resource/crazyflie_vel.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /crazychoir_examples/resource/crazyflie_xyz.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /crazychoir_examples/resource/real_pose_vicon.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /crazychoir_examples/resource/real_pose_lighthouse.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /crazychoir/crazychoir/planner/trajectory_handler/__init__.py: -------------------------------------------------------------------------------- 1 | from .trajectory_handler import TrajectoryHandler, AccelerationTrajHandler, FullStateTrajHandler 2 | from .polynomial_7 import Polynomial7th 3 | from .spline import Spline, SplinePosition -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | numpy~=1.23.3 2 | scipy~=1.10.0 3 | dill>=0.3.2 4 | recordclass>=0.14.3 5 | autograd>=1.3 6 | cvxopt>=1.2.3 7 | cvxpy>=1.0.25 8 | osqp>=0.6.1 9 | pyqt5==5.12.0 10 | matplotlib~=3.6.1 11 | cflib 12 | PyYAML~=6.0 13 | qt-material 14 | -------------------------------------------------------------------------------- /crazychoir/crazychoir/crazyflie_communication/sender_strategy.py: -------------------------------------------------------------------------------- 1 | class SenderStrategy: 2 | def __init__(self, send_freq): 3 | self.send_freq = send_freq 4 | 5 | def command_sender(self, thrust, desired_attitude, pqr): 6 | raise NotImplementedError -------------------------------------------------------------------------------- /crazychoir_examples/crazychoir_examples/tracking_vicon/radio.py: -------------------------------------------------------------------------------- 1 | import rclpy 2 | from crazychoir.radio_handler import RadioHandlerFPQR 3 | 4 | def main(): 5 | rclpy.init() 6 | 7 | radio = RadioHandlerFPQR() 8 | 9 | rclpy.spin(radio) 10 | rclpy.shutdown() 11 | -------------------------------------------------------------------------------- /crazychoir/crazychoir/planner/__init__.py: -------------------------------------------------------------------------------- 1 | from .position_reference import PositionTraj 2 | from .acceleration_reference import AccelerationTraj 3 | from .fullstate_reference import FullStateTraj 4 | from .planner import PointToPointPlanner_2D 5 | from .trajectory_strategy import TrajHandlerStrategy -------------------------------------------------------------------------------- /docker/requirements.txt: -------------------------------------------------------------------------------- 1 | numpy~=1.23.3 2 | scipy>=1.2.1 3 | dill>=0.3.2 4 | recordclass>=0.14.3 5 | autograd>=1.3 6 | cvxopt>=1.2.3 7 | cvxpy>=1.0.25 8 | networkx 9 | osqp>=0.6.1 10 | black 11 | pre-commit 12 | pytest-cov 13 | matplotlib~=3.6.1 14 | qt-material 15 | # pyqt5==5.12.0 16 | # PyYAML~=6.0 17 | cflib 18 | -------------------------------------------------------------------------------- /crazychoir/crazychoir/crazyflie_communication/vel_sender.py: -------------------------------------------------------------------------------- 1 | from .sender_strategy import SenderStrategy 2 | import numpy as np 3 | 4 | class VelSender(SenderStrategy): 5 | def __init__(self, send_freq): 6 | super().__init__(send_freq) 7 | 8 | def command_sender(self, vel_input): 9 | return vel_input -------------------------------------------------------------------------------- /crazychoir_examples/crazychoir_examples/formation_rviz/rviz.py: -------------------------------------------------------------------------------- 1 | import rclpy 2 | from choirbot.utils import SimpleVisualizer 3 | 4 | 5 | def main(): 6 | rclpy.init() 7 | 8 | visualizer = SimpleVisualizer(pose_handler='pubsub', pose_topic='odom') 9 | 10 | rclpy.spin(visualizer) 11 | rclpy.shutdown() 12 | -------------------------------------------------------------------------------- /crazychoir_examples/crazychoir_examples/task_assignment_vicon/radio.py: -------------------------------------------------------------------------------- 1 | import rclpy 2 | from rclpy.node import Node 3 | from crazychoir.radio_handler import RadioHandlerXYZ 4 | 5 | 6 | def main(): 7 | rclpy.init() 8 | 9 | radio = RadioHandlerXYZ() 10 | 11 | rclpy.spin(radio) 12 | rclpy.shutdown() 13 | -------------------------------------------------------------------------------- /crazychoir/crazychoir/utils/__init__.py: -------------------------------------------------------------------------------- 1 | # Change this path to your crazyflie-firmware folder 2 | import sys 3 | sys.path.append('/home/opt4smart/crazyflie-firmware/build') 4 | 5 | try: 6 | import cffirmware 7 | except ImportError: 8 | raise ImportError('Set your crazyflie-firmware folder path in crazychoir/utils/__init__.py') 9 | -------------------------------------------------------------------------------- /crazychoir_examples/crazychoir_examples/formation_vicon/radio.py: -------------------------------------------------------------------------------- 1 | import rclpy 2 | from rclpy.node import Node 3 | from crazychoir.radio_handler import RadioHandlerFPQR, RadioHandlerFRPY 4 | 5 | 6 | def main(): 7 | rclpy.init() 8 | 9 | radio = RadioHandlerFPQR() 10 | 11 | rclpy.spin(radio) 12 | rclpy.shutdown() 13 | -------------------------------------------------------------------------------- /crazychoir_examples/crazychoir_examples/aggregative_lighthouse/radio.py: -------------------------------------------------------------------------------- 1 | import rclpy 2 | from rclpy.node import Node 3 | from crazychoir.radio_handler import RadioHandlerVelLightHouse 4 | 5 | 6 | def main(): 7 | rclpy.init() 8 | 9 | radio = RadioHandlerVelLightHouse() 10 | 11 | rclpy.spin(radio) 12 | rclpy.shutdown() 13 | -------------------------------------------------------------------------------- /crazychoir_examples/crazychoir_examples/formation_rviz/integrator.py: -------------------------------------------------------------------------------- 1 | import rclpy 2 | from crazychoir.integrator import CrazyflieIntegrator 3 | 4 | 5 | def main(): 6 | rclpy.init() 7 | 8 | frequency = 100 9 | 10 | integrator = CrazyflieIntegrator(frequency) 11 | 12 | rclpy.spin(integrator) 13 | rclpy.shutdown() 14 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "ChoiRbot"] 2 | path = ChoiRbot 3 | url = https://github.com/OPT4SMART/ChoiRbot.git 4 | [submodule "ros2-vicon-receiver"] 5 | path = ros2-vicon-receiver 6 | url = https://github.com/OPT4SMART/ros2-vicon-receiver.git 7 | [submodule "webots_ros2"] 8 | path = webots_ros2 9 | url = https://github.com/cyberbotics/webots_ros2.git 10 | -------------------------------------------------------------------------------- /crazychoir/crazychoir/guidance/__init__.py: -------------------------------------------------------------------------------- 1 | from .bearing_formation import BearingFormation 2 | from .distributed_control import DistributedControl 3 | from .simple_guidance import SimpleGuidance 4 | from .aggregative import AggregativeGuidanceCrazyflie 5 | from .aggregative import AggregativeGuidanceCrazyflieMovingIntruder 6 | from .goto_guidance import GoToGuidance 7 | 8 | -------------------------------------------------------------------------------- /crazychoir/crazychoir/crazyflie_communication/fpqr_sender.py: -------------------------------------------------------------------------------- 1 | from .sender_strategy import SenderStrategy 2 | import numpy as np 3 | 4 | class FPQRSender(SenderStrategy): 5 | def __init__(self, send_freq): 6 | super().__init__(send_freq) 7 | 8 | def command_sender(self, thrust, desired_attitude, pqr): 9 | u = np.append(thrust, pqr) 10 | return u -------------------------------------------------------------------------------- /crazychoir/crazychoir/planner/trajectory_strategy.py: -------------------------------------------------------------------------------- 1 | class TrajHandlerStrategy: 2 | def __init__(self, msg_type, topic): 3 | self.msg_type = msg_type 4 | self.topic = topic 5 | self.des_ref = {} 6 | 7 | def handler_func(self, msg): 8 | raise NotImplementedError 9 | 10 | def get_desired_reference(self): 11 | return self.des_ref -------------------------------------------------------------------------------- /crazychoir_examples/crazychoir_examples/task_assignment_webots/simple_guidance.py: -------------------------------------------------------------------------------- 1 | import rclpy 2 | from crazychoir.guidance import SimpleGuidance 3 | 4 | def main(): 5 | rclpy.init() 6 | 7 | guidance = SimpleGuidance(pose_handler='pubsub', pose_topic='odom') 8 | 9 | guidance.get_logger().info('Go!') 10 | 11 | rclpy.spin(guidance) 12 | rclpy.shutdown() 13 | -------------------------------------------------------------------------------- /crazychoir_examples/crazychoir_examples/collision_webots/closest_robot_getter.py: -------------------------------------------------------------------------------- 1 | import rclpy 2 | from choirbot.collision import ClosestRobotGetter 3 | 4 | 5 | def main(): 6 | rclpy.init() 7 | 8 | sensing_distance = 2.0 9 | closest_robot_getter = ClosestRobotGetter(sensing_distance=sensing_distance) 10 | 11 | rclpy.spin(closest_robot_getter) 12 | rclpy.shutdown() 13 | -------------------------------------------------------------------------------- /crazychoir_examples/crazychoir_examples/tracking_webots/guidance.py: -------------------------------------------------------------------------------- 1 | import rclpy 2 | from crazychoir.guidance import SimpleGuidance 3 | import time 4 | 5 | 6 | def main(): 7 | rclpy.init() 8 | 9 | frequency = 100 10 | 11 | guidance = SimpleGuidance(pose_handler='pubsub', pose_topic='odom') 12 | 13 | guidance.get_logger().info('Go!') 14 | 15 | rclpy.spin(guidance) 16 | rclpy.shutdown() 17 | -------------------------------------------------------------------------------- /crazychoir/crazychoir/radio_handler/__init__.py: -------------------------------------------------------------------------------- 1 | from .radio_handler_frpy import RadioHandlerFRPY 2 | from .radio_handler_fpqr import RadioHandlerFPQR 3 | from .radio_handler_xyz import RadioHandlerXYZ 4 | from .radio_handler import RadioHandler 5 | from .radio_handler_vel import RadioHandlerVel 6 | from .radio_handler_vel_lighthouse import RadioHandlerVelLightHouse 7 | from .radio_handler_xyz_lighthouse import RadioHandlerXYZLightHouse 8 | 9 | -------------------------------------------------------------------------------- /crazychoir/crazychoir/planner/position_reference.py: -------------------------------------------------------------------------------- 1 | from geometry_msgs.msg import Vector3 2 | import numpy as np 3 | from .trajectory_strategy import TrajHandlerStrategy 4 | 5 | class PositionTraj(TrajHandlerStrategy): 6 | def __init__(self, msg_type=Vector3, topic='position'): 7 | super().__init__(msg_type, topic) 8 | 9 | def handler_func(self, msg): 10 | self.des_ref["position"] = np.array([msg.x, msg.y, msg.z]) -------------------------------------------------------------------------------- /crazychoir_examples/crazychoir_examples/collision_webots/trajectory.py: -------------------------------------------------------------------------------- 1 | import rclpy 2 | from crazychoir.planner.trajectory_handler import Spline 3 | 4 | def main(): 5 | rclpy.init() 6 | 7 | frequency = 100 8 | 9 | trajectory = Spline(update_frequency = frequency, pose_handler = 'pubsub', pose_topic = 'odom') 10 | 11 | trajectory.get_logger().info('Go!') 12 | 13 | rclpy.spin(trajectory) 14 | rclpy.shutdown() 15 | -------------------------------------------------------------------------------- /crazychoir_examples/crazychoir_examples/formation_webots/trajectory.py: -------------------------------------------------------------------------------- 1 | import rclpy 2 | from crazychoir.planner.trajectory_handler import Spline 3 | 4 | def main(): 5 | rclpy.init() 6 | 7 | frequency = 100 8 | 9 | trajectory = Spline(update_frequency = frequency, pose_handler = 'pubsub', pose_topic = 'odom') 10 | 11 | trajectory.get_logger().info('Go!') 12 | 13 | rclpy.spin(trajectory) 14 | rclpy.shutdown() 15 | -------------------------------------------------------------------------------- /crazychoir_examples/crazychoir_examples/tracking_webots/trajectory.py: -------------------------------------------------------------------------------- 1 | import rclpy 2 | from crazychoir.planner.trajectory_handler import Spline 3 | 4 | def main(): 5 | rclpy.init() 6 | 7 | frequency = 100 8 | 9 | trajectory = Spline(update_frequency = frequency, pose_handler = 'pubsub', pose_topic = 'odom') 10 | 11 | trajectory.get_logger().info('Go!') 12 | 13 | rclpy.spin(trajectory) 14 | rclpy.shutdown() 15 | -------------------------------------------------------------------------------- /crazychoir_examples/worlds/obj_sphere.wbt: -------------------------------------------------------------------------------- 1 | Robot { 2 | name "$NAME" 3 | translation $X $Y $Z 4 | children [ 5 | Solid { 6 | children [ 7 | Shape { 8 | appearance PBRAppearance { 9 | baseColor 1 0 0 10 | transparency 0.4 11 | metalness 0 12 | } 13 | geometry Sphere { 14 | radius 0.1 15 | } 16 | } 17 | ] 18 | } 19 | ] 20 | } -------------------------------------------------------------------------------- /crazychoir_examples/worlds/obj_target.wbt: -------------------------------------------------------------------------------- 1 | Robot { 2 | name "$NAME" 3 | translation $X $Y $Z 4 | children [ 5 | Solid { 6 | children [ 7 | Shape { 8 | appearance PBRAppearance { 9 | baseColor 0 0 1 10 | transparency 0.1 11 | metalness 0 12 | } 13 | geometry Sphere { 14 | radius 0.1 15 | } 16 | } 17 | ] 18 | } 19 | ] 20 | } -------------------------------------------------------------------------------- /crazychoir_examples/crazychoir_examples/goto_webots/trajectory.py: -------------------------------------------------------------------------------- 1 | import rclpy 2 | from crazychoir.planner.trajectory_handler import SplinePosition 3 | 4 | def main(): 5 | rclpy.init() 6 | 7 | frequency = 10 8 | 9 | trajectory = SplinePosition(update_frequency = frequency, pose_handler = 'pubsub', pose_topic = 'odom') 10 | 11 | trajectory.get_logger().info('Go!') 12 | 13 | rclpy.spin(trajectory) 14 | rclpy.shutdown() 15 | -------------------------------------------------------------------------------- /crazychoir/crazychoir/planner/acceleration_reference.py: -------------------------------------------------------------------------------- 1 | from geometry_msgs.msg import Vector3 2 | import numpy as np 3 | from .trajectory_strategy import TrajHandlerStrategy 4 | 5 | class AccelerationTraj(TrajHandlerStrategy): 6 | def __init__(self, msg_type=Vector3, topic='acceleration'): 7 | super().__init__(msg_type, topic) 8 | 9 | def handler_func(self, msg): 10 | self.des_ref["acceleration"] = np.array([msg.x, msg.y, msg.z]) 11 | -------------------------------------------------------------------------------- /crazychoir_examples/crazychoir_examples/goto_webots/guidance.py: -------------------------------------------------------------------------------- 1 | import rclpy 2 | from crazychoir.guidance import GoToGuidance 3 | import time 4 | 5 | 6 | def main(): 7 | rclpy.init() 8 | 9 | take_ff_time = 5.0 10 | 11 | guidance = GoToGuidance(pose_handler='pubsub', pose_topic='odom', pose_callback=None, takeoff_time=take_ff_time) 12 | 13 | guidance.get_logger().info('Go!') 14 | 15 | rclpy.spin(guidance) 16 | rclpy.shutdown() 17 | -------------------------------------------------------------------------------- /crazychoir/crazychoir/webots_plugin/__init__.py: -------------------------------------------------------------------------------- 1 | """ 2 | The .urdf file of the Crazyflie needs the Module 'MotorCtrl*' to link the 3 | motor controller plugin to the webots model. 4 | """ 5 | 6 | from .motor_ctrl_fpqr import MotorCtrlFPQR 7 | from .motor_ctrl_xyz import MotorCtrlXYZ 8 | from .motor_ctrl_vel import MotorCtrlVel 9 | from .real_pose_ctrl import RealPoseCtrl 10 | from .real_pose_ctrl import RealPoseCtrlLighthouse 11 | from .real_pose_ctrl import RealPoseCtrlVicon -------------------------------------------------------------------------------- /crazychoir_examples/crazychoir_examples/formation_webots/guidance.py: -------------------------------------------------------------------------------- 1 | import rclpy 2 | from rclpy.node import Node 3 | from crazychoir.guidance import BearingFormation 4 | import time 5 | 6 | 7 | def main(): 8 | rclpy.init() 9 | 10 | frequency = 100 11 | 12 | guidance = BearingFormation(update_frequency=frequency, pose_handler='pubsub', pose_topic='odom') 13 | 14 | guidance.get_logger().info('Go!') 15 | 16 | rclpy.spin(guidance) 17 | rclpy.shutdown() 18 | -------------------------------------------------------------------------------- /crazychoir_examples/crazychoir_examples/collision_webots/guidance.py: -------------------------------------------------------------------------------- 1 | import rclpy 2 | from rclpy.node import Node 3 | from crazychoir.guidance import GoToGuidance 4 | import time 5 | 6 | 7 | def main(): 8 | rclpy.init() 9 | 10 | take_ff_time = 5.0 11 | 12 | guidance = GoToGuidance(pose_handler='pubsub', pose_topic='odom', pose_callback=None, takeoff_time=take_ff_time) 13 | 14 | guidance.get_logger().info('Go!') 15 | 16 | rclpy.spin(guidance) 17 | rclpy.shutdown() 18 | -------------------------------------------------------------------------------- /crazychoir_examples/worlds/obj_real_pose.wbt: -------------------------------------------------------------------------------- 1 | Robot { 2 | name "$NAME" 3 | translation $X $Y $Z 4 | controller "" 5 | supervisor TRUE 6 | children [ 7 | GPS { 8 | } 9 | Solid { 10 | children [ 11 | Shape { 12 | appearance PBRAppearance { 13 | baseColor 0 1 0 14 | metalness 0 15 | } 16 | geometry Sphere { 17 | radius 0.08 18 | } 19 | } 20 | ] 21 | } 22 | ] 23 | } -------------------------------------------------------------------------------- /docker/createContainer: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | CONTAINER_NAME=$1 4 | IMAGE_NAME=$2 5 | DEV_NAME=opt4smart 6 | WS_NAME=crazychoir_ws 7 | 8 | # Authorization 9 | xhost + 10 | 11 | # Run and create container 12 | docker run -it \ 13 | --privileged \ 14 | --gpus=all \ 15 | --network=host \ 16 | --shm-size=200mb \ 17 | --env="DISPLAY" \ 18 | --volume="/tmp/.X11-unix:/tmp/.X11-unix:rw" \ 19 | --volume=./$WS_NAME:/home/$DEV_NAME/$WS_NAME \ 20 | --name $CONTAINER_NAME \ 21 | $IMAGE_NAME 22 | -------------------------------------------------------------------------------- /crazychoir_examples/crazychoir_examples/aggregative_webots/guidance_intruder.py: -------------------------------------------------------------------------------- 1 | import rclpy 2 | from rclpy.node import Node 3 | from crazychoir.guidance import GoToGuidance 4 | import time 5 | 6 | 7 | def main(): 8 | rclpy.init() 9 | 10 | take_ff_time = 5.0 11 | 12 | guidance = GoToGuidance(pose_handler='pubsub', pose_topic='odom', pose_callback=None, takeoff_time=take_ff_time) 13 | 14 | guidance.get_logger().info('Go!') 15 | 16 | rclpy.spin(guidance) 17 | rclpy.shutdown() 18 | -------------------------------------------------------------------------------- /crazychoir_examples/crazychoir_examples/task_assignment_webots/planner.py: -------------------------------------------------------------------------------- 1 | import rclpy 2 | from crazychoir.planner import PointToPointPlanner_2D 3 | from rclpy.executors import MultiThreadedExecutor 4 | 5 | 6 | def main(): 7 | rclpy.init() 8 | 9 | goal_tolerance = 0.05 10 | 11 | planner = PointToPointPlanner_2D(goal_tolerance, pose_handler='pubsub', pose_topic='odom') 12 | 13 | executor = MultiThreadedExecutor() 14 | rclpy.spin(planner, executor) 15 | rclpy.shutdown() 16 | -------------------------------------------------------------------------------- /crazychoir/crazychoir/crazyflie_communication/frpy_sender.py: -------------------------------------------------------------------------------- 1 | from .sender_strategy import SenderStrategy 2 | import numpy as np 3 | from scipy.spatial.transform import Rotation as R 4 | 5 | class FRPYSender(SenderStrategy): 6 | def __init__(self, send_freq): 7 | super().__init__(send_freq) 8 | 9 | def command_sender(self, thrust, desired_attitude, pqr): 10 | rpy = R.from_matrix(desired_attitude).as_euler('xyz') 11 | u = np.append(thrust, rpy) 12 | return u 13 | 14 | -------------------------------------------------------------------------------- /crazychoir_examples/crazychoir_examples/aggregative_lighthouse/guidance_intruder.py: -------------------------------------------------------------------------------- 1 | import rclpy 2 | from rclpy.node import Node 3 | from crazychoir.guidance import GoToGuidance 4 | import time 5 | 6 | 7 | def main(): 8 | rclpy.init() 9 | 10 | takeoff_time = 5.0 11 | 12 | guidance = GoToGuidance(pose_handler='pubsub', pose_topic='cf_odom', pose_callback=None, takeoff_time=takeoff_time) 13 | 14 | guidance.get_logger().info('Go!') 15 | 16 | rclpy.spin(guidance) 17 | rclpy.shutdown() 18 | -------------------------------------------------------------------------------- /crazychoir_interfaces/setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import setup 2 | 3 | package_name = 'crazychoir_interfaces' 4 | 5 | setup( 6 | name=package_name, 7 | version='0.0.1', 8 | maintainer='OPT4SMART', 9 | maintainer_email='info@opt4smart.eu', 10 | description='Interfaces for crazychoir', 11 | license='GNU General Public License v3.0', 12 | packages=[package_name], 13 | data_files=[ 14 | ('share/' + package_name, ['package.xml']) 15 | ], 16 | install_requires=['setuptools'], 17 | zip_safe=True 18 | ) 19 | -------------------------------------------------------------------------------- /crazychoir_examples/crazychoir_examples/tracking_vicon/guidance.py: -------------------------------------------------------------------------------- 1 | import rclpy 2 | from crazychoir.guidance import SimpleGuidance 3 | from crazychoir.filtering import Estimator 4 | 5 | def main(): 6 | rclpy.init() 7 | 8 | frequency = 100 9 | 10 | callback = Estimator() 11 | 12 | guidance = SimpleGuidance(pose_handler='vicon', pose_topic='/vicon/cf1/cf1', pose_callback=callback) 13 | 14 | callback.set_pose(guidance.current_pose) 15 | 16 | guidance.get_logger().info('Go!') 17 | 18 | rclpy.spin(guidance) 19 | rclpy.shutdown() 20 | -------------------------------------------------------------------------------- /crazychoir_examples/crazychoir_examples/task_assignment_webots/guidance.py: -------------------------------------------------------------------------------- 1 | import rclpy 2 | from choirbot.guidance.task import TaskGuidance, PositionTaskExecutor 3 | from choirbot.optimizer import TaskOptimizer 4 | 5 | def main(): 6 | rclpy.init() 7 | 8 | # initialize task guidance 9 | opt_settings = {'max_iterations': 20} 10 | executor = PositionTaskExecutor() 11 | optimizer = TaskOptimizer(settings=opt_settings) 12 | 13 | guidance = TaskGuidance(optimizer, executor, None, 'pubsub', 'odom') 14 | 15 | rclpy.spin(guidance) 16 | rclpy.shutdown() 17 | -------------------------------------------------------------------------------- /crazychoir_examples/crazychoir_examples/formation_rviz/guidance.py: -------------------------------------------------------------------------------- 1 | import rclpy 2 | from choirbot.guidance.distributed_control import BearingFormationControlGuidance 3 | import time 4 | 5 | 6 | def main(): 7 | rclpy.init() 8 | 9 | frequency = 100 10 | prop_gain = 1.0 11 | deriv_gain = 5.0 12 | 13 | guidance = BearingFormationControlGuidance(frequency, prop_gain, deriv_gain, 'pubsub', 'odom') 14 | 15 | guidance.get_logger().info('Waiting for 5 seconds to let all nodes be ready') 16 | time.sleep(5) 17 | 18 | rclpy.spin(guidance) 19 | rclpy.shutdown() 20 | -------------------------------------------------------------------------------- /crazychoir_examples/crazychoir_examples/tracking_vicon/trajectory.py: -------------------------------------------------------------------------------- 1 | import rclpy 2 | from crazychoir.planner.trajectory_handler import Spline 3 | from crazychoir.filtering import Estimator 4 | 5 | 6 | def main(): 7 | rclpy.init() 8 | 9 | frequency = 100 10 | callback = Estimator() 11 | 12 | trajectory = Spline(update_frequency = frequency, pose_handler='vicon', pose_topic='/vicon/cf1/cf1', pose_callback=callback) 13 | 14 | callback.set_pose(trajectory.current_pose) 15 | 16 | trajectory.get_logger().info('Go!') 17 | 18 | rclpy.spin(trajectory) 19 | rclpy.shutdown() 20 | -------------------------------------------------------------------------------- /crazychoir_interfaces/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(crazychoir_interfaces) 3 | 4 | # load requires packages 5 | find_package(ament_cmake_python REQUIRED) 6 | find_package(rosidl_default_generators REQUIRED) 7 | find_package(geometry_msgs) 8 | 9 | # generate C++ and Python classes 10 | rosidl_generate_interfaces(${PROJECT_NAME} 11 | "msg/FullState.msg" 12 | DEPENDENCIES geometry_msgs 13 | ) 14 | 15 | # Install the python module for this package 16 | ament_python_install_package(${PROJECT_NAME}) 17 | 18 | ament_export_dependencies(ament_cmake_python) 19 | 20 | ament_package() 21 | -------------------------------------------------------------------------------- /crazychoir_examples/crazychoir_examples/aggregative_webots/trajectory.py: -------------------------------------------------------------------------------- 1 | import rclpy 2 | from crazychoir.planner.trajectory_handler import Spline, SplinePosition 3 | from rclpy.node import Node 4 | 5 | def main(): 6 | rclpy.init() 7 | 8 | params = Node('traj_params', allow_undeclared_parameters=True, automatically_declare_parameters_from_overrides=True) 9 | frequency = params.get_parameter('freq').value 10 | 11 | trajectory = SplinePosition(update_frequency = frequency, pose_handler = 'pubsub', pose_topic = 'odom') 12 | 13 | trajectory.get_logger().info('Go!') 14 | 15 | rclpy.spin(trajectory) 16 | rclpy.shutdown() 17 | -------------------------------------------------------------------------------- /crazychoir_examples/crazychoir_examples/collision_webots/trajectory_vel.py: -------------------------------------------------------------------------------- 1 | import rclpy 2 | from crazychoir.planner.trajectory_handler import Spline, SplinePosition 3 | from rclpy.node import Node 4 | 5 | def main(): 6 | rclpy.init() 7 | 8 | params = Node('traj_params', allow_undeclared_parameters=True, automatically_declare_parameters_from_overrides=True) 9 | frequency = params.get_parameter('freq').value 10 | 11 | trajectory = SplinePosition(update_frequency = frequency, pose_handler = 'pubsub', pose_topic = 'odom') 12 | 13 | trajectory.get_logger().info('Go!') 14 | 15 | rclpy.spin(trajectory) 16 | rclpy.shutdown() 17 | -------------------------------------------------------------------------------- /crazychoir_examples/crazychoir_examples/aggregative_lighthouse/trajectory.py: -------------------------------------------------------------------------------- 1 | import rclpy 2 | from crazychoir.planner.trajectory_handler import Spline, SplinePosition 3 | from rclpy.node import Node 4 | 5 | def main(): 6 | rclpy.init() 7 | 8 | params = Node('traj_params', allow_undeclared_parameters=True, automatically_declare_parameters_from_overrides=True) 9 | frequency = params.get_parameter('freq').value 10 | 11 | trajectory = SplinePosition(update_frequency = frequency, pose_handler = 'pubsub', pose_topic = 'cf_odom') 12 | 13 | trajectory.get_logger().info('Go!') 14 | 15 | rclpy.spin(trajectory) 16 | rclpy.shutdown() 17 | -------------------------------------------------------------------------------- /crazychoir/crazychoir/controller/__init__.py: -------------------------------------------------------------------------------- 1 | from .hierarchical_control import CrazyflieController 2 | from .hierarchical_control import HierarchicalController 3 | from .hierarchical_control import PositionCtrlStrategy 4 | from .hierarchical_control import AttitudeCtrlStrategy 5 | from .hierarchical_control import SafeHierarchicalController 6 | from .flatness_position_ctrl import FlatnessPositionCtrl 7 | from .flatness_position_ctrl import SafeFlatnessPositionCtrl 8 | from .flatness_position_ctrl import FlatnessAccelerationCtrl 9 | from .geometric_attitude_ctrl import GeometryAttitudeCtrl 10 | from .velocity_ctrl import VelocityCtrlStrategy 11 | from .velocity_ctrl import VelocityController -------------------------------------------------------------------------------- /crazychoir_examples/crazychoir_examples/task_assignment_vicon/simple_guidance.py: -------------------------------------------------------------------------------- 1 | import rclpy 2 | from rclpy.node import Node 3 | from crazychoir.guidance import SimpleGuidance 4 | 5 | def main(): 6 | rclpy.init() 7 | 8 | simple_guidance_params = Node('simple_guidance_params', allow_undeclared_parameters=True, automatically_declare_parameters_from_overrides=True) 9 | vicon_id = simple_guidance_params.get_parameter('vicon_id').value 10 | 11 | simple_guidance = SimpleGuidance(pose_handler='vicon', pose_topic='/vicon/cf{}/cf{}'.format(vicon_id,vicon_id)) 12 | simple_guidance.get_logger().info('Go!') 13 | 14 | rclpy.spin(simple_guidance) 15 | rclpy.shutdown() 16 | -------------------------------------------------------------------------------- /docker/buildImage: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | IMAGE_PATH=$1 4 | IMAGE_NAME=$2 5 | 6 | if [ -z $IMAGE_PATH ]; then 7 | echo "You must provide the path to the directory with the Dockerfile you want to build" 8 | echo "bash buildImage.sh ./path/to/image image_name" 9 | exit 1 10 | fi 11 | 12 | if [ -z $IMAGE_NAME ]; then 13 | echo "You must provide the name of the image you want to build" 14 | echo "bash buildImage.sh ./path/to/image image_name" 15 | exit 1 16 | fi 17 | 18 | if [ ! -d $IMAGE_PATH ]; then 19 | echo "The path does not exist:" 20 | echo "$1" 21 | exit 1 22 | fi 23 | 24 | # Build base docker image 25 | docker build \ 26 | --file $IMAGE_PATH/Dockerfile \ 27 | --tag $IMAGE_NAME \ 28 | $IMAGE_PATH 29 | -------------------------------------------------------------------------------- /crazychoir_examples/crazychoir_examples/aggregative_webots/simple_guidance.py: -------------------------------------------------------------------------------- 1 | import rclpy 2 | from crazychoir.guidance import SimpleGuidance 3 | from rclpy.node import Node 4 | 5 | def main(): 6 | rclpy.init() 7 | 8 | node = Node('simpl_guid_cf', allow_undeclared_parameters=True, automatically_declare_parameters_from_overrides=True) 9 | agent_id = node.get_parameter('agent_id').value 10 | 11 | # height = 0.5 + agent_id*0.15 12 | height = 1.0 13 | takeoff_time = 3.0 14 | 15 | guidance = SimpleGuidance(pose_handler='pubsub', pose_topic='odom', takeoff_time = takeoff_time, height=height) 16 | 17 | guidance.get_logger().info('Go!') 18 | 19 | rclpy.spin(guidance) 20 | rclpy.shutdown() 21 | -------------------------------------------------------------------------------- /crazychoir_examples/crazychoir_examples/aggregative_lighthouse/simple_guidance.py: -------------------------------------------------------------------------------- 1 | import rclpy 2 | from crazychoir.guidance import SimpleGuidance 3 | from rclpy.node import Node 4 | 5 | def main(): 6 | rclpy.init() 7 | 8 | node = Node('simpl_guid_cf', allow_undeclared_parameters=True, automatically_declare_parameters_from_overrides=True) 9 | agent_id = node.get_parameter('agent_id').value 10 | 11 | # height = 0.5 + agent_id*0.15 12 | height = 1.0 13 | takeoff_time = 2.0 14 | 15 | guidance = SimpleGuidance(pose_handler='pubsub', pose_topic='cf_odom', takeoff_time = takeoff_time, height=height) 16 | 17 | guidance.get_logger().info('Go!') 18 | 19 | rclpy.spin(guidance) 20 | rclpy.shutdown() 21 | -------------------------------------------------------------------------------- /crazychoir_interfaces/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | crazychoir_interfaces 5 | 0.0.1 6 | Interfaces for crazychoir 7 | OPT4SMART 8 | GNU General Public License v3.0 9 | 10 | rosidl_default_generators 11 | 12 | rosidl_interface_packages 13 | geometry_msgs 14 | 15 | 16 | ament_cmake 17 | 18 | 19 | -------------------------------------------------------------------------------- /crazychoir/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | crazychoir 5 | 0.0.0 6 | TODO: Package description 7 | andrea 8 | TODO: License declaration 9 | 10 | ament_copyright 11 | ament_flake8 12 | ament_pep257 13 | python3-pytest 14 | choirbot 15 | 16 | ament_python 17 | 18 | 19 | -------------------------------------------------------------------------------- /crazychoir/setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import setup 2 | 3 | package_name = 'crazychoir' 4 | 5 | setup( 6 | name=package_name, 7 | version='0.0.0', 8 | packages=[package_name], 9 | data_files=[ 10 | ('share/ament_index/resource_index/packages', 11 | ['resource/' + package_name]), 12 | ('share/' + package_name, ['package.xml']), 13 | ], 14 | install_requires=['setuptools', 'choirbot'], 15 | zip_safe=True, 16 | maintainer='Andrea Testa', 17 | maintainer_email='a.testa@unibo.it', 18 | description='TODO: Package description', 19 | license='TODO: License declaration', 20 | tests_require=['pytest'], 21 | entry_points={ 22 | 'console_scripts': [ 23 | ], 24 | }, 25 | ) 26 | -------------------------------------------------------------------------------- /crazychoir_examples/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | crazychoir_examples 5 | 0.0.0 6 | TODO: Package description 7 | opt4smart 8 | TODO: License declaration 9 | 10 | ament_copyright 11 | ament_flake8 12 | ament_pep257 13 | python3-pytest 14 | crazychoir 15 | 16 | ament_python 17 | 18 | 19 | -------------------------------------------------------------------------------- /crazychoir_examples/crazychoir_examples/task_assignment_vicon/planner.py: -------------------------------------------------------------------------------- 1 | import rclpy 2 | from rclpy.node import Node 3 | from crazychoir.planner import PointToPointPlanner_2D 4 | from rclpy.executors import MultiThreadedExecutor 5 | 6 | 7 | def main(): 8 | rclpy.init() 9 | 10 | planner_params = Node('planner_params', allow_undeclared_parameters=True, automatically_declare_parameters_from_overrides=True) 11 | vicon_id = planner_params.get_parameter('vicon_id').value 12 | 13 | goal_tolerance = 0.05 14 | 15 | planner = PointToPointPlanner_2D(goal_tolerance, pose_handler='vicon', pose_topic='/vicon/cf{}/cf{}'.format(vicon_id,vicon_id)) 16 | 17 | executor = MultiThreadedExecutor() 18 | rclpy.spin(planner, executor) 19 | rclpy.shutdown() 20 | -------------------------------------------------------------------------------- /crazychoir_examples/crazychoir_examples/formation_vicon/guidance.py: -------------------------------------------------------------------------------- 1 | import rclpy 2 | from rclpy.node import Node 3 | from crazychoir.guidance import BearingFormation 4 | from crazychoir.filtering import Estimator 5 | 6 | def main(): 7 | rclpy.init() 8 | 9 | guidance_params = Node('guidance_params', allow_undeclared_parameters=True, automatically_declare_parameters_from_overrides=True) 10 | vicon_id = guidance_params.get_parameter('vicon_id').value 11 | 12 | frequency = 100 13 | callback = Estimator() 14 | guidance = BearingFormation(update_frequency=frequency, pose_handler='vicon', pose_topic='/vicon/cf{}/cf{}'.format(vicon_id,vicon_id), pose_callback=callback) 15 | 16 | callback.set_pose(guidance.current_pose) 17 | 18 | rclpy.spin(guidance) 19 | rclpy.shutdown() 20 | -------------------------------------------------------------------------------- /crazychoir_examples/crazychoir_examples/goto_webots/controller.py: -------------------------------------------------------------------------------- 1 | import rclpy 2 | from crazychoir.controller import VelocityController, VelocityCtrlStrategy 3 | from crazychoir.planner import PositionTraj 4 | from crazychoir.crazyflie_communication import VelSender 5 | 6 | def main(): 7 | rclpy.init() 8 | 9 | frequency = 10 10 | 11 | velocity_strategy = VelocityCtrlStrategy(frequency) 12 | sender = VelSender(frequency) 13 | desired_reference = PositionTraj() 14 | 15 | controller = VelocityController(pose_handler='pubsub', pose_topic='odom', pose_callback=None, 16 | velocity_strategy=velocity_strategy, 17 | command_sender=sender, traj_handler=desired_reference) 18 | 19 | controller.get_logger().info('Go!') 20 | 21 | rclpy.spin(controller) 22 | rclpy.shutdown() -------------------------------------------------------------------------------- /crazychoir_examples/crazychoir_examples/formation_vicon/trajectory.py: -------------------------------------------------------------------------------- 1 | import rclpy 2 | from rclpy.node import Node 3 | 4 | from crazychoir.planner.trajectory_handler import Spline 5 | from crazychoir.filtering import Estimator 6 | 7 | def main(): 8 | rclpy.init() 9 | 10 | trajectory_params = Node('trajectory_params', allow_undeclared_parameters=True, automatically_declare_parameters_from_overrides=True) 11 | vicon_id = trajectory_params.get_parameter('vicon_id').value 12 | 13 | frequency = 100 14 | callback = Estimator() 15 | trajectory = Spline(update_frequency = frequency, pose_handler = 'vicon', pose_topic = '/vicon/cf{}/cf{}'.format(vicon_id,vicon_id), pose_callback = callback) 16 | 17 | callback.set_pose(trajectory.current_pose) 18 | 19 | trajectory.get_logger().info('Go!') 20 | 21 | rclpy.spin(trajectory) 22 | rclpy.shutdown() 23 | -------------------------------------------------------------------------------- /docker/final_setup.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | DEV_NAME=opt4smart 4 | ROS_DISTRO=foxy 5 | WS_NAME=crazychoir_ws 6 | 7 | 8 | cd /home/${DEV_NAME}/${WS_NAME}/src/ros2-vicon-receiver 9 | ./install_libs.sh 10 | 11 | # Clean up unnecessary webots_ros2 packages 12 | rm -rf /home/${DEV_NAME}/${WS_NAME}/src/webots_ros2/webots_ros2_core \ 13 | /home/${DEV_NAME}/${WS_NAME}/src/webots_ros2/webots_ros2_epuck \ 14 | /home/${DEV_NAME}/${WS_NAME}/src/webots_ros2/webots_ros2_mavic \ 15 | /home/${DEV_NAME}/${WS_NAME}/src/webots_ros2/webots_ros2_tesla \ 16 | /home/${DEV_NAME}/${WS_NAME}/src/webots_ros2/webots_ros2_tests \ 17 | /home/${DEV_NAME}/${WS_NAME}/src/webots_ros2/webots_ros2_tiago \ 18 | /home/${DEV_NAME}/${WS_NAME}/src/webots_ros2/webots_ros2_turtlebot \ 19 | /home/${DEV_NAME}/${WS_NAME}/src/webots_ros2/webots_ros2_universal_robot -------------------------------------------------------------------------------- /crazychoir_examples/crazychoir_examples/formation_rviz/controller.py: -------------------------------------------------------------------------------- 1 | import rclpy 2 | from crazychoir.controller import HierarchicalController, FlatnessAccelerationCtrl, GeometryAttitudeCtrl 3 | from crazychoir.planner import AccelerationTraj 4 | from crazychoir.crazyflie_communication import FPQRSender 5 | 6 | def main(): 7 | rclpy.init() 8 | 9 | frequency = 100 10 | position_ctrl = FlatnessAccelerationCtrl(frequency) 11 | attitude_ctrl = GeometryAttitudeCtrl(frequency) 12 | sender = FPQRSender(frequency) 13 | desired_acceleration = AccelerationTraj() 14 | 15 | controller = HierarchicalController(pose_handler='pubsub', pose_topic='odom', 16 | position_strategy=position_ctrl, attitude_strategy=attitude_ctrl, 17 | command_sender=sender, traj_handler=desired_acceleration) 18 | 19 | rclpy.spin(controller) 20 | rclpy.shutdown() 21 | -------------------------------------------------------------------------------- /crazychoir_examples/test/test_flake8.py: -------------------------------------------------------------------------------- 1 | # Copyright 2017 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_flake8.main import main 16 | import pytest 17 | 18 | 19 | @pytest.mark.flake8 20 | @pytest.mark.linter 21 | def test_flake8(): 22 | rc = main(argv=[]) 23 | assert rc == 0, 'Found errors' 24 | -------------------------------------------------------------------------------- /crazychoir_examples/crazychoir_examples/task_assignment_vicon/guidance.py: -------------------------------------------------------------------------------- 1 | import rclpy 2 | from rclpy.node import Node 3 | from choirbot.guidance.task import TaskGuidance, PositionTaskExecutor 4 | from choirbot.optimizer import TaskOptimizer 5 | 6 | def main(): 7 | rclpy.init() 8 | 9 | guidance_params = Node('guidance_params', allow_undeclared_parameters=True, automatically_declare_parameters_from_overrides=True) 10 | vicon_id = guidance_params.get_parameter('vicon_id').value 11 | 12 | # initialize task guidance 13 | opt_settings = {'max_iterations': 20} 14 | executor = PositionTaskExecutor() 15 | optimizer = TaskOptimizer(settings=opt_settings) 16 | 17 | guidance = TaskGuidance(optimizer=optimizer, executor=executor, data=None, pose_handler='vicon', pose_topic='/vicon/cf{}/cf{}'.format(vicon_id,vicon_id)) 18 | 19 | rclpy.spin(guidance) 20 | rclpy.shutdown() 21 | -------------------------------------------------------------------------------- /crazychoir/test/test_copyright.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_copyright.main import main 16 | import pytest 17 | 18 | 19 | @pytest.mark.copyright 20 | @pytest.mark.linter 21 | def test_copyright(): 22 | rc = main(argv=['.', 'test']) 23 | assert rc == 0, 'Found errors' 24 | -------------------------------------------------------------------------------- /crazychoir_examples/test/test_copyright.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_copyright.main import main 16 | import pytest 17 | 18 | 19 | @pytest.mark.copyright 20 | @pytest.mark.linter 21 | def test_copyright(): 22 | rc = main(argv=['.', 'test']) 23 | assert rc == 0, 'Found errors' 24 | -------------------------------------------------------------------------------- /crazychoir/test/test_pep257.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_pep257.main import main 16 | import pytest 17 | 18 | 19 | @pytest.mark.linter 20 | @pytest.mark.pep257 21 | def test_pep257(): 22 | rc = main(argv=['.', 'test']) 23 | assert rc == 0, 'Found code style errors / warnings' 24 | -------------------------------------------------------------------------------- /crazychoir_examples/test/test_pep257.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_pep257.main import main 16 | import pytest 17 | 18 | 19 | @pytest.mark.linter 20 | @pytest.mark.pep257 21 | def test_pep257(): 22 | rc = main(argv=['.', 'test']) 23 | assert rc == 0, 'Found code style errors / warnings' 24 | -------------------------------------------------------------------------------- /crazychoir_examples/crazychoir_examples/tracking_webots/controller.py: -------------------------------------------------------------------------------- 1 | import rclpy 2 | from crazychoir.controller import HierarchicalController, GeometryAttitudeCtrl, FlatnessPositionCtrl 3 | from crazychoir.planner import FullStateTraj 4 | from crazychoir.crazyflie_communication import FPQRSender 5 | import time 6 | 7 | def main(): 8 | rclpy.init() 9 | 10 | frequency = 100 11 | position_ctrl = FlatnessPositionCtrl(frequency) 12 | desired_traj = FullStateTraj() 13 | attitude_ctrl = GeometryAttitudeCtrl(frequency) 14 | sender = FPQRSender(frequency) 15 | 16 | controller = HierarchicalController(pose_handler='pubsub', pose_topic='odom', pose_callback=None, 17 | position_strategy=position_ctrl, attitude_strategy=attitude_ctrl, 18 | command_sender=sender, traj_handler=desired_traj) 19 | 20 | controller.get_logger().info('Go!') 21 | 22 | rclpy.spin(controller) 23 | rclpy.shutdown() 24 | 25 | -------------------------------------------------------------------------------- /crazychoir_examples/crazychoir_examples/formation_webots/controller_leaders.py: -------------------------------------------------------------------------------- 1 | import rclpy 2 | from crazychoir.controller import HierarchicalController, GeometryAttitudeCtrl, FlatnessPositionCtrl 3 | from crazychoir.planner import FullStateTraj 4 | from crazychoir.crazyflie_communication import FPQRSender 5 | import time 6 | 7 | def main(): 8 | rclpy.init() 9 | 10 | frequency = 100 11 | position_ctrl = FlatnessPositionCtrl(frequency) 12 | desired_traj = FullStateTraj() 13 | attitude_ctrl = GeometryAttitudeCtrl(frequency) 14 | sender = FPQRSender(frequency) 15 | 16 | controller = HierarchicalController(pose_handler='pubsub', pose_topic='odom', pose_callback=None, 17 | position_strategy=position_ctrl, attitude_strategy=attitude_ctrl, 18 | command_sender=sender, traj_handler=desired_traj) 19 | 20 | controller.get_logger().info('Go!') 21 | 22 | rclpy.spin(controller) 23 | rclpy.shutdown() 24 | 25 | -------------------------------------------------------------------------------- /crazychoir/crazychoir/planner/fullstate_reference.py: -------------------------------------------------------------------------------- 1 | from crazychoir_interfaces.msg import FullState 2 | import numpy as np 3 | from .trajectory_strategy import TrajHandlerStrategy 4 | from scipy.spatial.transform import Rotation as R 5 | 6 | class FullStateTraj(TrajHandlerStrategy): 7 | def __init__(self, msg_type=FullState, topic='fullstate'): 8 | super().__init__(msg_type, topic) 9 | 10 | def handler_func(self, msg): 11 | self.des_ref["position"] = np.array([msg.pose.position.x, msg.pose.position.y, msg.pose.position.z]) 12 | self.des_ref["velocity"] = np.array([msg.vel.linear.x, msg.vel.linear.y, msg.vel.linear.z]) 13 | self.des_ref["acceleration"] = np.array([msg.acc.linear.x, msg.acc.linear.y, msg.acc.linear.z]) 14 | 15 | _, _, self.des_ref["yaw"] = R.from_quat(np.array([msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z, msg.pose.orientation.w])).as_euler('xyz') 16 | # self.des_ref["yaw_rate"] = ... -------------------------------------------------------------------------------- /crazychoir_examples/crazychoir_examples/formation_webots/controller_followers.py: -------------------------------------------------------------------------------- 1 | import rclpy 2 | from crazychoir.controller import HierarchicalController, FlatnessAccelerationCtrl, GeometryAttitudeCtrl 3 | from crazychoir.planner import AccelerationTraj 4 | from crazychoir.crazyflie_communication import FPQRSender 5 | import time 6 | 7 | def main(): 8 | rclpy.init() 9 | 10 | frequency = 100 11 | position_ctrl = FlatnessAccelerationCtrl(frequency) 12 | desired_traj = AccelerationTraj() 13 | attitude_ctrl = GeometryAttitudeCtrl(frequency) 14 | sender = FPQRSender(frequency) 15 | 16 | controller = HierarchicalController(pose_handler='pubsub', pose_topic='odom', pose_callback=None, 17 | position_strategy=position_ctrl, attitude_strategy=attitude_ctrl, 18 | command_sender=sender, traj_handler=desired_traj) 19 | 20 | controller.get_logger().info('Go!') 21 | 22 | rclpy.spin(controller) 23 | rclpy.shutdown() 24 | 25 | -------------------------------------------------------------------------------- /crazychoir_examples/crazychoir_examples/goto_webots/gui.py: -------------------------------------------------------------------------------- 1 | import sys 2 | from crazychoir.gui.gui import MainWindow 3 | from crazychoir.gui.controller import SettingsParser 4 | from qt_material import apply_stylesheet 5 | from PyQt5 import QtWidgets 6 | import rclpy 7 | 8 | def main(args=None): 9 | 10 | rclpy.init(args=args) 11 | 12 | # Extra settings for application theme 13 | extra = { 14 | 'density_scale': '2', 15 | } 16 | 17 | app = QtWidgets.QApplication(sys.argv) 18 | screen_resolution = app.desktop().screenGeometry() 19 | 20 | exit_code = 1 21 | 22 | while exit_code == 1: 23 | settings = SettingsParser(app) 24 | settings.set_screen_res(screen_resolution) 25 | 26 | window = MainWindow(settings) 27 | apply_stylesheet(app, theme='dark_teal.xml', extra=extra) 28 | 29 | window.show() 30 | exit_code = app.exec_() 31 | window.close() 32 | 33 | if __name__ == '__main__': 34 | main() -------------------------------------------------------------------------------- /crazychoir_examples/crazychoir_examples/tracking_vicon/gui.py: -------------------------------------------------------------------------------- 1 | import sys 2 | from crazychoir.gui.gui import MainWindow 3 | from crazychoir.gui.controller import SettingsParser 4 | from qt_material import apply_stylesheet 5 | from PyQt5 import QtWidgets 6 | import rclpy 7 | 8 | def main(args=None): 9 | 10 | rclpy.init(args=args) 11 | 12 | # Extra settings for application theme 13 | extra = { 14 | 'density_scale': '2', 15 | } 16 | 17 | app = QtWidgets.QApplication(sys.argv) 18 | screen_resolution = app.desktop().screenGeometry() 19 | 20 | exit_code = 1 21 | 22 | while exit_code == 1: 23 | settings = SettingsParser(app) 24 | settings.set_screen_res(screen_resolution) 25 | 26 | window = MainWindow(settings) 27 | apply_stylesheet(app, theme='dark_teal.xml', extra=extra) 28 | 29 | window.show() 30 | exit_code = app.exec_() 31 | window.close() 32 | 33 | if __name__ == '__main__': 34 | main() -------------------------------------------------------------------------------- /crazychoir_examples/crazychoir_examples/aggregative_webots/gui.py: -------------------------------------------------------------------------------- 1 | import sys 2 | from crazychoir.gui.gui import MainWindow 3 | from crazychoir.gui.controller import SettingsParser 4 | from qt_material import apply_stylesheet 5 | from PyQt5 import QtWidgets 6 | import rclpy 7 | 8 | def main(args=None): 9 | 10 | rclpy.init(args=args) 11 | 12 | # Extra settings for application theme 13 | extra = { 14 | 'density_scale': '2', 15 | } 16 | 17 | app = QtWidgets.QApplication(sys.argv) 18 | screen_resolution = app.desktop().screenGeometry() 19 | 20 | exit_code = 1 21 | 22 | while exit_code == 1: 23 | settings = SettingsParser(app) 24 | settings.set_screen_res(screen_resolution) 25 | 26 | window = MainWindow(settings) 27 | apply_stylesheet(app, theme='dark_teal.xml', extra=extra) 28 | 29 | window.show() 30 | exit_code = app.exec_() 31 | window.close() 32 | 33 | if __name__ == '__main__': 34 | main() -------------------------------------------------------------------------------- /crazychoir_examples/crazychoir_examples/collision_webots/gui.py: -------------------------------------------------------------------------------- 1 | import sys 2 | from crazychoir.gui.gui import MainWindow 3 | from crazychoir.gui.controller import SettingsParser 4 | from qt_material import apply_stylesheet 5 | from PyQt5 import QtWidgets 6 | import rclpy 7 | 8 | def main(args=None): 9 | 10 | rclpy.init(args=args) 11 | 12 | # Extra settings for application theme 13 | extra = { 14 | 'density_scale': '2', 15 | } 16 | 17 | app = QtWidgets.QApplication(sys.argv) 18 | screen_resolution = app.desktop().screenGeometry() 19 | 20 | exit_code = 1 21 | 22 | while exit_code == 1: 23 | settings = SettingsParser(app) 24 | settings.set_screen_res(screen_resolution) 25 | 26 | window = MainWindow(settings) 27 | apply_stylesheet(app, theme='dark_teal.xml', extra=extra) 28 | 29 | window.show() 30 | exit_code = app.exec_() 31 | window.close() 32 | 33 | if __name__ == '__main__': 34 | main() -------------------------------------------------------------------------------- /crazychoir_examples/crazychoir_examples/formation_vicon/gui.py: -------------------------------------------------------------------------------- 1 | import sys 2 | from crazychoir.gui.gui import MainWindow 3 | from crazychoir.gui.controller import SettingsParser 4 | from qt_material import apply_stylesheet 5 | from PyQt5 import QtWidgets 6 | import rclpy 7 | 8 | def main(args=None): 9 | 10 | rclpy.init(args=args) 11 | 12 | # Extra settings for application theme 13 | extra = { 14 | 'density_scale': '2', 15 | } 16 | 17 | app = QtWidgets.QApplication(sys.argv) 18 | screen_resolution = app.desktop().screenGeometry() 19 | 20 | exit_code = 1 21 | 22 | while exit_code == 1: 23 | settings = SettingsParser(app) 24 | settings.set_screen_res(screen_resolution) 25 | 26 | window = MainWindow(settings) 27 | apply_stylesheet(app, theme='dark_teal.xml', extra=extra) 28 | 29 | window.show() 30 | exit_code = app.exec_() 31 | window.close() 32 | 33 | if __name__ == '__main__': 34 | main() -------------------------------------------------------------------------------- /crazychoir_examples/crazychoir_examples/formation_webots/gui.py: -------------------------------------------------------------------------------- 1 | import sys 2 | from crazychoir.gui.gui import MainWindow 3 | from crazychoir.gui.controller import SettingsParser 4 | from qt_material import apply_stylesheet 5 | from PyQt5 import QtWidgets 6 | import rclpy 7 | 8 | def main(args=None): 9 | 10 | rclpy.init(args=args) 11 | 12 | # Extra settings for application theme 13 | extra = { 14 | 'density_scale': '2', 15 | } 16 | 17 | app = QtWidgets.QApplication(sys.argv) 18 | screen_resolution = app.desktop().screenGeometry() 19 | 20 | exit_code = 1 21 | 22 | while exit_code == 1: 23 | settings = SettingsParser(app) 24 | settings.set_screen_res(screen_resolution) 25 | 26 | window = MainWindow(settings) 27 | apply_stylesheet(app, theme='dark_teal.xml', extra=extra) 28 | 29 | window.show() 30 | exit_code = app.exec_() 31 | window.close() 32 | 33 | if __name__ == '__main__': 34 | main() -------------------------------------------------------------------------------- /crazychoir_examples/crazychoir_examples/tracking_webots/gui.py: -------------------------------------------------------------------------------- 1 | import sys 2 | from crazychoir.gui.gui import MainWindow 3 | from crazychoir.gui.controller import SettingsParser 4 | from qt_material import apply_stylesheet 5 | from PyQt5 import QtWidgets 6 | import rclpy 7 | 8 | def main(args=None): 9 | 10 | rclpy.init(args=args) 11 | 12 | # Extra settings for application theme 13 | extra = { 14 | 'density_scale': '2', 15 | } 16 | 17 | app = QtWidgets.QApplication(sys.argv) 18 | screen_resolution = app.desktop().screenGeometry() 19 | 20 | exit_code = 1 21 | 22 | while exit_code == 1: 23 | settings = SettingsParser(app) 24 | settings.set_screen_res(screen_resolution) 25 | 26 | window = MainWindow(settings) 27 | apply_stylesheet(app, theme='dark_teal.xml', extra=extra) 28 | 29 | window.show() 30 | exit_code = app.exec_() 31 | window.close() 32 | 33 | if __name__ == '__main__': 34 | main() -------------------------------------------------------------------------------- /crazychoir_examples/crazychoir_examples/aggregative_lighthouse/gui.py: -------------------------------------------------------------------------------- 1 | import sys 2 | from crazychoir.gui.gui import MainWindow 3 | from crazychoir.gui.controller import SettingsParser 4 | from qt_material import apply_stylesheet 5 | from PyQt5 import QtWidgets 6 | import rclpy 7 | 8 | def main(args=None): 9 | 10 | rclpy.init(args=args) 11 | 12 | # Extra settings for application theme 13 | extra = { 14 | 'density_scale': '2', 15 | } 16 | 17 | app = QtWidgets.QApplication(sys.argv) 18 | screen_resolution = app.desktop().screenGeometry() 19 | 20 | exit_code = 1 21 | 22 | while exit_code == 1: 23 | settings = SettingsParser(app) 24 | settings.set_screen_res(screen_resolution) 25 | 26 | window = MainWindow(settings) 27 | apply_stylesheet(app, theme='dark_teal.xml', extra=extra) 28 | 29 | window.show() 30 | exit_code = app.exec_() 31 | window.close() 32 | 33 | if __name__ == '__main__': 34 | main() -------------------------------------------------------------------------------- /crazychoir_examples/crazychoir_examples/task_assignment_vicon/gui.py: -------------------------------------------------------------------------------- 1 | import sys 2 | from crazychoir.gui.gui import MainWindow 3 | from crazychoir.gui.controller import SettingsParser 4 | from qt_material import apply_stylesheet 5 | from PyQt5 import QtWidgets 6 | import rclpy 7 | 8 | def main(args=None): 9 | 10 | rclpy.init(args=args) 11 | 12 | # Extra settings for application theme 13 | extra = { 14 | 'density_scale': '2', 15 | } 16 | 17 | app = QtWidgets.QApplication(sys.argv) 18 | screen_resolution = app.desktop().screenGeometry() 19 | 20 | exit_code = 1 21 | 22 | while exit_code == 1: 23 | settings = SettingsParser(app) 24 | settings.set_screen_res(screen_resolution) 25 | 26 | window = MainWindow(settings) 27 | apply_stylesheet(app, theme='dark_teal.xml', extra=extra) 28 | 29 | window.show() 30 | exit_code = app.exec_() 31 | window.close() 32 | 33 | if __name__ == '__main__': 34 | main() -------------------------------------------------------------------------------- /crazychoir_examples/crazychoir_examples/task_assignment_webots/gui.py: -------------------------------------------------------------------------------- 1 | import sys 2 | from crazychoir.gui.gui import MainWindow 3 | from crazychoir.gui.controller import SettingsParser 4 | from qt_material import apply_stylesheet 5 | from PyQt5 import QtWidgets 6 | import rclpy 7 | 8 | def main(args=None): 9 | 10 | rclpy.init(args=args) 11 | 12 | # Extra settings for application theme 13 | extra = { 14 | 'density_scale': '2', 15 | } 16 | 17 | app = QtWidgets.QApplication(sys.argv) 18 | screen_resolution = app.desktop().screenGeometry() 19 | 20 | exit_code = 1 21 | 22 | while exit_code == 1: 23 | settings = SettingsParser(app) 24 | settings.set_screen_res(screen_resolution) 25 | 26 | window = MainWindow(settings) 27 | apply_stylesheet(app, theme='dark_teal.xml', extra=extra) 28 | 29 | window.show() 30 | exit_code = app.exec_() 31 | window.close() 32 | 33 | if __name__ == '__main__': 34 | main() -------------------------------------------------------------------------------- /crazychoir_examples/crazychoir_examples/task_assignment_webots/table.py: -------------------------------------------------------------------------------- 1 | import rclpy 2 | from rclpy.node import Node 3 | from choirbot.guidance.task import PositionTaskTable 4 | from std_msgs.msg import Empty 5 | 6 | class TriggerNode(Node): 7 | def __init__(self): 8 | super().__init__('table_parameters', allow_undeclared_parameters=True, automatically_declare_parameters_from_overrides=True) 9 | self.n_agents = self.get_parameter('N').value 10 | self.create_subscription(Empty, '/experiment_trigger', self.experiment_trigger, 10) 11 | 12 | def experiment_trigger(self, _): 13 | self.get_logger().info('Starting experiment') 14 | 15 | table = PositionTaskTable(self.n_agents) 16 | table.gc.trigger() 17 | 18 | rclpy.spin(table) 19 | rclpy.shutdown() 20 | 21 | def main(): 22 | rclpy.init() 23 | trigger = TriggerNode() 24 | rclpy.spin_once(trigger) 25 | rclpy.shutdown() 26 | 27 | if __name__ == '__main__': 28 | main() -------------------------------------------------------------------------------- /crazychoir/test/test_flake8.py: -------------------------------------------------------------------------------- 1 | # Copyright 2017 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_flake8.main import main_with_errors 16 | import pytest 17 | 18 | 19 | @pytest.mark.flake8 20 | @pytest.mark.linter 21 | def test_flake8(): 22 | rc, errors = main_with_errors(argv=[]) 23 | assert rc == 0, \ 24 | 'Found %d code style errors / warnings:\n' % len(errors) + \ 25 | '\n'.join(errors) 26 | -------------------------------------------------------------------------------- /crazychoir_examples/worlds/empty_world.wbt: -------------------------------------------------------------------------------- 1 | #VRML_SIM R2022a utf8 2 | WorldInfo { 3 | basicTimeStep 9.5 4 | } 5 | Viewpoint { 6 | fieldOfView 0.8 7 | orientation -0.4 0.4 0.8 1.75285 8 | position 0 -3.2 5.3 9 | } 10 | TexturedBackground { 11 | } 12 | TexturedBackgroundLight { 13 | luminosity 0 14 | castShadows FALSE 15 | } 16 | DirectionalLight { 17 | direction 1 1 -1 18 | color 0.83 0.83 0.75 19 | ambientIntensity 1 20 | } 21 | Floor { 22 | translation 0 0 0 23 | rotation 0 1 0 0 24 | size 10 10 25 | appearance Appearance { 26 | material Material { 27 | ambientIntensity 0 28 | shininess 0 29 | } 30 | texture ImageTexture { 31 | url [ 32 | "/home/opt4smart/crazychoir_ws/src/crazychoir/crazychoir/crazychoir/utils/images/casy_logo.png" 33 | ] 34 | repeatS FALSE 35 | repeatT FALSE 36 | filtering 0 37 | } 38 | textureTransform TextureTransform { 39 | center 0.0 0.0 40 | scale 0.2 0.2 41 | } 42 | } 43 | } 44 | -------------------------------------------------------------------------------- /crazychoir_examples/crazychoir_examples/task_assignment_vicon/table.py: -------------------------------------------------------------------------------- 1 | import rclpy 2 | from rclpy.node import Node 3 | from choirbot.guidance.task import PositionTaskTable 4 | from std_msgs.msg import Empty 5 | 6 | class TriggerNode(Node): 7 | def __init__(self): 8 | super().__init__('table_parameters', allow_undeclared_parameters=True, automatically_declare_parameters_from_overrides=True) 9 | self.n_agents = self.get_parameter('N').value 10 | self.create_subscription(Empty, '/experiment_trigger', self.experiment_trigger, 10) 11 | 12 | def experiment_trigger(self, _): 13 | self.get_logger().info('Starting experiment') 14 | 15 | table = PositionTaskTable(self.n_agents) 16 | table.gc.trigger() 17 | 18 | rclpy.spin(table) 19 | rclpy.shutdown() 20 | 21 | def main(): 22 | rclpy.init() 23 | trigger = TriggerNode() 24 | rclpy.spin_once(trigger) 25 | # rclpy.spin(trigger) 26 | rclpy.shutdown() 27 | 28 | if __name__ == '__main__': 29 | main() -------------------------------------------------------------------------------- /crazychoir_examples/crazychoir_examples/aggregative_webots/controller.py: -------------------------------------------------------------------------------- 1 | import rclpy 2 | from crazychoir.controller import VelocityController, VelocityCtrlStrategy 3 | from crazychoir.planner import PositionTraj, FullStateTraj 4 | from crazychoir.crazyflie_communication import VelSender 5 | import time 6 | from rclpy.node import Node 7 | 8 | def main(): 9 | rclpy.init() 10 | 11 | params = Node('contr_params', allow_undeclared_parameters=True, automatically_declare_parameters_from_overrides=True) 12 | frequency = params.get_parameter('freq').value 13 | 14 | velocity_strategy = VelocityCtrlStrategy(frequency) 15 | sender = VelSender(frequency) 16 | # desired_reference = FullStateTraj() 17 | desired_reference = PositionTraj() 18 | 19 | controller = VelocityController(pose_handler='pubsub', pose_topic='odom', pose_callback=None, 20 | velocity_strategy=velocity_strategy, 21 | command_sender=sender, traj_handler=desired_reference) 22 | 23 | controller.get_logger().info('Go!') 24 | 25 | rclpy.spin(controller) 26 | rclpy.shutdown() -------------------------------------------------------------------------------- /crazychoir_examples/crazychoir_examples/collision_webots/controller_vel.py: -------------------------------------------------------------------------------- 1 | import rclpy 2 | from crazychoir.controller import VelocityController, VelocityCtrlStrategy 3 | from crazychoir.planner import PositionTraj, FullStateTraj 4 | from crazychoir.crazyflie_communication import VelSender 5 | import time 6 | from rclpy.node import Node 7 | 8 | def main(): 9 | rclpy.init() 10 | 11 | params = Node('contr_params', allow_undeclared_parameters=True, automatically_declare_parameters_from_overrides=True) 12 | frequency = params.get_parameter('freq').value 13 | 14 | velocity_strategy = VelocityCtrlStrategy(frequency) 15 | sender = VelSender(frequency) 16 | # desired_reference = FullStateTraj() 17 | desired_reference = PositionTraj() 18 | 19 | controller = VelocityController(pose_handler='pubsub', pose_topic='odom', pose_callback=None, 20 | velocity_strategy=velocity_strategy, 21 | command_sender=sender, traj_handler=desired_reference) 22 | 23 | controller.get_logger().info('Go!') 24 | 25 | rclpy.spin(controller) 26 | rclpy.shutdown() -------------------------------------------------------------------------------- /crazychoir_examples/crazychoir_examples/aggregative_lighthouse/controller.py: -------------------------------------------------------------------------------- 1 | import rclpy 2 | from crazychoir.controller import VelocityController, VelocityCtrlStrategy 3 | from crazychoir.planner import PositionTraj, FullStateTraj 4 | from crazychoir.crazyflie_communication import VelSender 5 | import time 6 | from rclpy.node import Node 7 | 8 | def main(): 9 | rclpy.init() 10 | 11 | params = Node('contr_params', allow_undeclared_parameters=True, automatically_declare_parameters_from_overrides=True) 12 | frequency = params.get_parameter('freq').value 13 | 14 | velocity_strategy = VelocityCtrlStrategy(frequency) 15 | sender = VelSender(frequency) 16 | # desired_reference = FullStateTraj() 17 | desired_reference = PositionTraj() 18 | 19 | controller = VelocityController(pose_handler='pubsub', pose_topic='cf_odom', pose_callback=None, 20 | velocity_strategy=velocity_strategy, 21 | command_sender=sender, traj_handler=desired_reference) 22 | 23 | controller.get_logger().info('Go!') 24 | 25 | rclpy.spin(controller) 26 | rclpy.shutdown() -------------------------------------------------------------------------------- /crazychoir_examples/crazychoir_examples/tracking_vicon/controller.py: -------------------------------------------------------------------------------- 1 | import rclpy 2 | from crazychoir.controller import HierarchicalController, GeometryAttitudeCtrl, FlatnessPositionCtrl 3 | from crazychoir.planner import FullStateTraj 4 | from crazychoir.crazyflie_communication import FPQRSender 5 | from crazychoir.filtering import Estimator 6 | 7 | def main(): 8 | rclpy.init() 9 | 10 | frequency = 100 11 | position_ctrl = FlatnessPositionCtrl(update_frequency=frequency, vicon=True) 12 | desired_traj = FullStateTraj() 13 | attitude_ctrl = GeometryAttitudeCtrl(update_frequency=frequency, vicon=True) 14 | sender = FPQRSender(frequency) 15 | 16 | callback = Estimator() 17 | 18 | controller = HierarchicalController(pose_handler='vicon', pose_topic='/vicon/cf1/cf1', pose_callback=callback, 19 | position_strategy=position_ctrl, attitude_strategy=attitude_ctrl, 20 | command_sender=sender, traj_handler=desired_traj) 21 | 22 | callback.set_pose(controller.current_pose) 23 | 24 | controller.get_logger().info('Go!') 25 | 26 | rclpy.spin(controller) 27 | rclpy.shutdown() 28 | 29 | -------------------------------------------------------------------------------- /crazychoir_examples/worlds/world_generator.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | def generate_webots_world_file(robots, source_filename, target_filename): 4 | with open(source_filename, 'r') as source_file: 5 | contents = source_file.read() 6 | 7 | with open(target_filename, 'w') as target_file: 8 | target_file.write(contents) 9 | 10 | for robot in robots: 11 | template_filename = os.path.join(os.path.dirname(source_filename), f'obj_{robot["type"]}.wbt') 12 | with open(template_filename, 'r') as template_file: 13 | template = template_file.read() 14 | template = template.replace('$NAME', robot["name"]) 15 | template = template.replace('$X', str(robot["position"][0])) 16 | template = template.replace('$Y', str(robot["position"][1])) 17 | template = template.replace('$Z', str(robot["position"][2])) 18 | target_file.write(template) 19 | 20 | # Define the robots 21 | robots = [ 22 | {"name": "agent_0", "type": "crazyflie", "position": [0, 0, 0]}, 23 | {"name": "agent_1", "type": "crazyflie", "position": [0, 0, 0]}, 24 | # Add more robots here... 25 | ] 26 | 27 | # Use the function to generate a Webots world file 28 | generate_webots_world_file(robots, 'empty_world.wbt', 'my_world.wbt') -------------------------------------------------------------------------------- /crazychoir/crazychoir/utils/frequency_plotter.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from matplotlib import pyplot as plt 3 | 4 | crazyradio_limits = [2400,2525] 5 | 6 | # Scan occupied frequencies running 'sudo iwlist scan | grep Frequency | sort | uniq -c | sort -n' 7 | 8 | # Occupied frequencies example 9 | not_avaliable_freq = [ 10 | [2,2412], 11 | [2,5320], 12 | [2,5500], 13 | [2,5660], 14 | [3,2462], 15 | ] 16 | 17 | # Filter 2.4GHz signals 18 | filtered_2400Mhz_not_avaliable_freq = [freq for freq in not_avaliable_freq if freq[1] < 3000] 19 | 20 | # Plot settings 21 | linewidths = [5/signal_quality[0] for signal_quality in filtered_2400Mhz_not_avaliable_freq] 22 | fig,ax = plt.subplots(1) 23 | colors = [] 24 | 25 | # Plot carrier frequencies and its harmonics 26 | for freq in filtered_2400Mhz_not_avaliable_freq: 27 | line, = ax.plot([freq[1],freq[1]],[0,4/freq[0]]) 28 | for i in range(-12,13,1): 29 | ax.plot([freq[1]-i,freq[1]-i],[0, (12-(abs(i)))**2/freq[0] ], color = line.get_color()) 30 | 31 | x_tiks = [freq[1] for freq in filtered_2400Mhz_not_avaliable_freq] 32 | for limit in crazyradio_limits: 33 | x_tiks.append(limit) 34 | 35 | print(x_tiks) 36 | plt.xticks(x_tiks) 37 | ax.set_xlim(crazyradio_limits) 38 | plt.show() 39 | 40 | # Select your radio channel choosing free frequency slots 41 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | # C extensions 7 | *.so 8 | 9 | # Distribution / packaging 10 | .Python 11 | build/ 12 | develop-eggs/ 13 | dist/ 14 | downloads/ 15 | eggs/ 16 | .eggs/ 17 | lib/ 18 | lib64/ 19 | parts/ 20 | sdist/ 21 | var/ 22 | wheels/ 23 | pip-wheel-metadata/ 24 | share/python-wheels/ 25 | *.egg-info/ 26 | .installed.cfg 27 | *.egg 28 | MANIFEST 29 | 30 | # Sphinx documentation 31 | docs/_build/ 32 | 33 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow 34 | __pypackages__/ 35 | 36 | # Rope project settings 37 | .ropeproject 38 | 39 | # mypy 40 | .mypy_cache/ 41 | .dmypy.json 42 | dmypy.json 43 | 44 | # ROS folders 45 | devel/ 46 | logs/ 47 | build/ 48 | install/ 49 | log/ 50 | bin/ 51 | lib/ 52 | msg_gen/ 53 | srv_gen/ 54 | msg/_*.py 55 | build_isolated/ 56 | devel_isolated/ 57 | 58 | # Generated by dynamic reconfigure 59 | *.cfgc 60 | /cfg/cpp/ 61 | /cfg/*.py 62 | 63 | # Ignore generated docs 64 | *.dox 65 | *.wikidoc 66 | 67 | # vscode stuff 68 | .vscode/ 69 | 70 | # qcreator stuff 71 | CMakeLists.txt.user 72 | 73 | srv/_*.py 74 | *.pcd 75 | *.pyc 76 | qtcreator-* 77 | *.user 78 | 79 | /planning/cfg 80 | /planning/docs 81 | /planning/src 82 | 83 | *~ 84 | *.DS_Store 85 | LOCAL* 86 | 87 | # ChoiRbot Dependences 88 | choirbot/ 89 | choirbot_interfaces/ -------------------------------------------------------------------------------- /crazychoir/crazychoir/gui/gui/custom_dialogs.py: -------------------------------------------------------------------------------- 1 | from PyQt5.QtWidgets import QDialog, QDialogButtonBox, QLabel, QVBoxLayout, QLineEdit, QMessageBox 2 | 3 | 4 | class SaveDialog(QDialog): 5 | def __init__(self): 6 | super().__init__() 7 | 8 | QBtn = QDialogButtonBox.Ok | QDialogButtonBox.Cancel 9 | 10 | self.buttonBox = QDialogButtonBox(QBtn) 11 | self.buttonBox.accepted.connect(self.accept) 12 | self.buttonBox.rejected.connect(self.reject) 13 | 14 | self.layout = QVBoxLayout() 15 | message = QLabel("Nome file") 16 | self.filename = QLineEdit() 17 | #self.filename.setPlaceholderText("trajectory") 18 | 19 | self.layout.addWidget(message) 20 | self.layout.addWidget(self.filename) 21 | self.layout.addWidget(self.buttonBox) 22 | self.setLayout(self.layout) 23 | self.setFixedSize(150,150) 24 | #self.setWindowTitle("Salva la traiettoria") 25 | 26 | class DeleteDialog(QMessageBox): 27 | def __init__(self,filename): 28 | super().__init__(None) 29 | 30 | tmp = filename.split('/') 31 | file_name = tmp[len(tmp) - 1] 32 | message = f'Cancellare {file_name}?' 33 | 34 | self.setWindowTitle(" ") 35 | self.setStandardButtons(QMessageBox.Yes | QMessageBox.No) 36 | self.setIcon(QMessageBox.Question) 37 | self.setText(message) -------------------------------------------------------------------------------- /crazychoir_examples/crazychoir_examples/aggregative_lighthouse/guidance_aggregative.py: -------------------------------------------------------------------------------- 1 | import rclpy 2 | from crazychoir.guidance.aggregative import AggregativeGuidanceCrazyflie, AggregativeGuidanceCrazyflieMovingIntruder 3 | from choirbot.optimizer import AggregativeOptimizer 4 | from rclpy.node import Node 5 | 6 | from geometry_msgs.msg import Vector3 7 | 8 | def main(): 9 | rclpy.init() 10 | 11 | node = Node('guid_cf', allow_undeclared_parameters=True, automatically_declare_parameters_from_overrides=True) 12 | 13 | max_iter = node.get_parameter('max_iter').value 14 | 15 | opt_settings = { 16 | 'agent_dim': 2, # dimension of agent's optimization variable 17 | 'stepsize':5e-3, # stepsize for gradient tracking 18 | # 19 | 'gamma_intruder': 1.0, # weight for ||x_i - r_i||^2 20 | 'gamma_target': 1.0, # weight for ||\sigma(x) - b_i||^2 21 | 'gamma_shape': 1.0, # weight for ||x_i - \sigma(x)||^2 22 | # 23 | 'max_iterations': max_iter, 24 | 'enable_log': False, 25 | } 26 | 27 | optimizer = AggregativeOptimizer(settings=opt_settings) 28 | 29 | freq_reference = node.get_parameter('freq_reference').value 30 | height = 1.0 31 | guidance = AggregativeGuidanceCrazyflie(optimizer, height, freq_reference, 'pubsub', 'cf_odom', None, 'position', Vector3) 32 | 33 | rclpy.spin(guidance) 34 | rclpy.shutdown() 35 | -------------------------------------------------------------------------------- /crazychoir/crazychoir/guidance/distributed_control.py: -------------------------------------------------------------------------------- 1 | from choirbot.guidance import Guidance 2 | from geometry_msgs.msg import Vector3 3 | from typing import Callable 4 | 5 | class DistributedControl(Guidance): 6 | 7 | def __init__(self, update_frequency: float, pose_handler: str=None, pose_topic: str=None, pose_callback: Callable=None, input_topic: str = 'acceleration'): 8 | super().__init__(pose_handler, pose_topic, pose_callback) 9 | self.publisher_ = self.create_publisher(Vector3, input_topic, 1) 10 | self.update_frequency = update_frequency 11 | self.timer = self.create_timer(1.0/self.update_frequency, self.control) 12 | 13 | def control(self): 14 | # skip if position is not available yet 15 | if self.current_pose.position is None: 16 | return 17 | 18 | # exchange current position with neighbors 19 | data = self.communicator.neighbors_exchange(self.current_pose, self.in_neighbors, self.out_neighbors, False) 20 | 21 | # compute input 22 | u = self.evaluate_input(data) 23 | 24 | # send input to planner/controller 25 | self.send_reference(u) 26 | 27 | 28 | def send_reference(self, ref): 29 | msg = Vector3() 30 | 31 | msg.x = ref[0] 32 | msg.y = ref[1] 33 | msg.z = ref[2] 34 | 35 | self.publisher_.publish(msg) 36 | 37 | def evaluate_input(self, neigh_data): 38 | raise NotImplementedError 39 | 40 | -------------------------------------------------------------------------------- /crazychoir_examples/crazychoir_examples/aggregative_webots/guidance_aggregative.py: -------------------------------------------------------------------------------- 1 | import rclpy 2 | from crazychoir.guidance.aggregative import AggregativeGuidanceCrazyflie 3 | from choirbot.optimizer import AggregativeOptimizer 4 | from rclpy.node import Node 5 | 6 | from geometry_msgs.msg import Vector3 7 | 8 | def main(): 9 | rclpy.init() 10 | 11 | node = Node('guid_cf', allow_undeclared_parameters=True, automatically_declare_parameters_from_overrides=True) 12 | 13 | max_iter = node.get_parameter('max_iter').value 14 | 15 | opt_settings = { 16 | 'agent_dim': 2, # dimension of agent's optimization variable 17 | 'stepsize':1e-3, # stepsize for gradient tracking 18 | # 19 | 'gamma_intruder': 1.0, # weight for ||x_i - r_i||^2 20 | 'gamma_target': 1.0, # weight for ||\sigma(x) - b_i||^2 21 | 'gamma_shape': 1.0, # weight for ||x_i - \sigma(x)||^2 22 | # 23 | 'max_iterations': max_iter, 24 | 'enable_log': False, 25 | } 26 | 27 | optimizer = AggregativeOptimizer(settings=opt_settings) 28 | 29 | freq_reference = node.get_parameter('freq_reference').value 30 | agent_id = node.get_parameter('agent_id').value 31 | # height = 0.5 + agent_id*0.15 32 | height = 1.0 33 | guidance = AggregativeGuidanceCrazyflie(optimizer, height, freq_reference, 'pubsub', 'odom', None, 'position', Vector3) 34 | 35 | rclpy.spin(guidance) 36 | rclpy.shutdown() 37 | -------------------------------------------------------------------------------- /crazychoir_examples/crazychoir_examples/aggregative_webots/guidance_aggregative_moving.py: -------------------------------------------------------------------------------- 1 | import rclpy 2 | from crazychoir.guidance.aggregative import AggregativeGuidanceCrazyflie, AggregativeGuidanceCrazyflieMovingIntruder 3 | from choirbot.optimizer import AggregativeOptimizer 4 | from rclpy.node import Node 5 | 6 | from geometry_msgs.msg import Vector3 7 | 8 | def main(): 9 | rclpy.init() 10 | 11 | node = Node('guid_cf', allow_undeclared_parameters=True, automatically_declare_parameters_from_overrides=True) 12 | 13 | max_iter = node.get_parameter('max_iter').value 14 | 15 | opt_settings = { 16 | 'agent_dim': 2, # dimension of agent's optimization variable 17 | 'stepsize':5e-3, # stepsize for gradient tracking 18 | # 19 | 'gamma_intruder': 1.0, # weight for ||x_i - r_i||^2 20 | 'gamma_target': 1.0, # weight for ||\sigma(x) - b_i||^2 21 | 'gamma_shape': 1.0, # weight for ||x_i - \sigma(x)||^2 22 | # 23 | 'max_iterations': max_iter, 24 | 'enable_log': False, 25 | } 26 | 27 | optimizer = AggregativeOptimizer(settings=opt_settings) 28 | 29 | freq_reference = node.get_parameter('freq_reference').value 30 | agent_id = node.get_parameter('agent_id').value 31 | # height = 0.5 + agent_id*0.15 32 | height = 1.0 33 | guidance = AggregativeGuidanceCrazyflieMovingIntruder(optimizer, height, freq_reference, 'pubsub', 'odom', None, 'position', Vector3) 34 | 35 | rclpy.spin(guidance) 36 | rclpy.shutdown() 37 | -------------------------------------------------------------------------------- /crazychoir_examples/crazychoir_examples/aggregative_lighthouse/guidance_aggregative_moving.py: -------------------------------------------------------------------------------- 1 | import rclpy 2 | from crazychoir.guidance.aggregative import AggregativeGuidanceCrazyflie, AggregativeGuidanceCrazyflieMovingIntruder 3 | from choirbot.optimizer import AggregativeOptimizer 4 | from rclpy.node import Node 5 | 6 | from geometry_msgs.msg import Vector3 7 | 8 | def main(): 9 | rclpy.init() 10 | 11 | node = Node('guid_cf', allow_undeclared_parameters=True, automatically_declare_parameters_from_overrides=True) 12 | 13 | max_iter = node.get_parameter('max_iter').value 14 | 15 | opt_settings = { 16 | 'agent_dim': 2, # dimension of agent's optimization variable 17 | 'stepsize':5e-3, # stepsize for gradient tracking 18 | # 19 | 'gamma_intruder': 1.0, # weight for ||x_i - r_i||^2 20 | 'gamma_target': 1.0, # weight for ||\sigma(x) - b_i||^2 21 | 'gamma_shape': 1.0, # weight for ||x_i - \sigma(x)||^2 22 | # 23 | 'max_iterations': max_iter, 24 | 'enable_log': False, 25 | } 26 | 27 | optimizer = AggregativeOptimizer(settings=opt_settings) 28 | 29 | freq_reference = node.get_parameter('freq_reference').value 30 | agent_id = node.get_parameter('agent_id').value 31 | # height = 0.5 + agent_id*0.15 32 | height = 1.0 33 | guidance = AggregativeGuidanceCrazyflieMovingIntruder(optimizer, height, freq_reference, 'pubsub', 'cf_odom', None, 'position', Vector3) 34 | 35 | rclpy.spin(guidance) 36 | rclpy.shutdown() 37 | -------------------------------------------------------------------------------- /crazychoir_examples/crazychoir_examples/collision_webots/collision.py: -------------------------------------------------------------------------------- 1 | import rclpy 2 | from choirbot.collision import SingleIntegratorCollisionAvoidance 3 | from choirbot.collision.safety_filters import SingleIntegratorCBF 4 | 5 | from geometry_msgs.msg import Twist 6 | import numpy as np 7 | 8 | from rclpy.node import Node 9 | def main(): 10 | rclpy.init() 11 | 12 | 13 | collision_params = Node('collision_params', allow_undeclared_parameters=True, automatically_declare_parameters_from_overrides=True) 14 | freq = collision_params.get_parameter('freq').value 15 | 16 | node_frequency = freq 17 | 18 | safe_distance = 0.3 # m 19 | gamma = 1.0 # safety parameter 20 | max_velocity = 0.1 # [m/s] NOTE: These value is the maximum velocity for the single integrator dynamic. 21 | max_braking_velocity = 0.1 # [m/s] NOTE: These value is the maximum braking velocity for the single integrator dynamic. 22 | 23 | safety_filter = SingleIntegratorCBF( 24 | distance = safe_distance, 25 | gamma = gamma, 26 | max_velocity = max_velocity, 27 | max_braking_velocity = max_braking_velocity 28 | ) 29 | 30 | 31 | collision_avoidance = SingleIntegratorCollisionAvoidance( 32 | pose_handler='pubsub', 33 | pose_topic='odom', 34 | pose_callback=None, 35 | node_frequency=node_frequency, 36 | topic_msg=Twist, 37 | topic_name='cmd_vel', 38 | safety_filter=safety_filter 39 | ) 40 | 41 | rclpy.spin(collision_avoidance) 42 | rclpy.shutdown() 43 | -------------------------------------------------------------------------------- /crazychoir/crazychoir/planner/planner.py: -------------------------------------------------------------------------------- 1 | from choirbot.planner import PointToPointPlanner 2 | from trajectory_msgs.msg import JointTrajectory as Trajectory, JointTrajectoryPoint as TrajectoryPoint 3 | 4 | 5 | class PointToPointPlanner_2D(PointToPointPlanner): 6 | 7 | def __init__(self, goal_tolerance: float=0.05, pose_handler: str=None, pose_topic: str=None): 8 | super().__init__(goal_tolerance, pose_handler, pose_topic) 9 | 10 | self.publisher_traj_params = self.create_publisher(Trajectory, 'traj_params', 1) 11 | 12 | # Set setpoint height 13 | self.height = 0.5 + 0.15*self.agent_id # meter 14 | 15 | # Set setpoint duration 16 | self.duration = 20 # sec 17 | 18 | def send_to_controller(self): 19 | 20 | if self.current_pose.position is not None: 21 | msg = Trajectory() 22 | name = 'Agent_{}'.format(self.agent_id) 23 | 24 | point = TrajectoryPoint() 25 | x = self.goal_point[0] 26 | y = self.goal_point[1] 27 | z = self.height 28 | point.positions = [x,y,z] 29 | point.velocities = [0.0,0.0,0.0] 30 | point.accelerations = [0.0,0.0,0.0] 31 | point.effort = [0.0,0.0,0.0] 32 | point.time_from_start.sec = int(self.duration) 33 | point.time_from_start.nanosec = int(0) 34 | 35 | # publish message 36 | self.get_logger().info('Goal {} sent'.format([x,y,z])) 37 | msg.joint_names.append(name) 38 | msg.points.append(point) 39 | self.publisher_traj_params.publish(msg) -------------------------------------------------------------------------------- /crazychoir_examples/crazychoir_examples/formation_vicon/controller.py: -------------------------------------------------------------------------------- 1 | import rclpy 2 | from rclpy.node import Node 3 | from crazychoir.controller import HierarchicalController, FlatnessAccelerationCtrl, GeometryAttitudeCtrl, FlatnessPositionCtrl 4 | from crazychoir.planner import AccelerationTraj, FullStateTraj 5 | from crazychoir.crazyflie_communication import FPQRSender, FRPYSender 6 | from crazychoir.filtering import Estimator 7 | 8 | def main(): 9 | rclpy.init() 10 | 11 | controller_params = Node('controller_params', allow_undeclared_parameters=True, automatically_declare_parameters_from_overrides=True) 12 | vicon_id = controller_params.get_parameter('vicon_id').value 13 | is_leader = controller_params.get_parameter('is_leader').value 14 | 15 | frequency = 100 16 | 17 | if is_leader: 18 | position_ctrl = FlatnessPositionCtrl(update_frequency= frequency, vicon= True) 19 | desired_acceleration = FullStateTraj() 20 | else: 21 | position_ctrl = FlatnessAccelerationCtrl(update_frequency=frequency, vicon=True) 22 | desired_acceleration = AccelerationTraj() 23 | 24 | attitude_ctrl = GeometryAttitudeCtrl(update_frequency=frequency, vicon=True) 25 | sender = FPQRSender(frequency) 26 | 27 | callback = Estimator() 28 | controller = HierarchicalController(pose_handler='vicon', pose_topic='/vicon/cf{}/cf{}'.format(vicon_id,vicon_id), pose_callback=callback, 29 | position_strategy=position_ctrl, attitude_strategy=attitude_ctrl, 30 | command_sender=sender, traj_handler=desired_acceleration) 31 | 32 | callback.set_pose(controller.current_pose) 33 | 34 | controller.get_logger().info('Go!') 35 | 36 | rclpy.spin(controller) 37 | rclpy.shutdown() 38 | 39 | -------------------------------------------------------------------------------- /crazychoir/crazychoir/controller/geometric_attitude_ctrl.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from scipy.spatial.transform import Rotation as R 3 | from .hierarchical_control import AttitudeCtrlStrategy 4 | from numpy import cos, sin 5 | 6 | 7 | class GeometryAttitudeCtrl(AttitudeCtrlStrategy): 8 | 9 | def __init__(self, update_frequency, vicon= False): 10 | super().__init__(update_frequency) 11 | 12 | # Vicon flag 13 | self.vicon = vicon 14 | 15 | if self.vicon: 16 | # Vicon parameters 17 | self.K_r = np.array([ 12000, 12000, 8000])*1e-3 18 | self.K_w = np.array([ 100, 100, 80])*1e-3 19 | else: 20 | # Webots parameters 21 | self.K_r = np.array([16000, 16000, 200])*1e-3 22 | self.K_w = np.array([ 560, 560, 70])*1e-3 23 | 24 | def control(self, current_pose, desired_attitude, desired_reference): 25 | RR = R.from_quat(current_pose.orientation).as_matrix() 26 | error = 0.5*(np.dot(desired_attitude.transpose(),RR) - (np.dot(RR.transpose(),desired_attitude))) 27 | e_r = np.array([error[2,1], error[0,2], error[1,0]]) 28 | # dumping angular velocity 29 | e_w = current_pose.angular 30 | # angular velocity inputs 31 | omega = - self.K_r*e_r - self.K_w*e_w 32 | # angular rates 33 | pqr = np.dot(self.change_matrix(current_pose), omega) 34 | return pqr 35 | 36 | def change_matrix(self, current_pose): 37 | # convert angular velocities to euler rates 38 | angles = R.from_quat(current_pose.orientation).as_euler('xyz') 39 | cphi = cos(angles[0]) 40 | ctheta = cos(angles[1]) 41 | sphi = sin(angles[0]) 42 | stheta = sin(angles[1]) 43 | ttheta = stheta/ctheta 44 | Rot = np.array([[1,sphi*ttheta,cphi*ttheta],[0,cphi,-stheta],[0,sphi/ctheta,cphi/ctheta]]) 45 | return Rot 46 | 47 | 48 | 49 | -------------------------------------------------------------------------------- /crazychoir_examples/crazychoir_examples/collision_webots/controller.py: -------------------------------------------------------------------------------- 1 | import rclpy 2 | from crazychoir.controller import HierarchicalController, GeometryAttitudeCtrl, FlatnessPositionCtrl, SafeFlatnessPositionCtrl, SafeHierarchicalController 3 | from crazychoir.planner import FullStateTraj 4 | from crazychoir.crazyflie_communication import FPQRSender 5 | 6 | from choirbot.collision.safety_filters import DoubleIntegratorCBF 7 | 8 | from rclpy.node import Node 9 | 10 | import time 11 | 12 | def main(): 13 | rclpy.init() 14 | 15 | 16 | planner_params = Node('controller_params', allow_undeclared_parameters=True, automatically_declare_parameters_from_overrides=True) 17 | agent_id = planner_params.get_parameter('agent_id').value 18 | 19 | 20 | frequency = 100 21 | # position_ctrl = FlatnessPositionCtrl(frequency) 22 | 23 | safe_distance = 0.2 # m 24 | gamma = 5.0 # safety parameter: The lower, the safer 25 | kappa = 1.0 # safety parameter {1,2}: 26 | max_acceleration = 0.005 # m/s^2 27 | max_braking_acceleration = 0.005 # m/s^2 28 | 29 | safety_filter = DoubleIntegratorCBF( 30 | distance = safe_distance, 31 | gamma = gamma, 32 | kappa = kappa, 33 | max_acceleration = max_acceleration, 34 | max_braking_acceleration = max_braking_acceleration, # m/s^2 35 | ) 36 | 37 | 38 | position_ctrl = SafeFlatnessPositionCtrl(agent_id, frequency, safety_filter = safety_filter, max_acceleration=max_acceleration) 39 | desired_traj = FullStateTraj() 40 | attitude_ctrl = GeometryAttitudeCtrl(frequency) 41 | sender = FPQRSender(frequency) 42 | 43 | controller = SafeHierarchicalController(pose_handler='pubsub', pose_topic='odom', pose_callback=None, 44 | position_strategy=position_ctrl, attitude_strategy=attitude_ctrl, 45 | command_sender=sender, traj_handler=desired_traj) 46 | 47 | controller.get_logger().info('Go!') 48 | 49 | rclpy.spin(controller) 50 | rclpy.shutdown() 51 | 52 | -------------------------------------------------------------------------------- /crazychoir_examples/setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import setup 2 | from glob import glob 3 | 4 | 5 | package_name = 'crazychoir_examples' 6 | scripts = { 7 | 'collision_webots': ['collision','controller','controller_vel','closest_robot_getter','gui','guidance','trajectory','trajectory_vel'], 8 | 'formation_rviz': ['controller','guidance','integrator','rviz'], 9 | 'formation_vicon': ['controller','gui','guidance','radio','trajectory'], 10 | 'formation_webots': ['controller_leaders','controller_followers','gui','guidance','trajectory'], 11 | 'goto_webots': ['controller','guidance','gui','trajectory'], 12 | 'task_assignment_webots': ['guidance','simple_guidance', 'gui','planner','table'], 13 | 'task_assignment_vicon': ['guidance','simple_guidance', 'gui','planner','table','radio'], 14 | 'tracking_vicon': ['controller','guidance','gui','radio','trajectory'], 15 | 'tracking_webots': ['controller','guidance','gui','trajectory'], 16 | 'aggregative_lighthouse': ['controller','gui','guidance_aggregative','guidance_aggregative_moving','guidance_intruder', 'simple_guidance','trajectory','radio'], 17 | 'aggregative_webots': ['controller','gui','guidance_aggregative','guidance_aggregative_moving','guidance_intruder', 'simple_guidance','trajectory'], 18 | } 19 | setup( 20 | name=package_name, 21 | version='0.0.0', 22 | packages=[package_name], 23 | data_files=[ 24 | ('share/ament_index/resource_index/packages', 25 | ['resource/' + package_name]), 26 | ('share/' + package_name, ['package.xml']), 27 | ('share/' + package_name, glob('launch/*.launch.py')), 28 | ('share/' + package_name, glob('resource/*.rviz')), 29 | ('share/' + package_name, glob('resource/*.urdf')), 30 | ('share/' + package_name + '/worlds', glob('worlds/*.wbt')), 31 | ('share/' + package_name + '/worlds/meshes', glob('worlds/meshes/*.stl')), 32 | ], 33 | install_requires=['setuptools','crazychoir'], 34 | zip_safe=True, 35 | maintainer='opt4smart', 36 | maintainer_email='opt4smart@todo.todo', 37 | description='TODO: Package description', 38 | license='TODO: License declaration', 39 | tests_require=['pytest'], 40 | entry_points={ 41 | 'console_scripts': [ 42 | 'crazychoir_{0}_{1} = crazychoir_examples.{0}.{1}:main'.format(package, file) 43 | for package, files in scripts.items() for file in files 44 | ], 45 | }, 46 | ) 47 | -------------------------------------------------------------------------------- /crazychoir/crazychoir/gui/gui/rosWidget.py: -------------------------------------------------------------------------------- 1 | from PyQt5.QtCore import QDir, Qt 2 | from PyQt5.QtGui import QIcon 3 | from PyQt5.QtWidgets import QWidget, QVBoxLayout, QPushButton, QHBoxLayout, QLabel, QLineEdit 4 | 5 | 6 | class RosWidget(QWidget): 7 | 8 | def __init__(self): 9 | super().__init__() 10 | 11 | layout = QVBoxLayout() 12 | layout.setAlignment(Qt.AlignCenter) 13 | layout.setSpacing(10) 14 | 15 | main_toolbar = QHBoxLayout() 16 | second_toolbar = QHBoxLayout() 17 | 18 | self.bttn_start = QPushButton("START", self) 19 | self.bttn_take_off = QPushButton(QIcon(QDir.currentPath() + "/gui/res/icons/drone.png"), "TAKE-OFF", self) 20 | self.bttn_land = QPushButton("LAND", self) 21 | self.bttn_stop = QPushButton("STOP", self) 22 | self.bttn_plot = QPushButton("PLOT", self) 23 | 24 | main_toolbar.addWidget(self.bttn_take_off) 25 | main_toolbar.addWidget(self.bttn_land) 26 | main_toolbar.addWidget(self.bttn_stop) 27 | second_toolbar.addWidget(self.bttn_start) 28 | second_toolbar.addWidget(self.bttn_plot) 29 | 30 | layout.addLayout(main_toolbar) 31 | layout.addLayout(second_toolbar) 32 | layout.addSpacing(50) 33 | 34 | # Direct command tool 35 | layout.addWidget(QLabel("Send direct command")) 36 | 37 | row = QHBoxLayout() 38 | row.addWidget(QLabel("ID:")) 39 | self.direct_id = QLineEdit() 40 | row.addWidget(self.direct_id) 41 | layout.addLayout(row) 42 | 43 | row = QHBoxLayout() 44 | row.addWidget(QLabel("x:")) 45 | self.direct_x = QLineEdit() 46 | row.addWidget(self.direct_x) 47 | layout.addLayout(row) 48 | 49 | row = QHBoxLayout() 50 | row.addWidget(QLabel("y:")) 51 | self.direct_y = QLineEdit() 52 | row.addWidget(self.direct_y) 53 | layout.addLayout(row) 54 | 55 | row = QHBoxLayout() 56 | row.addWidget(QLabel("z:")) 57 | self.direct_z = QLineEdit() 58 | row.addWidget(self.direct_z) 59 | layout.addLayout(row) 60 | 61 | row = QHBoxLayout() 62 | row.addWidget(QLabel("time (seconds):")) 63 | self.direct_t = QLineEdit() 64 | row.addWidget(self.direct_t) 65 | layout.addLayout(row) 66 | 67 | self.bttn_direct = QPushButton("SEND DIRECT COMMAND") 68 | layout.addWidget(self.bttn_direct) 69 | 70 | self.setLayout(layout) -------------------------------------------------------------------------------- /crazychoir/crazychoir/radio_handler/radio_handler_vel_lighthouse.py: -------------------------------------------------------------------------------- 1 | from .radio_handler_vel import RadioHandlerVel 2 | import time 3 | from cflib.crazyflie.log import LogConfig 4 | from nav_msgs.msg import Odometry 5 | 6 | class RadioHandlerVelLightHouse(RadioHandlerVel): 7 | 8 | def __init__(self): 9 | super().__init__() 10 | print('Radio Vel Lighthouse Started!') 11 | 12 | def set_log(self, uri, cf, log_period_in_ms, agent_id): 13 | """ 14 | Logging groups and variables retrived from 15 | https://www.bitcraze.io/documentation/repository/crazyflie-firmware/master/api/logs/ 16 | """ 17 | 18 | # The definition of the logconfig can be made before connecting 19 | self._lg_stab = LogConfig(name='Stabilizer', period_in_ms=log_period_in_ms) 20 | self._lg_stab.add_variable('kalman.stateX', 'float') 21 | self._lg_stab.add_variable('kalman.stateY', 'float') 22 | self._lg_stab.add_variable('kalman.stateZ', 'float') 23 | 24 | # The fetch-as argument can be set to FP16 to save space in the log packet 25 | # self._lg_stab.add_variable('pm.vbat', 'FP16') 26 | 27 | 28 | try: 29 | cf.log.add_config(self._lg_stab) 30 | # This callback will receive the data 31 | self._lg_stab.data_received_cb.add_callback(lambda timestamp, data, logconf: self._stab_log_data(timestamp, data, logconf, agent_id)) 32 | # This callback will be called on errors 33 | self._lg_stab.error_cb.add_callback(self._stab_log_error) 34 | # Start the logging 35 | self._lg_stab.start() 36 | except KeyError as e: 37 | print('Could not start log configuration,' 38 | '{} not found in TOC'.format(str(e))) 39 | except AttributeError: 40 | print('Could not add Stabilizer log config, bad configuration.') 41 | 42 | 43 | def _stab_log_data(self, timestamp, data, logconf, agent_id): 44 | odom = Odometry() 45 | odom.pose.pose.position.x = data['kalman.stateX'] 46 | odom.pose.pose.position.y = data['kalman.stateY'] 47 | odom.pose.pose.position.z = data['kalman.stateZ'] 48 | odom.twist.twist.linear.x = 0.0 49 | odom.twist.twist.linear.y = 0.0 50 | odom.twist.twist.linear.z = 0.0 51 | odom.pose.pose.orientation.x = 0.0 52 | odom.pose.pose.orientation.y = 0.0 53 | odom.pose.pose.orientation.z = 0.0 54 | odom.pose.pose.orientation.w = 1.0 55 | odom.twist.twist.angular.x = 0.0 56 | odom.twist.twist.angular.y = 0.0 57 | odom.twist.twist.angular.z = 0.0 58 | self.pub_odom[agent_id].publish(odom) 59 | -------------------------------------------------------------------------------- /crazychoir/crazychoir/radio_handler/radio_handler_xyz_lighthouse.py: -------------------------------------------------------------------------------- 1 | from .radio_handler_xyz import RadioHandlerXYZ 2 | import time 3 | from cflib.crazyflie.log import LogConfig 4 | from nav_msgs.msg import Odometry 5 | 6 | class RadioHandlerXYZLightHouse(RadioHandlerXYZ): 7 | 8 | def __init__(self): 9 | super().__init__() 10 | print('Radio XYZ Lighthouse Started!') 11 | 12 | def set_log(self, uri, cf, log_period_in_ms, agent_id): 13 | """ 14 | Logging groups and variables retrived from 15 | https://www.bitcraze.io/documentation/repository/crazyflie-firmware/master/api/logs/ 16 | """ 17 | 18 | # The definition of the logconfig can be made before connecting 19 | self._lg_stab = LogConfig(name='Stabilizer', period_in_ms=log_period_in_ms) 20 | self._lg_stab.add_variable('kalman.stateX', 'float') 21 | self._lg_stab.add_variable('kalman.stateY', 'float') 22 | self._lg_stab.add_variable('kalman.stateZ', 'float') 23 | 24 | # The fetch-as argument can be set to FP16 to save space in the log packet 25 | # self._lg_stab.add_variable('pm.vbat', 'FP16') 26 | 27 | 28 | try: 29 | cf.log.add_config(self._lg_stab) 30 | # This callback will receive the data 31 | self._lg_stab.data_received_cb.add_callback(lambda timestamp, data, logconf: self._stab_log_data(timestamp, data, logconf, agent_id)) 32 | # This callback will be called on errors 33 | self._lg_stab.error_cb.add_callback(self._stab_log_error) 34 | # Start the logging 35 | self._lg_stab.start() 36 | except KeyError as e: 37 | print('Could not start log configuration,' 38 | '{} not found in TOC'.format(str(e))) 39 | except AttributeError: 40 | print('Could not add Stabilizer log config, bad configuration.') 41 | 42 | 43 | def _stab_log_data(self, timestamp, data, logconf, agent_id): 44 | print('Send ODOM') 45 | odom = Odometry() 46 | odom.pose.pose.position.x = data['kalman.stateX'] 47 | odom.pose.pose.position.y = data['kalman.stateY'] 48 | odom.pose.pose.position.z = data['kalman.stateZ'] 49 | odom.twist.twist.linear.x = 0.0 50 | odom.twist.twist.linear.y = 0.0 51 | odom.twist.twist.linear.z = 0.0 52 | odom.pose.pose.orientation.x = 0.0 53 | odom.pose.pose.orientation.y = 0.0 54 | odom.pose.pose.orientation.z = 0.0 55 | odom.pose.pose.orientation.w = 1.0 56 | odom.twist.twist.angular.x = 0.0 57 | odom.twist.twist.angular.y = 0.0 58 | odom.twist.twist.angular.z = 0.0 59 | self.pub_odom[agent_id].publish(odom) 60 | -------------------------------------------------------------------------------- /crazychoir/crazychoir/webots_plugin/real_pose_ctrl.py: -------------------------------------------------------------------------------- 1 | import rclpy 2 | 3 | from choirbot import Pose 4 | from choirbot.utils.position_getter import pose_subscribe 5 | 6 | 7 | class RealPoseCtrl: 8 | def init(self, webots_node, properties): 9 | 10 | # Declare the robot name and fix the timestep 11 | self.robot = webots_node.robot 12 | self.timestep = int(self.robot.getBasicTimeStep()) 13 | self.robot_name = self.robot.getName() 14 | self.gps = self.robot.getDevice("gps") 15 | self.gps.enable(self.timestep) 16 | 17 | # Initialize webots driver node 18 | rclpy.init(args=None) 19 | self.namespace = str(self.robot_name) 20 | self.intruder_id = int(self.robot_name[-1]) 21 | self.intruder_driver = rclpy.create_node( 22 | 'real_pose_driver', 23 | namespace=self.namespace, 24 | allow_undeclared_parameters=True, 25 | automatically_declare_parameters_from_overrides=True) 26 | 27 | self.robot.root_node_ref = self.robot.getSelf() 28 | self.robot.root_translation_field = self.robot.root_node_ref.getField("translation") 29 | self.robot.root_rotation_field = self.robot.root_node_ref.getField("rotation") 30 | 31 | self.real_pose = Pose(None, None, None, None) 32 | 33 | def step(self): 34 | rclpy.spin_once(self.intruder_driver, timeout_sec=0) 35 | 36 | if self.real_pose.position is None: 37 | return 38 | 39 | position_list = list(self.real_pose.position) 40 | 41 | self.robot.root_translation_field.setSFVec3f(position_list) 42 | 43 | class RealPoseCtrlLighthouse(RealPoseCtrl): 44 | def init(self, webots_node, properties): 45 | super().init(webots_node, properties) 46 | 47 | print('RealPoseCtrlLightHouse Started!') 48 | 49 | pose_handler = 'pubsub' 50 | pose_topic = f'/agent_{self.robot_name[-1]}/cf_odom' 51 | pose_callback = None 52 | 53 | self.subscription = pose_subscribe(pose_handler, pose_topic, self.intruder_driver, self.real_pose, pose_callback) 54 | 55 | 56 | from crazychoir.filtering import Estimator 57 | 58 | class RealPoseCtrlVicon(RealPoseCtrl): 59 | def init(self, webots_node, properties): 60 | super().init(webots_node, properties) 61 | 62 | print('RealPoseCtrlVicon Started!') 63 | 64 | pose_handler = 'vicon' 65 | pose_topic = f'/vicon/cf{self.robot_name[-1]}/cf{self.robot_name[-1]}' 66 | pose_callback = Estimator() 67 | 68 | self.subscription = pose_subscribe(pose_handler, pose_topic, self.intruder_driver, self.real_pose, pose_callback) 69 | pose_callback.set_pose(self.real_pose) 70 | 71 | 72 | 73 | -------------------------------------------------------------------------------- /crazychoir/crazychoir/radio_handler/radio_handler_fpqr.py: -------------------------------------------------------------------------------- 1 | import time 2 | from math import degrees 3 | import numpy as np 4 | from .radio_handler import RadioHandler 5 | 6 | class RadioHandlerFPQR(RadioHandler): 7 | 8 | def __init__(self): 9 | super().__init__() 10 | 11 | print('Radio FPQR Started!') 12 | 13 | # Conversion factors 14 | self.mg_newton = 0.39 15 | self.mg_pwm = 42000 16 | self.newton2pwm = self.mg_pwm/self.mg_newton 17 | 18 | # cmd limits 19 | self.thrust_lim_max = 60000 20 | self.thrust_lim_min = 10001 21 | self.pq_lim = 720 22 | self.r_lim = 450 23 | 24 | # takeoff permission 25 | self.takeoff = False 26 | 27 | 28 | def cmd_sender(self, msg, uri, cf): 29 | 30 | # TODO: do something during cmd_vel initialization -> cf needs a cmd every 1s 31 | 32 | # Check safety area 33 | self.check_safety_area() 34 | 35 | cmd_thrust = np.clip(int(msg.linear.z*self.newton2pwm),self.thrust_lim_min, self.thrust_lim_max) # uint 36 | cmd_roll = np.clip(degrees( msg.angular.x) ,-self.pq_lim, self.pq_lim) #deg/s 37 | cmd_pitch = np.clip(degrees( msg.angular.y) ,-self.pq_lim, self.pq_lim) #no minus 38 | cmd_yaw = np.clip(degrees(-msg.angular.z) ,-self.r_lim, self.r_lim) 39 | 40 | if self.emergency_stop or not self.takeoff: 41 | cmd_thrust = 0 42 | cmd_roll = 0.0 43 | cmd_pitch = 0.0 44 | cmd_yaw = 0.0 45 | 46 | # string = "Commands: p={:.4f} q={:.4f} r={:.4f} thrust = {}".format(cmd_roll, cmd_pitch, cmd_yaw, cmd_thrust) 47 | # print(string) 48 | 49 | cf.commander.send_setpoint(cmd_roll, cmd_pitch, cmd_yaw, cmd_thrust) 50 | 51 | def cmd_stop(self, _): 52 | self.emergency_stop = True 53 | 54 | def cmd_take_off(self, _): 55 | self.takeoff = True 56 | 57 | def set_values(self,uri, cf): 58 | # cf.param.set_value('commander.enHighLevel', 0) -> default 0 59 | # cf.param.set_value('stabilizer.controller', 1) # PID(1), Mellinger(2) -> default 1 60 | # cf.param.set_value('stabilizer.estimator', 1) # Complementary(1), Kalman(2) -> default 1 61 | # cf.param.set_value('flightmode.yawMode',2) # plus_mode(1), x_mode(2) -> default 1 62 | cf.param.set_value('flightmode.stabModeRoll',0) # rate(0) angle(1) -> default 1 63 | cf.param.set_value('flightmode.stabModePitch', 0) # rate(0) angle(1) -> default 1 64 | # cf.param.set_value('flightmode.stabModeYaw', 0) # rate(0) angle(1) -> default 0 65 | 66 | print("[{}] \t cf{} - parameters set!".format(time.time(), int(uri[-1]))) 67 | -------------------------------------------------------------------------------- /crazychoir/crazychoir/radio_handler/radio_handler_frpy.py: -------------------------------------------------------------------------------- 1 | import time 2 | from math import degrees 3 | import numpy as np 4 | from .radio_handler import RadioHandler 5 | 6 | class RadioHandlerFRPY(RadioHandler): 7 | 8 | def __init__(self): 9 | super().__init__() 10 | 11 | print('Radio FRPY Started!') 12 | 13 | # Conversion factors 14 | self.mg_newton = 0.39 15 | self.mg_pwm = 42000 16 | self.newton2pwm = self.mg_pwm/self.mg_newton 17 | 18 | # cmd limits 19 | self.thrust_lim_max = 60000 20 | self.thrust_lim_min = 10001 21 | self.rp_lim = 720 22 | self.y_lim = 450 23 | 24 | # takeoff permission 25 | self.takeoff = False 26 | 27 | 28 | def cmd_sender(self, msg, uri, cf): 29 | 30 | # TODO: do something during cmd_vel initialization -> cf needs a cmd every 1s 31 | 32 | # Check safety area 33 | self.check_safety_area() 34 | 35 | 36 | cmd_thrust = np.clip(int(msg.linear.z*self.newton2pwm), self.thrust_lim_min, self.thrust_lim_max) # uint 37 | cmd_roll = np.clip(degrees( msg.angular.x) , -self.rp_lim, self.rp_lim) #deg/s 38 | cmd_pitch = np.clip(degrees( msg.angular.y) , -self.rp_lim, self.rp_lim) #no minus 39 | cmd_yaw = np.clip(degrees(-msg.angular.z) , -self.y_lim , self.y_lim) 40 | 41 | if self.emergency_stop or not self.takeoff: 42 | cmd_thrust = 0 43 | cmd_roll = 0.0 44 | cmd_pitch = 0.0 45 | cmd_yaw = 0.0 46 | 47 | # string = "Commands: p={:.4f} q={:.4f} r={:.4f} thrust = {}".format(cmd_roll, cmd_pitch, cmd_yaw, cmd_thrust) 48 | # print(string) 49 | 50 | cf.commander.send_setpoint(cmd_roll, cmd_pitch, cmd_yaw, cmd_thrust) 51 | 52 | def cmd_stop(self, _): 53 | self.emergency_stop = True 54 | 55 | def cmd_take_off(self, _): 56 | self.takeoff = True 57 | 58 | def set_values(self,uri, cf): 59 | # cf.param.set_value('commander.enHighLevel', 0) -> default 0 60 | # cf.param.set_value('stabilizer.controller', 1) # PID(1), Mellinger(2) -> default 1 61 | # cf.param.set_value('stabilizer.estimator', 1) # Complementary(1), Kalman(2) -> default 1 62 | # cf.param.set_value('flightmode.yawMode',2) # plus_mode(1), x_mode(2) -> default 1 63 | # cf.param.set_value('flightmode.stabModeRoll',1) # rate(0) angle(1) -> default 1 64 | # cf.param.set_value('flightmode.stabModePitch', 1) # rate(0) angle(1) -> default 1 65 | cf.param.set_value('flightmode.stabModeYaw', 1) # rate(0) angle(1) -> default 0 66 | 67 | print("[{}] \t cf{} - parameters setted!".format(time.time(), int(uri[-1]))) 68 | -------------------------------------------------------------------------------- /crazychoir/crazychoir/integrator/crazyflie_integrator.py: -------------------------------------------------------------------------------- 1 | from choirbot.integrator import Integrator 2 | from scipy.integrate import solve_ivp 3 | from geometry_msgs.msg import Twist 4 | from scipy.spatial.transform import Rotation as R 5 | from scipy.constants import g, pi 6 | import numpy as np 7 | from numpy import cos, sin 8 | 9 | 10 | class CrazyflieIntegrator(Integrator): 11 | 12 | def __init__(self, integration_freq: float, odom_freq: float=None): 13 | super().__init__(integration_freq, odom_freq) 14 | 15 | # create input subscription 16 | self.u = np.zeros(4) 17 | self.current_vel = np.zeros(3) 18 | self.current_ang_vel = np.zeros(3) 19 | self.subscription = self.create_subscription(Twist, 'cmd_vel', self.input_callback, 1) 20 | 21 | self.mass = 0.038 22 | self.dt = 1.0/integration_freq 23 | 24 | self.state = np.zeros(9) 25 | self.state[0:3] = np.copy(self.current_pos) 26 | self.state[3:6] = R.from_quat(self.current_or).as_euler('xyz') 27 | 28 | self.get_logger().info('Integrator {} started'.format(self.agent_id)) 29 | 30 | # initialize input to hover 31 | self.u = np.array([self.mass*g,0.0,0.0,0.0]) 32 | 33 | def input_callback(self, msg): 34 | # save received input 35 | self.u = np.array([msg.linear.z, msg.angular.x, msg.angular.y, msg.angular.z]) 36 | 37 | def integrate(self): 38 | sol = solve_ivp(lambda t, v: self.state_dot(), [0, self.dt], self.state) 39 | self.state = sol.y[:,-1] 40 | self.state[3:6] = self.wrap_angle(self.state[3:6]) 41 | 42 | self.current_pos = np.copy(self.state[0:3]) 43 | self.current_or = R.from_euler('xyz',self.state[3:6]).as_quat() 44 | self.current_vel = np.copy(self.state[6:9]) 45 | 46 | self.current_ang_vel = np.dot(np.linalg.inv(self.change_matrix()), self.u[1:4]) 47 | 48 | def state_dot(self): 49 | # State space representation: [x y z phi theta psi x_dot y_dot z_dot phi_dot theta_dot psi_dot] 50 | # TODO add drag 51 | RR = R.from_euler('xyz', self.state[3:6]).as_matrix() 52 | state_dot = np.zeros(9) 53 | state_dot[0:3] = np.copy(self.state[6:9]) 54 | state_dot[3:6] = np.copy(self.u[1:4]) 55 | state_dot[6:9] = -np.array([0,0,g]) + np.dot(RR, np.array([0,0,self.u[0]]))/self.mass 56 | return state_dot 57 | 58 | def wrap_angle(self, val): 59 | return( ( val + pi) % (2 * pi ) - pi ) 60 | 61 | def change_matrix(self): 62 | # convert angular velocities to euler rates 63 | angles = np.copy(self.state[3:6]) 64 | cphi = cos(angles[0]) 65 | ctheta = cos(angles[1]) 66 | sphi = sin(angles[0]) 67 | stheta = sin(angles[1]) 68 | ttheta = stheta/ctheta 69 | R = np.array([[1,sphi*ttheta,cphi*ttheta],[0,cphi,-stheta],[0,sphi/ctheta,cphi/ctheta]]) 70 | return R -------------------------------------------------------------------------------- /crazychoir/crazychoir/radio_handler/radio_handler_xyz.py: -------------------------------------------------------------------------------- 1 | import time 2 | from .radio_handler import RadioHandler 3 | 4 | class RadioHandlerXYZ(RadioHandler): 5 | 6 | def __init__(self): 7 | super().__init__() 8 | print('Radio XYZ Started!') 9 | 10 | def cmd_sender(self, msg, uri, cf): 11 | 12 | # Check safety area 13 | self.check_safety_area() 14 | 15 | if self.emergency_stop: 16 | print("[{}] \t cf{} - STOP PUSHED".format(time.time(), int(uri[-1]))) 17 | return 18 | 19 | # TODO: do something during cmd_vel initialization -> cf needs a cmd every 1s 20 | 21 | setpoint = msg.points[-1] 22 | x = setpoint.positions[0] 23 | y = setpoint.positions[1] 24 | z = setpoint.positions[2] 25 | yaw = 0.0 26 | duration = float(setpoint.time_from_start.sec + setpoint.time_from_start.nanosec*1e-9) 27 | relative = False 28 | 29 | # string = "[{}] \t cf{} goto: x={:.4f} y={:.4f} z={:.4f} yaw = {:.4f}".format(time.time(), int(uri[-1]), x, y, z, yaw) 30 | # print(string) 31 | cf.high_level_commander.go_to(x, y, z, yaw, duration, relative) 32 | 33 | def cmd_take_off(self, _): 34 | pass 35 | 36 | def cmd_stop(self, _): 37 | self.emergency_stop = True 38 | for uri, scf in self.swarm._cfs.items(): 39 | cf = scf.cf 40 | cf.high_level_commander.stop() 41 | print("[{}] \t cf{} - STOP!".format(time.time(), int(uri[-1]))) 42 | 43 | 44 | def set_values(self,uri, cf): 45 | cf.param.set_value('commander.enHighLevel', 1) # -> default 0 46 | # cf.param.set_value('stabilizer.controller', 1) # PID(1), Mellinger(2) -> default 1 47 | cf.param.set_value('stabilizer.estimator', 2) # Complementary(1), Kalman(2) -> default 1 48 | # cf.param.set_value('flightmode.yawMode',2) # plus_mode(1), x_mode(2) -> default 1 49 | # cf.param.set_value('flightmode.stabModeRoll',0) # rate(0) angle(1) -> default 1 50 | # cf.param.set_value('flightmode.stabModePitch', 0) # rate(0) angle(1) -> default 1 51 | # cf.param.set_value('flightmode.stabModeYaw', 0) # rate(0) angle(1) -> default 0 52 | 53 | # Set the std deviation for the quaternion data pushed into the 54 | # kalman filter. The default value seems to be a bit too low. 55 | cf.param.set_value('locSrv.extQuatStdDev', 0.06) 56 | 57 | print("[{}] \t cf{} - parameters setted!".format(time.time(), int(uri[-1]))) 58 | 59 | time.sleep(0.1) 60 | cf.param.set_value('kalman.resetEstimation', '1') 61 | time.sleep(0.1) 62 | cf.param.set_value('kalman.resetEstimation', '0') 63 | 64 | print("[{}] \t cf{} - kalman filter reset done!".format(time.time(), int(uri[-1]))) 65 | 66 | -------------------------------------------------------------------------------- /crazychoir/crazychoir/guidance/goto_guidance.py: -------------------------------------------------------------------------------- 1 | from typing import Callable 2 | import numpy as np 3 | 4 | from rclpy.node import Node 5 | from choirbot import Pose 6 | from choirbot.utils.position_getter import pose_subscribe 7 | 8 | from std_msgs.msg import Empty 9 | from trajectory_msgs.msg import JointTrajectory as Trajectory, JointTrajectoryPoint as TrajectoryPoint 10 | 11 | from time import sleep 12 | 13 | class GoToGuidance(Node): 14 | 15 | def __init__(self, pose_handler: str=None, pose_topic: str=None, pose_callback: Callable=None, takeoff_time: float=5.0): 16 | 17 | super().__init__('goto_guidance', allow_undeclared_parameters=True, 18 | automatically_declare_parameters_from_overrides=True) 19 | 20 | # get parameters 21 | self.agent_id = self.get_parameter('agent_id').value 22 | self.N = self.get_parameter('N').value 23 | self.init_pos = self.get_parameter('init_pos').value 24 | 25 | # list of goals: [time_0, x_0, y_0, z_0, time_1, x_1, y_1, z_1, ...] 26 | self.goals = self.get_parameter('goals').value 27 | self.goals = [self.goals[i:i+4] for i in range(0, len(self.goals), 4)] 28 | 29 | # takeoff time 30 | self.takeoff_time = takeoff_time 31 | 32 | # initialize pose subscription 33 | self.current_pose = Pose(None, None, None, None) 34 | self.subscription = pose_subscribe(pose_handler, pose_topic, self, self.current_pose, pose_callback) 35 | 36 | # button subscription 37 | self.experiment_trigger_subscription = self.create_subscription(Empty, '/experiment_trigger', self.execute_goal_list, 10) 38 | 39 | # subscription to trajectory topic 40 | self.publishers_traj_params = self.create_publisher(Trajectory, 'traj_params', 1) 41 | 42 | self.get_logger().info('GoToGuidance {} started'.format(self.agent_id)) 43 | 44 | def execute_goal_list(self, _): 45 | self.get_logger().info('Starting goto') 46 | 47 | for goal in self.goals: 48 | goal_time = goal[0] 49 | goal_pos = goal[1:] 50 | self.goto(goal_time, goal_pos) 51 | print(f'agent {self.agent_id} going to {goal_pos}. Wait {goal[0]}') 52 | sleep(goal_time) 53 | 54 | def goto(self, goal_time, goal_pos): 55 | msg = Trajectory() 56 | name = 'Agent_{}'.format(self.agent_id) 57 | 58 | point = TrajectoryPoint() 59 | x = goal_pos[0] 60 | y = goal_pos[1] 61 | z = goal_pos[2] 62 | duration_s = goal_time 63 | point.positions = [x,y,z] 64 | point.velocities = [0.0,0.0,0.0] 65 | point.accelerations = [0.0,0.0,0.0] 66 | point.effort = [0.0,0.0,0.0] 67 | point.time_from_start.sec = int(duration_s) 68 | point.time_from_start.nanosec = int(0) 69 | 70 | msg.joint_names.append(name) 71 | msg.points.append(point) 72 | 73 | self.publishers_traj_params.publish(msg) -------------------------------------------------------------------------------- /crazychoir_examples/launch/tracking_vicon.launch.py: -------------------------------------------------------------------------------- 1 | from launch import LaunchDescription 2 | from launch_ros.actions import Node 3 | from launch.actions import TimerAction 4 | 5 | node_frequency = 100 # [Hz] 6 | 7 | def generate_launch_description(): 8 | 9 | # number of agents 10 | N = 1 11 | 12 | # Set uri address 13 | uri = 'radio://0/80/2M/E7E7E7E701' 14 | 15 | initial_position = [0.0, 0.0, 0.015] 16 | 17 | # initialize launch description 18 | launch_description = [] 19 | radio_launch = [] 20 | 21 | # Launch control Panel 22 | launch_description.append(Node( 23 | package='crazychoir_examples', 24 | executable='crazychoir_tracking_vicon_gui', 25 | output='screen', 26 | parameters=[{ 27 | 'n_agents': 1, 28 | }])) 29 | 30 | # Launch radio node 31 | radio_id = int(uri.split('/')[2]) 32 | radio_launch.append(Node( 33 | package='crazychoir_examples', 34 | executable='crazychoir_tracking_vicon_radio', 35 | output='screen', 36 | prefix='xterm -title "radio_{}" -hold -e'.format(radio_id), 37 | namespace='radio_{}'.format(radio_id), 38 | parameters=[{ 39 | 'uris': [uri], 40 | 'agent_ids': [0], 41 | 'cmd_topic': 'cmd_vel', 42 | }])) 43 | 44 | # Launch vicon node 45 | launch_description.append(Node( 46 | package='vicon_receiver', 47 | executable='vicon_client', 48 | output='screen', 49 | parameters=[{ 50 | 'hostname': '192.168.10.1', 51 | 'buffer_size': 200, 52 | 'namespace': 'vicon'}] 53 | )) 54 | 55 | # controller 56 | launch_description.append(Node( 57 | package='crazychoir_examples', 58 | executable='crazychoir_tracking_vicon_controller', 59 | namespace='agent_0', 60 | output='screen', 61 | parameters=[{ 62 | 'agent_id': 0, 63 | }])) 64 | 65 | # guidance 66 | launch_description.append(Node( 67 | package='crazychoir_examples', 68 | executable='crazychoir_tracking_vicon_guidance', 69 | namespace='agent_0', 70 | output='screen', 71 | parameters=[{ 72 | 'agent_id': 0, 73 | 'init_pos': initial_position, 74 | }])) 75 | 76 | 77 | # reference 78 | launch_description.append(Node( 79 | package='crazychoir_examples', 80 | executable='crazychoir_tracking_vicon_trajectory', 81 | namespace='agent_0', 82 | output='screen', 83 | parameters=[{ 84 | 'agent_id': 0, 85 | }])) 86 | 87 | # include delayed radio launcher 88 | timer_action = TimerAction(period=5.0, actions=[LaunchDescription(radio_launch)]) 89 | launch_description.append(timer_action) 90 | 91 | return LaunchDescription(launch_description) 92 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # CrazyChoir 2 | [**Website**](https://opt4smart.github.io/crazychoir/) 3 | | [**Installation**](https://opt4smart.github.io/crazychoir/installation) 4 | 5 | 6 | | :warning: Information for end users | 7 | |:------------------------------------| 8 | | Documentation pages are currently being uploaded. The latest version will be available soon. | 9 | | If you need urgent support, please contact [Lorenzo](https://github.com/lorenzopichierri)! | 10 | 11 | |🚨 **NEWS** 🚨| 12 | |:-------------| 13 | |**DOCKER** installer is now available [here](https://github.com/OPT4SMART/crazychoir/tree/master/docker)! | 14 | 15 | 16 | **CrazyChoir** is a ROS 2 toolbox allowing users to run simulations and experiments on swarms of Crazyflie nano-quadrotors. 17 | **CrazyChoir** implements several tools to model swarms of Crazyflie nano-quadrotors and to run distributed, complex tasks both in simulation and experiments. Examples include task assignment, formation control, and trajectory tracking settings. 18 | 19 | ## Requirements and Installation 20 | **CrazyChoir** requires ROS 2 Foxy to be installed on your system. 21 | 22 | Some preliminary steps are required to complete the installation. We refer the reader to the [installation guide](https://opt4smart.github.io/crazychoir/installation). 23 | 24 | ## Examples 25 | To check the installation and start to use **CrazyChoir**, you can run 26 | 27 | ``` 28 | ros2 launch crazychoir_examples formation_webots.launch.py 29 | ``` 30 | 31 | This will start a Webots simulation with 4 Crazyflie nano-quadrotor performing a leader-follower formation control task. A video of the simulation is available here: 32 | 33 | [![Alternate Text](http://img.youtube.com/vi/bOM_JnBsJ48/0.jpg)](https://youtu.be/bOM_JnBsJ48) 34 | 35 | >**Note:** The example was executed on a Dell XPS 15 9570 equipped with an Intel i7-7700HQ CPU and an NVIDIA GeForce GTX 1050 GPU. 36 | 37 | You can also run more advanced examples, such as the letter formation task, or the task assignment experiment. For more information, please refer to the [examples](https://opt4smart.github.io/crazychoir/video) page. 38 | 39 | ## Citing **CrazyChoir** 40 | If you are using **CrazyChoir** in research work to be published, please cite the accompanying paper 41 | 42 | ``` 43 | @ARTICLE{pichierri2023crazychoir, 44 | title={CrazyChoir: Flying Swarms of Crazyflie Quadrotors in ROS 2}, 45 | author={Pichierri, Lorenzo and Testa, Andrea and Notarstefano, Giuseppe}, 46 | journal={IEEE Robotics and Automation Letters}, 47 | volume={8}, 48 | number={8}, 49 | pages={4713-4720}, 50 | year={2023}, 51 | publisher={IEEE} 52 | ``` 53 | 54 | 55 | ## Contributors 56 | **CrazyChoir** is developed by 57 | [Lorenzo Pichierri](https://www.unibo.it/sitoweb/lorenzo.pichierri), 58 | [Andrea Testa](https://www.unibo.it/sitoweb/a.testa) and 59 | [Giuseppe Notarstefano](https://www.unibo.it/sitoweb/giuseppe.notarstefano) 60 | 61 | ## Acknowledgements 62 | This work was supported in part by the Italian Ministry of Foreign Affairs and International Cooperation, grant number BR22GR01. 63 | 64 |

65 | 66 |

67 | -------------------------------------------------------------------------------- /crazychoir/crazychoir/radio_handler/radio_handler_vel.py: -------------------------------------------------------------------------------- 1 | import time 2 | from math import degrees 3 | import numpy as np 4 | from .radio_handler import RadioHandler 5 | 6 | class RadioHandlerVel(RadioHandler): 7 | 8 | def __init__(self): 9 | super().__init__() 10 | 11 | print('Radio Vel Started!') 12 | 13 | # cmd limits 14 | self.vel_lim_max = 1.0 15 | self.vel_lim_min = -1.0 16 | 17 | # takeoff permission 18 | self.takeoff = False 19 | 20 | def cmd_sender(self, msg, uri, cf): 21 | 22 | # TODO: do something during cmd_vel initialization -> cf needs a cmd every 1s 23 | 24 | # Check safety area 25 | self.check_safety_area() 26 | cmd_vx = np.clip(msg.linear.x, self.vel_lim_min, self.vel_lim_max) 27 | cmd_vy = np.clip(msg.linear.y, self.vel_lim_min, self.vel_lim_max) 28 | cmd_vz = np.clip(msg.linear.z, self.vel_lim_min, self.vel_lim_max) 29 | cmd_yawrate = 0.0 30 | 31 | if self.emergency_stop or np.linalg.norm([cmd_vx, cmd_vy, cmd_vz]) < 1e-3: # or not self.takeoff: 32 | return 33 | 34 | 35 | string = "Commands: vx={:.4f} vy={:.4f} vz={:.4f}".format(cmd_vx, cmd_vy, cmd_vz) 36 | print(string) 37 | 38 | cf.commander.send_velocity_world_setpoint(cmd_vx, cmd_vy, cmd_vz, cmd_yawrate) 39 | 40 | # def cmd_stop(self, _): 41 | # self.emergency_stop = True 42 | 43 | def cmd_stop(self, _): 44 | self.emergency_stop = True 45 | for uri, scf in self.swarm._cfs.items(): 46 | cf = scf.cf 47 | cf.commander.send_stop_setpoint() 48 | print("[{}] \t cf{} - STOP!".format(time.time(), int(uri[-1]))) 49 | 50 | def cmd_take_off(self, _): 51 | self.takeoff = True 52 | 53 | 54 | def set_values(self,uri, cf): 55 | # cf.param.set_value('commander.enHighLevel', 0) -> default 0 56 | # cf.param.set_value('stabilizer.controller', 1) # PID(1), Mellinger(2) -> default 1 57 | cf.param.set_value('stabilizer.estimator', 2) # Complementary(1), Kalman(2) -> default 1 58 | # cf.param.set_value('flightmode.yawMode',2) # plus_mode(1), x_mode(2) -> default 1 59 | # cf.param.set_value('flightmode.stabModeRoll',0) # rate(0) angle(1) -> default 1 60 | # cf.param.set_value('flightmode.stabModePitch', 0) # rate(0) angle(1) -> default 1 61 | # cf.param.set_value('flightmode.stabModeYaw', 0) # rate(0) angle(1) -> default 0 62 | 63 | # Set the std deviation for the quaternion data pushed into the 64 | # kalman filter. The default value seems to be a bit too low. 65 | cf.param.set_value('locSrv.extQuatStdDev', 0.06) 66 | 67 | print("[{}] \t cf{} - parameters set!".format(time.time(), int(uri[-1]))) 68 | 69 | time.sleep(0.1) 70 | cf.param.set_value('kalman.resetEstimation', '1') 71 | time.sleep(0.1) 72 | cf.param.set_value('kalman.resetEstimation', '0') 73 | 74 | print("[{}] \t cf{} - kalman filter reset done!".format(time.time(), int(uri[-1]))) 75 | 76 | -------------------------------------------------------------------------------- /crazychoir/crazychoir/guidance/simple_guidance.py: -------------------------------------------------------------------------------- 1 | from typing import Callable 2 | import numpy as np 3 | 4 | from rclpy.node import Node 5 | from choirbot import Pose 6 | from choirbot.utils.position_getter import pose_subscribe 7 | 8 | from std_msgs.msg import Empty 9 | from trajectory_msgs.msg import JointTrajectory as Trajectory, JointTrajectoryPoint as TrajectoryPoint 10 | 11 | 12 | class SimpleGuidance(Node): 13 | 14 | def __init__(self, pose_handler: str=None, pose_topic: str=None, pose_callback: Callable=None, takeoff_time: float=5.0, height: float=1.0): 15 | 16 | super().__init__('simple_guidance', allow_undeclared_parameters=True, 17 | automatically_declare_parameters_from_overrides=True) 18 | 19 | # get parameters 20 | self.agent_id = self.get_parameter('agent_id').value 21 | self.init_pos = self.get_parameter('init_pos').value 22 | 23 | # initialize pose subscription 24 | self.current_pose = Pose(None, None, None, None) 25 | self.subscription = pose_subscribe(pose_handler, pose_topic, self, self.current_pose, pose_callback) 26 | 27 | # button subscription 28 | self.takeoff_trigger_subscription = self.create_subscription(Empty, '/takeoff', self.takeoff, 10) 29 | self.land_trigger_subscription = self.create_subscription(Empty, '/land', self.landing, 10) 30 | 31 | # subscription to trajectory topic 32 | self.publishers_traj_params = self.create_publisher(Trajectory, 'traj_params', 1) 33 | 34 | # Set default height and takeoff time 35 | self.height = height 36 | self.takeoff_time = takeoff_time 37 | 38 | self.get_logger().info('Simple Guidance {} started'.format(self.agent_id)) 39 | 40 | 41 | def takeoff(self, _): 42 | self.get_logger().info('Starting takeoff') 43 | 44 | msg = Trajectory() 45 | name = 'Agent_{}'.format(self.agent_id) 46 | 47 | point = TrajectoryPoint() 48 | x = self.init_pos[0] 49 | y = self.init_pos[1] 50 | z = self.height 51 | duration_s = self.takeoff_time 52 | point.positions = [x,y,z] 53 | point.velocities = [0.0,0.0,0.0] 54 | point.accelerations = [0.0,0.0,0.0] 55 | point.effort = [0.0,0.0,0.0] 56 | point.time_from_start.sec = int(duration_s) 57 | point.time_from_start.nanosec = int(0) 58 | 59 | msg.joint_names.append(name) 60 | msg.points.append(point) 61 | 62 | self.publishers_traj_params.publish(msg) 63 | 64 | def landing(self, _): 65 | self.get_logger().info('Starting landing') 66 | 67 | msg = Trajectory() 68 | name = 'Agent_{}'.format(self.agent_id) 69 | 70 | point = TrajectoryPoint() 71 | x = self.current_pose.position[0] 72 | y = self.current_pose.position[1] 73 | z = 0.01 74 | duration_s = self.takeoff_time 75 | point.positions = [x,y,z] 76 | point.velocities = [0.0,0.0,0.0] 77 | point.accelerations = [0.0,0.0,0.0] 78 | point.effort = [0.0,0.0,0.0] 79 | point.time_from_start.sec = int(duration_s) 80 | point.time_from_start.nanosec = int(0) 81 | 82 | msg.joint_names.append(name) 83 | msg.points.append(point) 84 | 85 | self.publishers_traj_params.publish(msg) -------------------------------------------------------------------------------- /crazychoir/crazychoir/filtering/single_stage_predictor.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | class SingleStagePredictor: 4 | 5 | def __init__(self, dt = 0.01,dt_dyn = 1e-2, 6 | sigma_u = 10, sigma_m = 1e-4, 7 | x0 = np.zeros((4,1)), PP0 = np.zeros((4,4))): 8 | """ 9 | Double integrator dynamics 10 | Kalman-filter formalism 11 | """ 12 | 13 | self.dt = dt 14 | 15 | self.FF = np.array([[1, dt, 0, 0], [0, 1, 0, 0], [0, 0, 1, dt], [0, 0, 0, 1]]) 16 | self.GG = np.array([[0, 0], [dt, 0], [0, 0], [0, dt]]) 17 | self.HH = np.array([[1,0, 0, 0],[0,0,1,0]]) # only the two positions are measured 18 | 19 | self.nx = np.shape(self.FF)[0] 20 | self.nu = np.shape(self.GG)[1] 21 | self.ny = np.shape(self.HH)[0] 22 | 23 | self.sigma_u = sigma_u # std dev in input 24 | self.sigma_m = sigma_m # std dev in measurement 25 | 26 | self.QQ = self.GG@self.GG.T*self.sigma_u**2 27 | self.RR = np.diag(self.sigma_m**2*np.ones((self.ny,))) 28 | 29 | self.xx_predict = x0 30 | self.PP_predict = PP0 31 | 32 | self.KK = self.observer_gain(self.PP_predict, self.RR) 33 | 34 | self.AA = np.array([[1, dt_dyn, 0, 0], [0, 1, 0, 0], [0, 0, 1, dt_dyn], [0, 0, 0, 1]]) 35 | self.BB = np.array([[0, 0], [dt_dyn, 0], [0, 0], [0, dt_dyn]]) 36 | 37 | def ss_predict(self, zz): 38 | """ 39 | single stage Prediction 40 | - predicts next state, covariance 41 | """ 42 | self.KK = self.observer_gain(self.PP_predict, self.RR) 43 | xx = self.prediction_state(self.xx_predict, zz) 44 | PP = self.prediction_covariance(self.PP_predict, self.RR) 45 | 46 | self.xx_predict = xx 47 | self.PP_predict = PP 48 | 49 | return xx, PP 50 | 51 | def measurement(self,xx): 52 | 53 | vv = np.random.multivariate_normal(np.zeros((self.ny, )), self.RR) 54 | 55 | zz = self.HH@xx + vv[:,None] 56 | 57 | return zz 58 | 59 | def prediction_state(self, xx, zz): 60 | """ 61 | Single-stage predictor 62 | xx -> xx_plus 63 | """ 64 | 65 | xxp = (self.FF -self.KK@self.HH)@xx + self.KK@zz 66 | 67 | return xxp 68 | 69 | def prediction_covariance(self, PP, RR): 70 | """ 71 | Single-stage predictor 72 | PP -> PP_plus 73 | """ 74 | 75 | PPp = (self.FF -self.KK@self.HH)@PP@(self.FF -self.KK@self.HH).T + \ 76 | self.KK@RR@self.KK.T + self.QQ 77 | 78 | return PPp 79 | 80 | def observer_gain(self, PP, RR): 81 | """ 82 | Optimal observer 83 | - covariance matrix (P) 84 | - R matrix (measurement) 85 | """ 86 | 87 | HH = self.HH 88 | 89 | KK = self.FF@PP@HH.T@np.linalg.inv(HH@PP@HH.T + RR) 90 | 91 | return KK 92 | 93 | def correction_state(self, xx, zz): 94 | """ 95 | xx_minus, zz -> xx 96 | """ 97 | 98 | KK = self.KK 99 | HH = self.HH 100 | 101 | xx_hat = xx + KK@(zz - HH@xx) 102 | 103 | return xx_hat 104 | 105 | def correction_convariance(self, PP): 106 | """ 107 | PP_minus -> PP 108 | """ 109 | 110 | II = np.eye(self.nx) 111 | KK = self.KK 112 | HH = self.HH 113 | RR = self.RR 114 | 115 | PP_correct = (II - KK@HH)@PP 116 | 117 | return PP_correct 118 | 119 | def dynamics(self, xx, uu): 120 | 121 | xxp = self.AA@xx + self.BB@uu 122 | 123 | return xxp 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | -------------------------------------------------------------------------------- /crazychoir/crazychoir/gui/controller/utils.py: -------------------------------------------------------------------------------- 1 | import matplotlib 2 | from PyQt5.QtCore import Qt, pyqtSignal 3 | from PyQt5.QtWidgets import QWidget, QVBoxLayout, QSlider, QLayout, QLabel 4 | 5 | matplotlib.use('Qt5Agg') 6 | from PyQt5 import QtWidgets, QtCore 7 | from matplotlib.figure import Figure 8 | from ..controller.spline import DrawSpline 9 | from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg 10 | import numpy as np 11 | 12 | 13 | class GraphPreview(QWidget): 14 | 15 | def __init__(self, file_name): 16 | super().__init__() 17 | self.x, self.y, self.t = DrawSpline.get_cords(file_name) 18 | 19 | self.canvas = MplCanvas(width=10, height=10, dpi=100) 20 | self.update_plot(len(self.t) - 1) 21 | 22 | # Create toolbar, passing canvas as first parament, parent (self, the MainWindow) as second. 23 | self.slider = QSlider(orientation=Qt.Horizontal) 24 | self.slider.setMaximum(len(self.t) - 1) 25 | self.slider.setMinimum(0) 26 | self.slider.valueChanged.connect(self.__my_list_slider_valuechange__) 27 | 28 | self.label = QLabel() 29 | self.label.setText("t = " + str(self.t[-1])) 30 | self.label.setAlignment(Qt.AlignCenter) 31 | 32 | layout = QtWidgets.QVBoxLayout() 33 | layout.addWidget(self.label) 34 | layout.addWidget(self.slider) 35 | layout.addWidget(self.canvas) 36 | 37 | self.setLayout(layout) 38 | 39 | def __my_list_slider_valuechange__(self, index): 40 | # update plot 41 | self.update_plot(index) 42 | self.label.setText("t = " + str(self.t[index])) 43 | 44 | def update_plot(self, max_time): 45 | xdata = self.x[:max_time] 46 | ydata = self.y[:max_time] 47 | 48 | self.canvas.axes.cla() # Clear the canvas. 49 | self.canvas.axes.plot(xdata, ydata, 'r') 50 | # Trigger the canvas to update and redraw. 51 | self.canvas.draw() 52 | 53 | 54 | class MplCanvas(FigureCanvasQTAgg): 55 | 56 | def __init__(self, parent=None, width=5, height=4, dpi=100): 57 | fig = Figure(figsize=(width, height), dpi=dpi) 58 | self.axes = fig.add_subplot(111) 59 | super(MplCanvas, self).__init__(fig) 60 | 61 | 62 | def get_max_value(axes): 63 | max_value = [] 64 | 65 | for ax in axes: 66 | data = np.array(ax.get_ydata()) 67 | max_value.append(np.max(data)) 68 | 69 | return np.max(max_value) 70 | 71 | 72 | class DoubleSlider(QSlider): 73 | 74 | # create our our signal that we can connect to if necessary 75 | doubleValueChanged = pyqtSignal(float) 76 | 77 | def __init__(self, decimals=3, *args, **kargs): 78 | super(DoubleSlider, self).__init__( *args, **kargs) 79 | self._multi = 10 ** decimals 80 | 81 | self.valueChanged.connect(self.emitDoubleValueChanged) 82 | 83 | def emitDoubleValueChanged(self): 84 | value = float(super(DoubleSlider, self).value())/self._multi 85 | self.doubleValueChanged.emit(value) 86 | 87 | def value(self): 88 | return float(super(DoubleSlider, self).value()) / self._multi 89 | 90 | def setMinimum(self, value): 91 | return super(DoubleSlider, self).setMinimum(value * self._multi) 92 | 93 | def setMaximum(self, value): 94 | return super(DoubleSlider, self).setMaximum(value * self._multi) 95 | 96 | def setSingleStep(self, value): 97 | return super(DoubleSlider, self).setSingleStep(value * self._multi) 98 | 99 | def singleStep(self): 100 | return float(super(DoubleSlider, self).singleStep()) / self._multi 101 | 102 | def setValue(self, value): 103 | super(DoubleSlider, self).setValue(int(value * self._multi)) -------------------------------------------------------------------------------- /crazychoir/crazychoir/webots_plugin/motor_ctrl_vel.py: -------------------------------------------------------------------------------- 1 | import rclpy 2 | from std_msgs.msg import Empty 3 | from nav_msgs.msg import Odometry 4 | from geometry_msgs.msg import Twist 5 | 6 | import numpy as np 7 | from math import degrees, radians 8 | 9 | from .motor_ctrl import MotorCtrl 10 | from ..utils import cffirmware 11 | 12 | class MotorCtrlVel(MotorCtrl): 13 | 14 | def init(self, webots_node, properties): 15 | super().init(webots_node, properties) 16 | 17 | print('MotorCrtlFPQR Started!') 18 | 19 | # Declare Subscriptions 20 | msg_type = Twist() 21 | self.target_cmd_vel = msg_type 22 | self.cf_driver.create_subscription(msg_type, '/{}/cmd_vel'.format(self.namespace), self.setpoint_callback, 1) 23 | self.odom_publisher = self.cf_driver.create_publisher(Odometry, '/{}/odom'.format(self.namespace), 10) 24 | self.stop_subscription = self.cf_driver.create_subscription(Empty, '/stop', self.stop, 10) 25 | 26 | # Initialize cf classes 27 | cffirmware.controllerPidInit() 28 | 29 | def setpoint_callback(self, twist): 30 | self.target_cmd_vel = twist 31 | 32 | def step(self): 33 | 34 | rclpy.spin_once(self.cf_driver, timeout_sec=0) 35 | self.time = self.robot.getTime() 36 | 37 | self.update_current_pose() 38 | self.publish_odometry() 39 | self.update_cf_state() 40 | self.update_cf_sensors() 41 | self.check_safety_area() 42 | 43 | if self.initialization: 44 | self.initialization = False 45 | self.init_setpoint() 46 | 47 | self.compute_setpoint() 48 | 49 | ## Firmware PID bindings 50 | tick = 100 #this value makes sure that the position controller and attitude controller are always always initiated 51 | string = "vx = {:.4f}\tvy = {:.4f}\tvz = {:.4f}\tYaw = {:.4f}".format(self.setpoint.velocity.x, self.setpoint.velocity.y, self.setpoint.velocity.z, self.setpoint.attitudeRate.yaw) 52 | # print(string) 53 | 54 | cffirmware.controllerPid(self.control, self.setpoint, self.sensors, self.state, tick) 55 | 56 | 57 | ## 58 | cmd_roll = radians(self.control.roll) # rad/s 59 | cmd_pitch = radians(self.control.pitch) # rad/s 60 | cmd_yaw = -radians(self.control.yaw) # rad/s 61 | cmd_thrust = self.control.thrust # uint (PWM) 62 | 63 | if self.emergency_stop: 64 | cmd_roll = 0.0 65 | cmd_pitch = 0.0 66 | cmd_yaw = 0.0 67 | cmd_thrust = 0.0 68 | 69 | string = "Thrust = {:.0f}\tRoll = {:.4f}\tPitch = {:.4f}\tYaw = {:.4f}".format(cmd_thrust, cmd_roll, cmd_pitch, cmd_yaw) 70 | # print(string) 71 | 72 | self.send_motor_cmd(cmd_thrust, cmd_roll, cmd_pitch, cmd_yaw) 73 | 74 | self.past_time = self.robot.getTime() 75 | self.past_position = self.current_pose.position 76 | 77 | 78 | def init_setpoint(self): 79 | #from modules/src/crtl_commander_generic.c VelocityDecoder 80 | 81 | # Initialize setpoint modes 82 | self.setpoint.mode.x = cffirmware.modeVelocity 83 | self.setpoint.mode.y = cffirmware.modeVelocity 84 | self.setpoint.mode.z = cffirmware.modeVelocity 85 | self.setpoint.mode.yaw = cffirmware.modeVelocity 86 | self.setpoint.velocity_body = False 87 | 88 | def compute_setpoint(self): 89 | 90 | self.setpoint.velocity.x = self.target_cmd_vel.linear.x #m/s 91 | # NOTE: minus @ pitch 92 | self.setpoint.velocity.y = self.target_cmd_vel.linear.y #m/s 93 | self.setpoint.velocity.z = self.target_cmd_vel.linear.z #m/s 94 | self.setpoint.attitudeRate.yaw = self.target_cmd_vel.angular.z # deg/s 95 | 96 | -------------------------------------------------------------------------------- /crazychoir/crazychoir/webots_plugin/motor_ctrl_fpqr.py: -------------------------------------------------------------------------------- 1 | import rclpy 2 | from std_msgs.msg import Empty 3 | from nav_msgs.msg import Odometry 4 | from geometry_msgs.msg import Twist 5 | 6 | import numpy as np 7 | from math import degrees, radians 8 | 9 | from .motor_ctrl import MotorCtrl 10 | from ..utils import cffirmware 11 | 12 | class MotorCtrlFPQR(MotorCtrl): 13 | 14 | def init(self, webots_node, properties): 15 | super().init(webots_node, properties) 16 | 17 | print('MotorCrtlFPQR Started!') 18 | 19 | # Declare Subscriptions 20 | msg_type = Twist() 21 | self.target_cmd_vel = msg_type 22 | self.cf_driver.create_subscription(msg_type, '/{}/cmd_vel'.format(self.namespace), self.setpoint_callback, 1) 23 | self.odom_publisher = self.cf_driver.create_publisher(Odometry, '/{}/odom'.format(self.namespace), 10) 24 | self.stop_subscription = self.cf_driver.create_subscription(Empty, '/stop', self.stop, 10) 25 | 26 | # Initialize cf classes 27 | cffirmware.controllerPidInit() 28 | 29 | def setpoint_callback(self, twist): 30 | self.target_cmd_vel = twist 31 | 32 | def step(self): 33 | 34 | rclpy.spin_once(self.cf_driver, timeout_sec=0) 35 | self.time = self.robot.getTime() 36 | 37 | self.update_current_pose() 38 | self.publish_odometry() 39 | self.update_cf_state() 40 | self.update_cf_sensors() 41 | self.check_safety_area() 42 | 43 | if self.initialization: 44 | self.initialization = False 45 | self.init_setpoint() 46 | 47 | self.compute_setpoint() 48 | 49 | ## Firmware PID bindings 50 | tick = 100 #this value makes sure that the position controller and attitude controller are always always initiated 51 | cffirmware.controllerPid(self.control, self.setpoint, self.sensors, self.state, tick) 52 | 53 | ## 54 | cmd_roll = radians(self.control.roll) # rad/s 55 | cmd_pitch = radians(self.control.pitch) # rad/s 56 | cmd_yaw = -radians(self.control.yaw) # rad/s 57 | cmd_thrust = self.control.thrust # uint (PWM) 58 | 59 | if self.emergency_stop: 60 | cmd_roll = 0.0 61 | cmd_pitch = 0.0 62 | cmd_yaw = 0.0 63 | cmd_thrust = 0.0 64 | 65 | string = "Thrust = {:.0f}\tRoll = {:.4f}\tPitch = {:.4f}\tYaw = {:.4f}".format(cmd_thrust, cmd_roll, cmd_pitch, cmd_yaw) 66 | # print(string) 67 | 68 | self.send_motor_cmd(cmd_thrust, cmd_roll, cmd_pitch, cmd_yaw) 69 | 70 | self.past_time = self.robot.getTime() 71 | self.past_position = self.current_pose.position 72 | 73 | 74 | def init_setpoint(self): 75 | 76 | # Initialize setpoint modes 77 | self.setpoint.mode.roll = cffirmware.modeVelocity 78 | self.setpoint.mode.pitch = cffirmware.modeVelocity 79 | self.setpoint.mode.yaw = cffirmware.modeVelocity 80 | self.setpoint.mode.z = cffirmware.modeDisable # needed to use thrust as setpoint 81 | 82 | def compute_setpoint(self): 83 | # Conversion constants 84 | mg_newton = 0.372652 # Thrust in Newton to take_off 85 | mg_pwm = 38615.0 # Thrust in PWM units to take_off with mass 0.038 (webots), scaling = 800 86 | newton2pwm = mg_pwm/mg_newton 87 | 88 | # Thrust limits 89 | thrust_min = 10001 90 | thrust_max = 60000 91 | 92 | self.setpoint.attitudeRate.roll = degrees(self.target_cmd_vel.angular.x) # deg/s 93 | # NOTE: minus @ pitch 94 | self.setpoint.attitudeRate.pitch = degrees(-self.target_cmd_vel.angular.y) # deg/s 95 | self.setpoint.attitudeRate.yaw = degrees(self.target_cmd_vel.angular.z) # deg/s 96 | self.setpoint.thrust = np.clip(self.target_cmd_vel.linear.z*newton2pwm, thrust_min, thrust_max) # PWM units 97 | -------------------------------------------------------------------------------- /crazychoir/crazychoir/controller/velocity_ctrl.py: -------------------------------------------------------------------------------- 1 | from .hierarchical_control import CrazyflieController 2 | 3 | from ..crazyflie_communication import SenderStrategy 4 | from ..planner import TrajHandlerStrategy 5 | 6 | from typing import Callable 7 | 8 | import numpy as np 9 | 10 | from geometry_msgs.msg import Twist 11 | 12 | 13 | class VelocityCtrlStrategy: 14 | def __init__(self, update_frequency): 15 | self.update_frequency = update_frequency 16 | self.K_p = np.array([ 0.4, 0.4, 0.4]) 17 | self.K_v = np.array([ 0.01, 0.01, 0.01]) 18 | 19 | 20 | def control(self, current_pose, desired_reference): 21 | ep = desired_reference["position"] - current_pose.position 22 | if not "velocity" in desired_reference: 23 | desired_reference["velocity"] = np.zeros(3) 24 | ev = desired_reference["velocity"] - current_pose.velocity 25 | u_vel = self.K_p * ep + self.K_v * ev 26 | return u_vel 27 | 28 | class VelocityLQRStrategy: 29 | pass 30 | 31 | class VelocityController(CrazyflieController): 32 | def __init__(self, pose_handler: str=None, pose_topic: str=None, pose_callback: Callable=None, 33 | velocity_strategy: VelocityCtrlStrategy=None, 34 | command_sender: SenderStrategy=None, traj_handler: TrajHandlerStrategy=None): 35 | 36 | super().__init__(pose_handler, pose_topic, pose_callback, command_sender, traj_handler) 37 | 38 | self.publisher_ = self.create_publisher(Twist, 'cmd_vel', 1) 39 | 40 | self.velocity_strategy = velocity_strategy 41 | 42 | if self.velocity_strategy is None: 43 | raise ValueError("A velocity controller is required ") 44 | 45 | self.ctrl_timer = self.create_timer(1.0/self.velocity_strategy.update_frequency, self.controller) 46 | 47 | self.set_steady_reference = True 48 | self.desired_reference = {} 49 | 50 | self.desired_reference["position"] = None 51 | self.desired_reference["velocity"] = None 52 | self.desired_reference["acceleration"] = None 53 | self.desired_reference["yaw"] = 0.0 54 | 55 | def controller(self): 56 | if self.current_pose.position is not None and self.current_pose.velocity is not None: 57 | if self.traj_handler is not None and bool(self.traj_handler.get_desired_reference()): # #NOTE and (there is a desired reference) 58 | self.desired_reference = self.traj_handler.get_desired_reference() 59 | self.set_steady_reference = True 60 | else: 61 | if self.set_steady_reference and self.desired_reference["position"] is not None: 62 | self.get_logger().warn('Reference lost - keeping hovering') 63 | self.set_steady_reference = False 64 | self.desired_reference["position"] = self.current_pose.position 65 | self.desired_reference["velocity"] = np.zeros(3) 66 | 67 | if self.desired_reference["position"] is None: 68 | crazyflie_input = self.command_sender.command_sender(np.zeros(3)) 69 | self.send_input(crazyflie_input) 70 | return 71 | 72 | u_vel = self.velocity_strategy.control(self.current_pose, self.desired_reference) 73 | crazyflie_input = self.command_sender.command_sender(u_vel) 74 | # print(f'u_vel= {crazyflie_input} | p_des= {self.desired_reference["position"]}') 75 | self.send_input(crazyflie_input) 76 | 77 | else: 78 | # self.get_logger().warn('Pose not available - stopping the drone') 79 | crazyflie_input = self.command_sender.command_sender(np.zeros(3)) 80 | self.send_input(crazyflie_input) 81 | 82 | 83 | def send_input(self, u): 84 | msg = Twist() 85 | msg.linear.x = u[0] 86 | msg.linear.y = u[1] 87 | msg.linear.z = u[2] 88 | self.publisher_.publish(msg) -------------------------------------------------------------------------------- /crazychoir_examples/launch/formation_rviz.launch.py: -------------------------------------------------------------------------------- 1 | from launch import LaunchDescription 2 | from launch_ros.actions import Node 3 | from ament_index_python.packages import get_package_share_directory 4 | import numpy as np 5 | import os 6 | 7 | 8 | def generate_launch_description(): 9 | 10 | # set rng seed 11 | np.random.seed(1) 12 | 13 | # number of agents 14 | N = 4 15 | 16 | # communication matrix 17 | Adj = np.array([ 18 | [0, 1, 0, 1], 19 | [1, 0, 1, 1], 20 | [0, 1, 0, 1], 21 | [1, 1, 1, 0], 22 | ]) 23 | 24 | # generate initial positions 25 | P = np.zeros((N, 3)) 26 | # fix static leader positions 27 | P[0,0:3] = np.array([1.0, 0.0, 1.0]) 28 | P[1,0:3] = np.array([2.0, 0.0, 1.0]) 29 | P[2,0:3] = np.array([0.0, 0.0, 1.0]) 30 | P[3,0:3] = np.array([0.0, 0.0, 1.0]) 31 | # set random follower positions on x-y plane 32 | P[2:4,0:2] = 2.0*np.random.rand(2,2) 33 | 34 | # compute orthogonal projection matrix 35 | # generate coordinates of a square to evaluate desired bearings 36 | D = np.zeros((N, 3)) 37 | D[0,0:3] = np.array([-1, -1, 1]) 38 | D[1,0:3] = np.array([ 1, -1, 1]) 39 | D[2,0:3] = np.array([ 1, 1, 1]) 40 | D[3,0:3] = np.array([-1, 1, 1]) 41 | dd = np.size(D,1) 42 | 43 | orth_proj = np.zeros((dd*N, dd*N)) 44 | for i in range(N): 45 | neigh_ii = np.nonzero(Adj[i, :])[0].tolist() 46 | for j in neigh_ii: 47 | x_des_i = D[i, :] 48 | x_des_j = D[j, :] 49 | # compute desired bearing 50 | g_ij = np.matrix((x_des_j - x_des_i)/np.linalg.norm(x_des_j - x_des_i)) 51 | # fill the matrix 52 | orth_proj[i*dd:i*dd+N-1,j*dd:j*dd+N-1] = np.eye(dd) - g_ij.T @ g_ij 53 | 54 | # initialize launch description with rviz executable 55 | rviz_config_dir = get_package_share_directory('crazychoir_examples') 56 | rviz_config_file = os.path.join(rviz_config_dir, 'rvizconf.rviz') 57 | launch_description = [Node(package='rviz2', executable='rviz2', output='screen', 58 | arguments=['-d', rviz_config_file])] 59 | 60 | # add executables for each robot 61 | for i in range(N): 62 | 63 | in_neighbors = np.nonzero(Adj[:, i])[0].tolist() 64 | out_neighbors = np.nonzero(Adj[i, :])[0].tolist() 65 | position = P[i, :].tolist() 66 | is_leader = True if i < 2 else False 67 | orth_proj_array = orth_proj[i*dd:(i+1)*dd,:].flatten().tolist() 68 | 69 | # guidance 70 | launch_description.append(Node( 71 | package='crazychoir_examples', 72 | executable='crazychoir_formation_rviz_guidance', 73 | output='screen', 74 | namespace='agent_{}'.format(i), 75 | parameters=[{'agent_id': i, 'N': N, 'dd': dd, 'in_neigh': in_neighbors, 'out_neigh': out_neighbors, 'is_leader': is_leader, 'ort_proj': orth_proj_array}])) 76 | 77 | # controller 78 | launch_description.append(Node( 79 | package='crazychoir_examples', 80 | executable='crazychoir_formation_rviz_controller', 81 | output='screen', 82 | namespace='agent_{}'.format(i), 83 | parameters=[{'agent_id': i}])) 84 | 85 | # integrator 86 | launch_description.append(Node( 87 | package='crazychoir_examples', 88 | executable='crazychoir_formation_rviz_integrator', 89 | output='screen', 90 | namespace='agent_{}'.format(i), 91 | parameters=[{'agent_id': i, 'init_pos': position}])) 92 | 93 | # RVIZ visualization 94 | launch_description.append(Node( 95 | package='crazychoir_examples', 96 | executable='crazychoir_formation_rviz_rviz', 97 | output='screen', 98 | namespace='agent_{}'.format(i), 99 | parameters=[{'agent_id': i}])) 100 | 101 | return LaunchDescription(launch_description) 102 | -------------------------------------------------------------------------------- /crazychoir/crazychoir/gui/controller/ros.py: -------------------------------------------------------------------------------- 1 | from rclpy.node import Node 2 | from std_msgs.msg import Empty 3 | from trajectory_msgs.msg import JointTrajectory as Trajectory, JointTrajectoryPoint as TrajectoryPoint 4 | from .spline import DrawSpline 5 | 6 | class ControlPanel(Node): 7 | 8 | def __init__(self): 9 | super().__init__('gui', 10 | allow_undeclared_parameters=True, 11 | automatically_declare_parameters_from_overrides=True) 12 | 13 | self.n_agents = self.get_parameter('n_agents').value 14 | self.publishers_traj_params = {} 15 | 16 | for i in range(self.n_agents): 17 | topic = '/agent_{}/traj_params'.format(i) 18 | self.publishers_traj_params[i] = self.create_publisher(Trajectory, topic, 1) 19 | 20 | self.publisher_experiment = self.create_publisher(Empty, '/experiment_trigger', 10) 21 | self.publisher_takeoff = self.create_publisher(Empty, '/takeoff', 10) 22 | self.publisher_land = self.create_publisher(Empty, '/land', 10) 23 | self.publisher_stop = self.create_publisher(Empty, '/stop', 10) 24 | self.publisher_plot = self.create_publisher(Empty, '/plot', 10) 25 | self.publisher_save_csv = self.create_publisher(Empty, '/csv', 10) 26 | self.publisher_save_pickle = self.create_publisher(Empty, '/pickle', 10) 27 | 28 | 29 | def send_trajectory(self,id,t,x,y,z): 30 | 31 | msg = Trajectory() 32 | name = 'Agent_{}'.format(id) 33 | msg.joint_names.append(name) 34 | 35 | for i in range(len(t)): 36 | point = TrajectoryPoint() 37 | point.positions = [float(x[i]), float(y[i]), float(z)] 38 | point.velocities = [0.0,0.0,0.0] 39 | point.accelerations = [0.0,0.0,0.0] 40 | point.effort = [0.0,0.0,0.0] 41 | point.time_from_start.sec = int(t[i]) 42 | point.time_from_start.nanosec = int((t[i]-int(t[i]))*1e9) #e9 43 | msg.points.append(point) 44 | # self.get_logger().info('Append point: ({}, {}, {}, {}) sent to cf{} from crazydraw'.format(point.time_from_start.nanosec,x[i],y[i],z,id)) 45 | 46 | self.publishers_traj_params[id].publish(msg) 47 | 48 | 49 | def send_full_trajectory(self,file_name, id, time_scale, interpolation_scale, height = 1): 50 | self.get_logger().info('Send full trajectory command sent') 51 | x, y, t = DrawSpline.get_cords(file_name) 52 | t = [value * time_scale for value in t] 53 | x, y, t = DrawSpline.__distance_interpolation__(x, y, t, interpolation_scale) 54 | self.send_trajectory(id, t ,x, y, height) 55 | 56 | 57 | def goto(self,id,t,x,y,z): 58 | self.get_logger().info('Go-to command sent') 59 | height = z 60 | self.send_trajectory(id, [t] ,[x], [y], height) 61 | 62 | def takeoff(self): 63 | self.get_logger().info('Take-off command sent') 64 | msg = Empty() 65 | self.publisher_takeoff.publish(msg) 66 | 67 | def land(self): 68 | self.get_logger().info('Land command sent') 69 | msg = Empty() 70 | self.publisher_land.publish(msg) 71 | 72 | def start_experiment(self): 73 | self.get_logger().info('Start experiment command sent') 74 | msg = Empty() 75 | self.publisher_experiment.publish(msg) 76 | 77 | def stop(self): 78 | self.get_logger().info('Stop command sent') 79 | msg = Empty() 80 | self.publisher_stop.publish(msg) 81 | 82 | def plot(self): 83 | self.get_logger().info('Plot command sent') 84 | msg = Empty() 85 | self.publisher_plot.publish(msg) 86 | 87 | def save_csv(self): 88 | self.get_logger().info('Save .csv command sent') 89 | msg = Empty() 90 | self.publisher_save_csv.publish(msg) 91 | 92 | def save_pickle(self): 93 | self.get_logger().info('Save .pkl command sent') 94 | msg = Empty() 95 | self.publisher_save_pickle.publish(msg) 96 | 97 | def void(self): 98 | msg = Empty() 99 | self.publisher_takeoff.publish(msg) 100 | 101 | def notvoid(self): 102 | msg = Empty() 103 | self.publisher_land.publish(msg) -------------------------------------------------------------------------------- /crazychoir/crazychoir/filtering/estimator.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from numpy import cos, sin 3 | from scipy.spatial.transform import Rotation as R 4 | from typing import Callable 5 | 6 | class Estimator(Callable): 7 | 8 | def __init__(self): 9 | self.filter_order = 3 10 | self.poses = { 11 | 'position' : [np.zeros(3) for i in range(self.filter_order + 1)], # +1 due to current pose 12 | 'pre_filtered_rpy' : [np.zeros(3) for i in range(self.filter_order + 1)], 13 | 'rpy' : [np.zeros(3) for i in range(self.filter_order + 1)], 14 | 'pre_filtered_velocity' : [np.zeros(3) for i in range(self.filter_order + 1)], 15 | 'velocity' : [np.zeros(3) for i in range(self.filter_order + 1)], 16 | 'pre_filtered_rpy_dot' : [np.zeros(3) for i in range(self.filter_order + 1)], 17 | 'rpy_dot' : [np.zeros(3) for i in range(self.filter_order + 1)], 18 | } 19 | 20 | def set_pose(self, pose): 21 | self.current_pose = pose 22 | 23 | def __call__(self): 24 | 25 | if self.current_pose.orientation is not None: 26 | self.poses["rpy"].pop(0) 27 | 28 | if np.linalg.norm(self.current_pose.orientation) < 0.8: 29 | print('WARN: Vicon sample lost') 30 | 31 | self.current_pose.position = np.copy(self.poses["position"][-1]) 32 | self.current_pose.orientation = R.from_euler('xyz',self.poses["rpy"][-1]).as_quat() 33 | 34 | self.poses["rpy"].append(R.from_quat(self.current_pose.orientation).as_euler('xyz')) 35 | self.current_pose.angular = self.angular_low_pass() 36 | 37 | if self.current_pose.position is not None: 38 | self.poses["position"].pop(0) 39 | 40 | self.poses["position"].append(self.current_pose.position) 41 | self.current_pose.velocity = self.velocity_low_pass() 42 | 43 | 44 | return self.current_pose 45 | 46 | def velocity_low_pass(self): 47 | 48 | gain = 200.0 49 | zeros = [ 1.0, 0.0, 0.0] 50 | poles = [-1.0, 0.0, 0.0] 51 | 52 | low_pass_3rd_order(gain, zeros, poles, self.poses['position'], self.poses['pre_filtered_velocity']) 53 | 54 | gain = 0.2 55 | zeros = [-1.0, 0.0, 0.0] 56 | poles = [ 0.6, 0.0, 0.0] 57 | 58 | low_pass_3rd_order(gain, zeros, poles, self.poses['pre_filtered_velocity'],self.poses['velocity']) 59 | return self.poses['velocity'][-1] 60 | 61 | def angular_low_pass(self): 62 | 63 | gain = 200.0 64 | zeros = [ 1.0, 0.0, 0.0] 65 | poles = [-1.0, 0.0, 0.0] 66 | 67 | low_pass_3rd_order(gain, zeros, poles, self.poses['rpy'], self.poses['pre_filtered_rpy_dot']) 68 | 69 | gain = 0.2 70 | zeros = [-1.0, 0.0, 0.0] 71 | poles = [ 0.6, 0.0, 0.0] 72 | 73 | low_pass_3rd_order(gain, zeros, poles, self.poses['pre_filtered_rpy_dot'],self.poses['rpy_dot']) 74 | 75 | return self.poses['rpy_dot'][-1] 76 | 77 | 78 | def low_pass_3rd_order(gain, zeros, poles, input, output): 79 | 80 | if len(input)<4 or len(output)<4: 81 | raise ValueError("Low-pass filter input needs at least 3 past samples pus the current one") 82 | 83 | N = np.array([ 84 | -(zeros[0]*zeros[1]*zeros[2]), 85 | zeros[0]*zeros[1] + zeros[1]*zeros[2] + zeros[0]*zeros[2], 86 | -(zeros[0] + zeros[1] + zeros[2]), 87 | 1.0, 88 | ]).reshape(1,4) 89 | 90 | D = np.array([ 91 | 0.0, 92 | poles[0]*poles[1]*poles[2], 93 | -(poles[0]*poles[1] + poles[1]*poles[2] + poles[0]*poles[2]), 94 | poles[0] + poles[1] + poles[2], 95 | ]).reshape(1,4) 96 | 97 | output.append((D@np.array(output) + gain*N@np.array(input)).reshape(3,)) 98 | output.pop(0) 99 | 100 | def change_matrix(rpy): 101 | # convert angular velocities to euler rates 102 | cphi = cos(rpy[0]) 103 | ctheta = cos(rpy[1]) 104 | sphi = sin(rpy[0]) 105 | stheta = sin(rpy[1]) 106 | ttheta = stheta/ctheta 107 | R = np.array([[1,sphi*ttheta,cphi*ttheta],[0,cphi,-stheta],[0,sphi/ctheta,cphi/ctheta]]) 108 | return R 109 | 110 | -------------------------------------------------------------------------------- /crazychoir/crazychoir/guidance/aggregative.py: -------------------------------------------------------------------------------- 1 | from geometry_msgs.msg import Vector3 2 | 3 | from choirbot.guidance.aggregative import AggregativeGuidance 4 | from choirbot.optimizer import AggregativeOptimizer 5 | import numpy as np 6 | 7 | from std_msgs.msg import Empty 8 | from datetime import datetime 9 | import dill 10 | import os 11 | import time 12 | 13 | from typing import Callable 14 | 15 | 16 | 17 | class AggregativeGuidanceCrazyflie(AggregativeGuidance): 18 | def __init__(self, optimizer: AggregativeOptimizer, height: float=1.0, freq_reference: float = 10.0, 19 | pose_handler: str = None, pose_topic: str = None, pose_callback: Callable=None, 20 | msg_topic: str='position', msg_type=Vector3, seed: int=5): 21 | super().__init__(optimizer, freq_reference, pose_handler, pose_topic, pose_callback, msg_topic, msg_type) 22 | 23 | self.height = height 24 | self.seed = seed 25 | np.random.seed(self.seed) 26 | 27 | self.target = self.get_parameter_or('target').value 28 | if self.target is None: 29 | self.target = False 30 | self.get_logger().warn('Static Target not set. Using default value False') 31 | else: 32 | self.target = np.array(self.target).reshape(-1,1) 33 | 34 | self.intruder = self.get_parameter_or('intruder').value 35 | if self.intruder is None: 36 | self.intruder = False 37 | self.get_logger().warn('Static Intruder not set. Using default value False') 38 | else: 39 | self.intruder = np.array(self.intruder).reshape(-1,1) 40 | 41 | self.land_trigger_subscription = self.create_subscription(Empty, '/land', self.end_experiment, 10) 42 | 43 | def end_experiment(self, _): 44 | self.get_logger().info('End of the experiment. Landing...') 45 | self.save_results() 46 | self.optimization_ended = True 47 | 48 | def get_initial_condition(self): 49 | if self.current_pose.position is not None: 50 | return self.current_pose.position[:2].reshape(-1,1) 51 | else: 52 | raise ValueError('Pose callback not ready') 53 | 54 | def get_intruder(self): 55 | if self.intruder is not False: 56 | return self.intruder[:2].reshape(-1,1) 57 | else: 58 | return False 59 | 60 | def get_target(self): 61 | if self.target is not False: 62 | return self.target[:2].reshape(-1,1) 63 | else: 64 | return False 65 | 66 | def save_results(self): 67 | print('save_results() not implemented yet :(') 68 | 69 | def generate_reference_msg(self, msg_type, x_des): 70 | 71 | # skip if not ready 72 | if self.current_pose.position is None or self.optimizer.algorithm is None or self.optimization_ended: 73 | return 74 | 75 | # get reference 76 | x_des = self.optimizer.get_result() 77 | 78 | x = x_des[0,0] 79 | y = x_des[1,0] 80 | z = self.height 81 | 82 | msg = msg_type() 83 | 84 | msg.x = x 85 | msg.y = y 86 | msg.z = z 87 | 88 | return msg 89 | 90 | from choirbot import Pose 91 | from choirbot.utils.position_getter import pose_subscribe 92 | 93 | class AggregativeGuidanceCrazyflieMovingIntruder(AggregativeGuidanceCrazyflie): 94 | 95 | def __init__(self, optimizer: AggregativeOptimizer, height: float=1.0, freq_reference: float = 10.0, 96 | pose_handler: str = None, pose_topic: str = None, pose_callback: Callable=None, 97 | msg_topic: str='position', msg_type=Vector3, seed: int=5): 98 | 99 | super().__init__(optimizer, height, freq_reference, pose_handler, pose_topic, pose_callback, msg_topic, msg_type, seed) 100 | 101 | self.get_logger().info('AggregativeGuidanceCrazyflieMovingIntruder Started!') 102 | self.intruder_pose = Pose(None, None,None, None) 103 | 104 | 105 | handler = 'pubsub' 106 | topic = f'/agent_{self.agent_id+self.n_agents}/odom' 107 | callback = None 108 | intruder_subscription = pose_subscribe(handler, topic, self, self.intruder_pose, callback) 109 | 110 | 111 | def get_intruder(self): 112 | print(self.intruder_pose.position) 113 | if self.intruder_pose.position is not None: 114 | return self.intruder_pose.position[:2].reshape(-1,1) 115 | else: 116 | self.get_logger().warn('Pose Intruder not available - returning False') 117 | return False 118 | 119 | -------------------------------------------------------------------------------- /crazychoir_examples/launch/tracking_webots.launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | import pathlib 3 | 4 | from launch import LaunchDescription 5 | from launch_ros.actions import Node 6 | from ament_index_python.packages import get_package_share_directory 7 | 8 | from webots_ros2_driver.webots_launcher import WebotsLauncher 9 | 10 | node_frequency = 100 # [Hz] 11 | 12 | def get_webots_driver_cf(agent_id): 13 | package_dir_driver = get_package_share_directory('crazychoir_examples') 14 | robot_description = pathlib.Path(os.path.join(package_dir_driver, 'crazyflie_fpqr.urdf')).read_text() 15 | crazyflie_driver = Node( 16 | package='webots_ros2_driver', 17 | executable='driver', 18 | namespace=f'agent_{agent_id}', 19 | output='screen', 20 | additional_env={ 21 | 'WEBOTS_ROBOT_NAME':f'agent_{agent_id}', 22 | }, 23 | parameters=[ 24 | {'robot_description': robot_description}, 25 | ] 26 | ) 27 | 28 | return crazyflie_driver 29 | 30 | def generate_webots_world_file(robots, source_filename, target_filename): 31 | with open(source_filename, 'r') as source_file: 32 | contents = source_file.read() 33 | 34 | with open(target_filename, 'w') as target_file: 35 | target_file.write(contents) 36 | 37 | for robot in robots: 38 | template_filename = os.path.join(os.path.dirname(source_filename), f'obj_{robot["type"]}.wbt') 39 | with open(template_filename, 'r') as template_file: 40 | template = template_file.read() 41 | template = template.replace('$NAME', robot["name"]) 42 | template = template.replace('$X', str(robot["position"][0])) 43 | template = template.replace('$Y', str(robot["position"][1])) 44 | template = template.replace('$Z', str(robot["position"][2])) 45 | target_file.write(template) 46 | 47 | 48 | def generate_launch_description(): 49 | 50 | initial_position = [0.0, 0.0, 0.015] 51 | robots = [{ 52 | 'name': 'agent_0', 53 | 'type': 'crazyflie', 54 | 'position': initial_position, 55 | }] 56 | 57 | 58 | # initialize launch description 59 | launch_description = [] 60 | 61 | world_package_dir = get_package_share_directory('crazychoir_examples') 62 | source_filename = os.path.join(world_package_dir, 'worlds', 'empty_world.wbt') 63 | target_filename = os.path.join(world_package_dir, 'worlds', 'my_world.wbt') 64 | generate_webots_world_file(robots, source_filename, target_filename) 65 | webots = WebotsLauncher(world=target_filename) 66 | launch_description.append(webots) 67 | 68 | 69 | # Launch control Panel 70 | launch_description.append(Node( 71 | package='crazychoir_examples', 72 | executable='crazychoir_tracking_webots_gui', 73 | output='screen', 74 | parameters=[{ 75 | 'n_agents': 1, 76 | }])) 77 | 78 | # webots exec 79 | launch_description.append(get_webots_driver_cf(0)) 80 | launch_description.append(Node( 81 | package='robot_state_publisher', 82 | executable='robot_state_publisher', 83 | additional_env={'WEBOTS_ROBOT_NAME':'agent_0'}, 84 | namespace='agent_0', 85 | output='screen', 86 | parameters=[{ 87 | 'robot_description': '', 88 | }])) 89 | 90 | # controller 91 | launch_description.append(Node( 92 | package='crazychoir_examples', 93 | executable='crazychoir_tracking_webots_controller', 94 | namespace='agent_0', 95 | output='screen', 96 | parameters=[{ 97 | 'agent_id': 0, 98 | }])) 99 | 100 | # guidance 101 | launch_description.append(Node( 102 | package='crazychoir_examples', 103 | executable='crazychoir_tracking_webots_guidance', 104 | namespace='agent_0', 105 | output='screen', 106 | parameters=[{ 107 | 'agent_id': 0, 108 | 'init_pos': initial_position, 109 | }])) 110 | 111 | # reference 112 | launch_description.append(Node( 113 | package='crazychoir_examples', 114 | executable='crazychoir_tracking_webots_trajectory', 115 | namespace='agent_0', 116 | output='screen', 117 | parameters=[{ 118 | 'agent_id': 0, 119 | }])) 120 | 121 | return LaunchDescription(launch_description) 122 | -------------------------------------------------------------------------------- /crazychoir/crazychoir/planner/trajectory_handler/trajectory_handler.py: -------------------------------------------------------------------------------- 1 | from rclpy.node import Node 2 | 3 | from choirbot import Pose 4 | from choirbot.utils.position_getter import pose_subscribe 5 | from crazychoir_interfaces.msg import FullState 6 | from geometry_msgs.msg import Vector3 7 | from trajectory_msgs.msg import JointTrajectory as Trajectory, JointTrajectoryPoint as TrajectoryPoint 8 | 9 | from typing import Callable 10 | 11 | class TrajectoryHandler(Node): 12 | """ 13 | #TODO 14 | """ 15 | 16 | def __init__(self, pose_handler: str=None, pose_topic: str=None, pose_callback: Callable = None): 17 | """ 18 | Args: 19 | pose_handler (str, optional): Pose handler (see 20 | :func:`~choirbot.utils.position_getter.pose_subscribe`). Defaults to None. 21 | pose_topic (str, optional): Topic where pose is published. Defaults to None. 22 | """ 23 | super().__init__('trajectory', allow_undeclared_parameters=True, 24 | automatically_declare_parameters_from_overrides=True) 25 | 26 | # get parameters 27 | self.agent_id = self.get_parameter('agent_id').value 28 | 29 | # initialize pose subscription 30 | self.current_pose = Pose(None, None, None, None) 31 | self.subscription = pose_subscribe(pose_handler, pose_topic, self, self.current_pose, pose_callback) 32 | 33 | # starting flags 34 | self.start_sender = False 35 | self.first_evaluation = False 36 | 37 | # initialize trajectory parameters subscription 38 | self.traj_params_topic = 'traj_params' 39 | self.create_subscription(Trajectory, self.traj_params_topic, self.traj_params_callback, 1) 40 | 41 | def traj_params_callback(self): 42 | raise NotImplementedError 43 | 44 | def handler(self): 45 | # skip if position is not available yet 46 | if self.current_pose.position is None: 47 | return 48 | 49 | if self.start_sender: 50 | # compute reference 51 | ref = self.evaluate_reference() 52 | 53 | # send reference to controller 54 | self.send_reference(ref) 55 | 56 | def send_reference(self, ref): 57 | raise NotImplementedError 58 | 59 | def evaluate_reference(self): 60 | raise NotImplementedError 61 | 62 | 63 | class AccelerationTrajHandler(TrajectoryHandler): 64 | ''' 65 | #TODO 66 | ''' 67 | 68 | def __init__(self, update_frequency: float, pose_handler: str=None, pose_topic: str=None, pose_callback: Callable = None, input_topic: str = 'acceleration'): 69 | 70 | super().__init__(pose_handler, pose_topic, pose_callback) 71 | 72 | self.publisher_ = self.create_publisher(Vector3, input_topic, 1) 73 | self.update_frequency = update_frequency 74 | self.timer = self.create_timer(1.0/self.update_frequency, self.handler) 75 | self.get_logger().info('Acceleration reference {} started'.format(self.agent_id)) 76 | 77 | def send_reference(self, ref): 78 | msg = Vector3() 79 | 80 | msg.x = ref[0] 81 | msg.y = ref[1] 82 | msg.z = ref[2] 83 | 84 | self.publisher_.publish(msg) 85 | 86 | 87 | class FullStateTrajHandler(TrajectoryHandler): 88 | ''' 89 | #TODO 90 | ''' 91 | 92 | def __init__(self, update_frequency: float, pose_handler: str=None, pose_topic: str=None, pose_callback: Callable = None, input_topic: str = 'fullstate'): 93 | 94 | super().__init__(pose_handler, pose_topic, pose_callback) 95 | 96 | self.publisher_ = self.create_publisher(FullState, input_topic, 1) 97 | self.update_frequency = update_frequency 98 | self.timer = self.create_timer(1.0/self.update_frequency, self.handler) 99 | self.get_logger().info('FullState reference {} started'.format(self.agent_id)) 100 | 101 | 102 | def send_reference(self, ref): 103 | msg = FullState() 104 | 105 | msg.pose.position.x = ref[0] 106 | msg.pose.position.y = ref[1] 107 | msg.pose.position.z = ref[2] 108 | 109 | msg.pose.orientation.x = ref[3] 110 | msg.pose.orientation.y = ref[4] 111 | msg.pose.orientation.z = ref[5] 112 | msg.pose.orientation.w = ref[6] 113 | 114 | msg.vel.linear.x = ref[7] 115 | msg.vel.linear.y = ref[8] 116 | msg.vel.linear.z = ref[9] 117 | 118 | msg.acc.linear.x = ref[10] 119 | msg.acc.linear.y = ref[11] 120 | msg.acc.linear.z = ref[12] 121 | 122 | self.publisher_.publish(msg) -------------------------------------------------------------------------------- /crazychoir_examples/resource/rvizconf.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Grid1 10 | - /Marker1 11 | Splitter Ratio: 0.5 12 | Tree Height: 787 13 | - Class: rviz_common/Selection 14 | Name: Selection 15 | - Class: rviz_common/Tool Properties 16 | Expanded: 17 | - /2D Nav Goal1 18 | - /Publish Point1 19 | Name: Tool Properties 20 | Splitter Ratio: 0.5886790156364441 21 | - Class: rviz_common/Views 22 | Expanded: 23 | - /Current View1 24 | Name: Views 25 | Splitter Ratio: 0.5 26 | Visualization Manager: 27 | Class: "" 28 | Displays: 29 | - Alpha: 0.5 30 | Cell Size: 1 31 | Class: rviz_default_plugins/Grid 32 | Color: 160; 160; 164 33 | Enabled: true 34 | Line Style: 35 | Line Width: 0.029999999329447746 36 | Value: Lines 37 | Name: Grid 38 | Normal Cell Count: 0 39 | Offset: 40 | X: 0 41 | Y: 0 42 | Z: 0 43 | Plane: XY 44 | Plane Cell Count: 10 45 | Reference Frame: my_frame 46 | Value: true 47 | - Class: rviz_default_plugins/Marker 48 | Enabled: true 49 | Name: Marker 50 | Namespaces: 51 | robots: true 52 | Queue Size: 10 53 | Topic: /visualization_marker 54 | Unreliable: false 55 | Value: true 56 | Enabled: true 57 | Global Options: 58 | Background Color: 48; 48; 48 59 | Fixed Frame: my_frame 60 | Frame Rate: 30 61 | Name: root 62 | Tools: 63 | - Class: rviz_default_plugins/MoveCamera 64 | - Class: rviz_default_plugins/Select 65 | - Class: rviz_default_plugins/FocusCamera 66 | - Class: rviz_default_plugins/Measure 67 | Line color: 128; 128; 0 68 | - Class: rviz_default_plugins/SetInitialPose 69 | Topic: /initialpose 70 | - Class: rviz_default_plugins/SetGoal 71 | Topic: /move_base_simple/goal 72 | - Class: rviz_default_plugins/PublishPoint 73 | Single click: true 74 | Topic: /clicked_point 75 | Transformation: 76 | Current: 77 | Class: rviz_default_plugins/TF 78 | Value: true 79 | Views: 80 | Current: 81 | Class: rviz_default_plugins/Orbit 82 | Distance: 13.463741302490234 83 | Enable Stereo Rendering: 84 | Stereo Eye Separation: 0.05999999865889549 85 | Stereo Focal Distance: 1 86 | Swap Stereo Eyes: false 87 | Value: false 88 | Focal Point: 89 | X: 0 90 | Y: 0 91 | Z: 0 92 | Focal Shape Fixed Size: true 93 | Focal Shape Size: 0.05000000074505806 94 | Invert Z Axis: false 95 | Name: Current View 96 | Near Clip Distance: 0.009999999776482582 97 | Pitch: 1.5697963237762451 98 | Target Frame: 99 | Value: Orbit (rviz) 100 | Yaw: 4.71238899230957 101 | Saved: ~ 102 | Window Geometry: 103 | Displays: 104 | collapsed: false 105 | Height: 1016 106 | Hide Left Dock: false 107 | Hide Right Dock: false 108 | QMainWindow State: 000000ff00000000fd0000000400000000000001560000039efc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000039e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004c80000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 109 | Selection: 110 | collapsed: false 111 | Tool Properties: 112 | collapsed: false 113 | Views: 114 | collapsed: false 115 | Width: 1849 116 | X: 71 117 | Y: 27 118 | -------------------------------------------------------------------------------- /crazychoir/crazychoir/gui/gui/splineWidget.py: -------------------------------------------------------------------------------- 1 | from PyQt5.QtWidgets import QWidget, QVBoxLayout 2 | from matplotlib.figure import Figure 3 | from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg, NavigationToolbar2QT as NavigationToolbar 4 | from ..controller.spline import DrawSpline 5 | from matplotlib.widgets import CheckButtons 6 | from ..controller import utils 7 | from .custom_dialogs import SaveDialog 8 | from datetime import datetime 9 | 10 | 11 | class SplineWidget(QWidget): 12 | 13 | def __init__(self,settings): 14 | super(SplineWidget, self).__init__() 15 | self.ax = None 16 | self.check = None 17 | self.toolbar = None 18 | self.canvas = None 19 | self.file_name = None 20 | self.settings = settings 21 | 22 | self.setMinimumWidth(int(settings.get_screen_res().width()*0.15)) 23 | self.setMaximumWidth(int(settings.get_screen_res().width() * 0.7)) 24 | self.setContentsMargins(5,5,5,5) 25 | 26 | self.layout = QVBoxLayout() 27 | self.setLayout(self.layout) 28 | 29 | def plot_spline(self, file_name, time_scale, interpolation_scale): 30 | 31 | if file_name is not None: 32 | self.layout.removeWidget(self.canvas) 33 | self.layout.removeWidget(self.toolbar) 34 | self.canvas = MplCanvasSpline() 35 | 36 | self.toolbar = NavigationToolbar(self.canvas) 37 | 38 | self.ax = [] 39 | x, y = DrawSpline.plot_cubic_spline(file_name, self.canvas.fig, self.canvas.axes, time_scale, interpolation_scale) 40 | self.ax.append(x) 41 | self.ax.append(y) 42 | 43 | self.__make_checkBox__() 44 | self.check[0].on_clicked(self.__on_check_1__) 45 | self.check[1].on_clicked(self.__on_check_2__) 46 | 47 | self.layout.addWidget(self.toolbar) 48 | self.layout.addWidget(self.canvas) 49 | self.setLayout(self.layout) 50 | 51 | def plot_comparison(self,file_name, time_scale, interpolation_scale): 52 | DrawSpline.spline_check(file_name,time_scale, interpolation_scale) 53 | 54 | def __make_checkBox__(self): 55 | 56 | self.check = [] 57 | i = 0 58 | ax = [] 59 | ax.append(self.canvas.fig.add_axes([0.9, 0.65, 0.1, 0.15])) 60 | ax.append(self.canvas.fig.add_axes([0.9, 0.25, 0.1, 0.15])) 61 | 62 | for plot in self.ax: 63 | labels = [str(line.get_label()) for line in plot] 64 | visibility = [line.get_visible() for line in plot] 65 | self.check.append(CheckButtons(ax[i], labels, visibility)) 66 | i += 1 67 | 68 | def __on_check_1__(self, label): 69 | 70 | ax = self.ax[0] 71 | labels = [str(line.get_label()) for line in ax] 72 | 73 | index = labels.index(label) 74 | ax[index].set_visible(not ax[index].get_visible()) 75 | 76 | tmp = ax.copy() 77 | # pop not visible 78 | i = 0 79 | for line in ax: 80 | if not line.get_visible(): 81 | tmp.pop(i) 82 | else: 83 | i += 1 84 | 85 | if len(tmp) > 0: 86 | max_value = utils.get_max_value(tmp) 87 | self.canvas.axes[0].set_ylim(-max_value, max_value) 88 | 89 | self.canvas.draw() 90 | 91 | def __on_check_2__(self, label): 92 | 93 | ax = self.ax[1] 94 | labels = [str(line.get_label()) for line in ax] 95 | 96 | index = labels.index(label) 97 | ax[index].set_visible(not ax[index].get_visible()) 98 | 99 | tmp = ax.copy() 100 | # pop not visible 101 | i = 0 102 | for line in ax: 103 | if not line.get_visible(): 104 | tmp.pop(i) 105 | else: 106 | i += 1 107 | 108 | if len(tmp) > 0: 109 | max_value = utils.get_max_value(tmp) 110 | self.canvas.axes[1].set_ylim(-max_value, max_value) 111 | 112 | self.canvas.draw() 113 | 114 | def save_spline(self, source_file, save_path, interpolation_distance): 115 | 116 | dlg = SaveDialog() 117 | result = dlg.exec() 118 | 119 | if result == 1: 120 | filename = dlg.filename.text() 121 | if filename == "": 122 | filename = datetime.now().strftime('%d-%m-%Y_%H-%M-%S') 123 | filename = "/"+ filename + "_poly.csv" 124 | utils.DrawSpline.print_poly_to_file(source_file, save_path+filename, interpolation_distance) 125 | 126 | 127 | class MplCanvasSpline(FigureCanvasQTAgg): 128 | 129 | def __init__(self, parent=None, width=5, height=4, dpi=100): 130 | self.fig = Figure(figsize=(width, height), dpi=dpi) 131 | self.axes = [] 132 | self.axes.append(self.fig.add_subplot(2, 1, 1, autoscale_on=True)) 133 | self.axes.append(self.fig.add_subplot(2, 1, 2, autoscale_on=True)) 134 | super(MplCanvasSpline, self).__init__(self.fig) 135 | -------------------------------------------------------------------------------- /crazychoir_examples/launch/task_assignment_vicon.launch.py: -------------------------------------------------------------------------------- 1 | 2 | import numpy as np 3 | 4 | from launch import LaunchDescription 5 | from launch_ros.actions import Node 6 | from launch.actions import TimerAction 7 | 8 | from disropt.utils.graph_constructor import binomial_random_graph 9 | 10 | frequency = 100 # [Hz] 11 | 12 | def generate_launch_description(): 13 | 14 | # Set uri address for each quadrotor 15 | uris = [ 16 | 'radio://0/80/2M/E7E7E7E701', 17 | 'radio://0/80/2M/E7E7E7E702', 18 | 'radio://1/90/2M/E7E7E7E703', 19 | 'radio://1/90/2M/E7E7E7E704', 20 | 'radio://2/100/2M/E7E7E7E705', 21 | 'radio://2/100/2M/E7E7E7E706', 22 | ] 23 | 24 | # number of agents 25 | N = 6 26 | 27 | # generate communication graph (this function also sets the seed) 28 | Adj = binomial_random_graph(N, 0.2, seed=3) 29 | 30 | # generate initial positions in [-3, 3] with z = 0 31 | P = np.zeros((N, 3)) 32 | P[0] = np.array([ 1.0, 1.0, 0.015]) 33 | P[1] = np.array([ 0.0, 1.5, 0.015]) 34 | P[2] = np.array([-1.0, 1.0, 0.015]) 35 | P[3] = np.array([-1.0,-1.0, 0.015]) 36 | P[4] = np.array([ 0.0,-1.5, 0.015]) 37 | P[5] = np.array([ 1.0,-1.0, 0.015]) 38 | 39 | # initialize launch description 40 | launch_description = [] # launched immediately 41 | radio_launch = [] 42 | 43 | # Launch control Panel 44 | launch_description.append(Node( 45 | package='crazychoir_examples', 46 | executable='crazychoir_task_assignment_vicon_gui', 47 | output='screen', 48 | parameters=[{ 49 | 'n_agents': N, 50 | }])) 51 | 52 | # Launch radio node 53 | radios = set([int(uri.split('/')[2]) for uri in uris]) 54 | for r in radios: 55 | uris_r = [uri for uri in uris if int(uri.split('/')[2]) == r] 56 | agent_ids_r = [uris.index(uri_r) for uri_r in uris_r] 57 | radio_launch.append(Node( 58 | package='crazychoir_examples', 59 | executable='crazychoir_task_assignment_vicon_radio', 60 | output='screen', 61 | namespace='radio_{}'.format(r), 62 | parameters=[{ 63 | 'uris': uris_r, 64 | 'agent_ids': agent_ids_r, 65 | 'cmd_topic': 'traj_params', 66 | }])) 67 | 68 | # Launch vicon node 69 | launch_description.append(Node( 70 | package='vicon_receiver', 71 | executable='vicon_client', 72 | output='screen', 73 | parameters=[{ 74 | 'hostname': '192.168.10.1', 75 | 'buffer_size': 200, 76 | 'namespace': 'vicon'}] 77 | )) 78 | 79 | # add task table executable 80 | launch_description.append(Node( 81 | package='crazychoir_examples', 82 | executable='crazychoir_task_assignment_vicon_table', 83 | output='screen', 84 | prefix='xterm -title "Table" -hold -e', 85 | parameters=[{'N': N}])) 86 | 87 | # add executables for each robot 88 | for i in range(N): 89 | 90 | uri = uris[i] 91 | in_neighbors = np.nonzero(Adj[:, i])[0].tolist() 92 | out_neighbors = np.nonzero(Adj[i, :])[0].tolist() 93 | initial_position = P[i, :].tolist() 94 | 95 | # guidance 96 | launch_description.append(Node( 97 | package='crazychoir_examples', 98 | executable='crazychoir_task_assignment_vicon_guidance', 99 | output='screen', 100 | namespace='agent_{}'.format(i), 101 | parameters=[{ 102 | 'agent_id': i, 103 | 'N': N, 104 | 'in_neigh': in_neighbors, 105 | 'out_neigh': out_neighbors, 106 | 'vicon_id': uri[-1], 107 | }])) 108 | 109 | # simple guidance (takeoff and landing) 110 | launch_description.append(Node( 111 | package='crazychoir_examples', 112 | executable='crazychoir_task_assignment_vicon_simple_guidance', 113 | output='screen', 114 | namespace='agent_{}'.format(i), 115 | parameters=[{ 116 | 'agent_id': i, 117 | 'vicon_id': uri[-1], 118 | 'init_pos': initial_position, 119 | }])) 120 | 121 | # planner 122 | launch_description.append(Node( 123 | package='crazychoir_examples', 124 | executable='crazychoir_task_assignment_vicon_planner', 125 | output='screen', 126 | namespace='agent_{}'.format(i), 127 | parameters=[{ 128 | 'agent_id': i, 129 | 'vicon_id': uri[-1], 130 | 'freq': frequency, 131 | }])) 132 | 133 | # include delayed radio launcher 134 | timer_action = TimerAction(period=5.0, actions=[LaunchDescription(radio_launch)]) 135 | launch_description.append(timer_action) 136 | 137 | return LaunchDescription(launch_description) 138 | -------------------------------------------------------------------------------- /crazychoir_examples/launch/aggregative_lighthouse.launch.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | from launch import LaunchDescription 4 | from launch_ros.actions import Node 5 | from disropt.utils.graph_constructor import binomial_random_graph, ring_graph, metropolis_hastings 6 | from launch.actions import TimerAction 7 | 8 | 9 | freq_guidance = 5 # [Hz] 10 | freq_reference = 10 # [Hz] 11 | freq_controller = 10 # [Hz] 12 | 13 | 14 | def generate_launch_description(): 15 | 16 | # Set uri address for each quadrotor 17 | uris = [ 18 | 'radio://0/40/2M/E7E7E7E704', 19 | 'radio://0/40/2M/E7E7E7E705', 20 | 'radio://0/40/2M/E7E7E7E706' 21 | ] 22 | 23 | N = 3 24 | max_iter = 20000 25 | 26 | 27 | # generate communication graph (this function also sets the seed) 28 | Adj = binomial_random_graph(N, p=0.5, seed=3) 29 | W = metropolis_hastings(Adj) 30 | 31 | P = np.zeros((N, 3)) 32 | P[0] = np.array([-0.3, -0.5, 0.0]) 33 | P[1] = np.array([ 0.2, -0.5, 0.0]) 34 | P[2] = np.array([ 0.7, -0.5, 0.0]) 35 | 36 | # Target 37 | target = [0.2, 0.0, 1.0] 38 | 39 | # Intruders 40 | intruders = np.zeros((N, 3)) 41 | intruders[0] = np.array([-0.8, 1.0, 1.0]) 42 | intruders[1] = np.array([-0.8, -1.0, 1.0]) 43 | intruders[2] = np.array([ 1.2, 0.0, 1.0]) 44 | 45 | 46 | launch_description = [] 47 | radio_launch = [] 48 | 49 | # Launch control Panel 50 | launch_description.append(Node( 51 | package='crazychoir_examples', 52 | executable='crazychoir_aggregative_lighthouse_gui', 53 | output='screen', 54 | parameters=[{ 55 | 'n_agents': N, 56 | }])) 57 | 58 | # Launch radio node 59 | radios = set([int(uri.split('/')[2]) for uri in uris]) 60 | for r in radios: 61 | uris_r = [uri for uri in uris if int(uri.split('/')[2]) == r] 62 | agent_ids_r = [uris.index(uri_r) for uri_r in uris_r] 63 | radio_launch.append(Node( 64 | package='crazychoir_examples', 65 | executable='crazychoir_aggregative_lighthouse_radio', 66 | prefix='xterm -title "radio_{}" -hold -e'.format(0), 67 | namespace='radio_{}'.format(r), 68 | parameters=[{ 69 | 'uris': uris_r, 70 | 'agent_ids': agent_ids_r, 71 | 'cmd_topic': 'cmd_vel', 72 | 'logger' : True, 73 | }])) 74 | 75 | # Add executables for each robot 76 | for i in range(N): 77 | initial_position = P[i, :].tolist() 78 | 79 | # Guidance 80 | in_neighbors = np.nonzero(Adj[:, i])[0].tolist() 81 | out_neighbors = np.nonzero(Adj[i, :])[0].tolist() 82 | weights = W[i,:].tolist() 83 | launch_description.append(Node( 84 | package='crazychoir_examples', 85 | executable='crazychoir_aggregative_lighthouse_guidance_aggregative', 86 | output='screen', 87 | namespace=f'agent_{i}', 88 | parameters=[{ 89 | 'freq': freq_guidance, 90 | 'agent_id': i, 91 | 'N': N, 92 | 'in_neigh': in_neighbors, 93 | 'out_neigh': out_neighbors, 94 | 'weights': weights, 95 | 'init_pos': initial_position, 96 | 'freq_reference': freq_reference, 97 | 'max_iter': max_iter, 98 | 'target': target, 99 | 'intruder': intruders[i].tolist(), 100 | }])) 101 | 102 | 103 | # Simple guidance (takeoff and landing) 104 | launch_description.append(Node( 105 | package='crazychoir_examples', 106 | executable='crazychoir_aggregative_lighthouse_simple_guidance', 107 | output='screen', 108 | namespace=f'agent_{i}', 109 | parameters=[{ 110 | 'agent_id': i, 111 | 'init_pos': initial_position, 112 | }])) 113 | 114 | # Controller 115 | launch_description.append(Node( 116 | package='crazychoir_examples', 117 | executable='crazychoir_aggregative_lighthouse_controller', 118 | namespace=f'agent_{i}', 119 | output='screen', 120 | parameters=[{ 121 | 'freq': freq_controller, 122 | 'agent_id': i, 123 | }])) 124 | 125 | # Trajectory 126 | launch_description.append(Node( 127 | package='crazychoir_examples', 128 | executable='crazychoir_aggregative_lighthouse_trajectory', 129 | namespace=f'agent_{i}', 130 | output='screen', 131 | parameters=[{ 132 | 'freq': freq_reference, 133 | 'agent_id': i, 134 | }])) 135 | 136 | # include delayed radio launcher 137 | timer_action = TimerAction(period=1.0, actions=[LaunchDescription(radio_launch)]) 138 | launch_description.append(timer_action) 139 | 140 | return LaunchDescription(launch_description) 141 | -------------------------------------------------------------------------------- /crazychoir_examples/worlds/obj_crazyflie.wbt: -------------------------------------------------------------------------------- 1 | 2 | Robot { 3 | name "$NAME" 4 | translation $X $Y $Z 5 | controller "" 6 | children [ 7 | Solid { 8 | translation 0 0 -0.015 9 | children [ 10 | DEF battery Shape { 11 | appearance PBRAppearance { 12 | baseColor 0.5 0.5 0.6 13 | metalness 0.1 14 | emissiveIntensity 0 15 | } 16 | geometry Mesh { 17 | url [ 18 | "meshes/battery.stl" 19 | ] 20 | } 21 | } 22 | DEF battery_holder Shape { 23 | appearance PBRAppearance { 24 | baseColor 0 0 0 25 | metalness 0.2 26 | } 27 | geometry Mesh { 28 | url [ 29 | "meshes/battery_holder.stl" 30 | ] 31 | } 32 | } 33 | DEF motors Shape { 34 | appearance PBRAppearance { 35 | baseColor 0.5 0.5 0.5 36 | emissiveColor 0.4 0.4 0.4 37 | } 38 | geometry Mesh { 39 | url [ 40 | "meshes/4_motors.stl" 41 | ] 42 | } 43 | } 44 | DEF motormounts Shape { 45 | appearance PBRAppearance { 46 | transparency 0.1 47 | metalness 0.2 48 | emissiveColor 0.5 0.5 0.5 49 | } 50 | geometry Mesh { 51 | url [ 52 | "meshes/4_motormounts.stl" 53 | ] 54 | } 55 | } 56 | DEF pinheader Shape { 57 | appearance DEF metal PBRAppearance { 58 | baseColor 0.5 0.5 0.5 59 | metalness 0.8 60 | emissiveColor 0.4 0.4 0.4 61 | } 62 | geometry Mesh { 63 | url [ 64 | "meshes/2_pinheaders.stl" 65 | ] 66 | } 67 | } 68 | DEF body Shape { 69 | appearance DEF PCB PBRAppearance { 70 | baseColor 0 0 0 71 | roughness 0.3 72 | metalness 0.5 73 | } 74 | geometry Mesh { 75 | url [ 76 | "meshes/cf_body.stl" 77 | ] 78 | } 79 | } 80 | ] 81 | name "body" 82 | } 83 | GPS { 84 | } 85 | Gyro { 86 | } 87 | InertialUnit { 88 | } 89 | DEF m1 Propeller { 90 | shaftAxis 0 0 1 91 | centerOfThrust 0.031 -0.031 0.008 92 | thrustConstants -4e-05 0 93 | torqueConstants 2.4e-06 0 94 | device RotationalMotor { 95 | name "m1_motor" 96 | maxVelocity 600 97 | maxTorque 30 98 | } 99 | slowHelix Solid { 100 | translation 0.031 -0.031 0.008 101 | children [ 102 | Shape { 103 | appearance DEF plastic PBRAppearance { 104 | baseColor 0 0 0 105 | metalness 0.3 106 | } 107 | geometry Mesh { 108 | url [ 109 | "meshes/ccw_prop.stl" 110 | ] 111 | } 112 | } 113 | ] 114 | } 115 | } 116 | DEF m2 Propeller { 117 | shaftAxis 0 0 1 118 | centerOfThrust -0.031 -0.031 0.008 119 | thrustConstants 4e-05 0 120 | torqueConstants 2.4e-06 0 121 | device RotationalMotor { 122 | name "m2_motor" 123 | maxVelocity 600 124 | maxTorque 30 125 | } 126 | slowHelix Solid { 127 | translation -0.031 -0.031 0.008 128 | children [ 129 | Shape { 130 | appearance USE plastic 131 | geometry Mesh { 132 | url [ 133 | "meshes/cw_prop.stl" 134 | ] 135 | } 136 | } 137 | ] 138 | } 139 | } 140 | DEF m3 Propeller { 141 | shaftAxis 0 0 1 142 | centerOfThrust -0.031 0.031 0.008 143 | thrustConstants -4e-05 0 144 | torqueConstants 2.4e-06 0 145 | device RotationalMotor { 146 | name "m3_motor" 147 | maxVelocity 600 148 | maxTorque 30 149 | } 150 | slowHelix Solid { 151 | translation -0.031 0.031 0.008 152 | children [ 153 | Shape { 154 | appearance USE plastic 155 | geometry Mesh { 156 | url [ 157 | "meshes/ccw_prop.stl" 158 | ] 159 | } 160 | } 161 | ] 162 | } 163 | } 164 | DEF m4 Propeller { 165 | shaftAxis 0 0 1 166 | centerOfThrust 0.031 0.031 0.008 167 | thrustConstants 4e-05 0 168 | torqueConstants 2.4e-06 0 169 | device RotationalMotor { 170 | name "m4_motor" 171 | maxVelocity 600 172 | maxTorque 30 173 | } 174 | slowHelix DEF prop Solid { 175 | translation 0.031 0.031 0.008 176 | children [ 177 | DEF prop Shape { 178 | appearance USE plastic 179 | geometry Mesh { 180 | url [ 181 | "meshes/cw_prop.stl" 182 | ] 183 | } 184 | } 185 | ] 186 | } 187 | } 188 | ] 189 | boundingObject Cylinder { 190 | height 0.03 191 | radius 0.05 192 | } 193 | physics Physics { 194 | density -1 195 | mass 0.038 196 | } 197 | } -------------------------------------------------------------------------------- /crazychoir_examples/worlds/obj_crazyflie_red.wbt: -------------------------------------------------------------------------------- 1 | 2 | Robot { 3 | name "$NAME" 4 | translation $X $Y $Z 5 | controller "" 6 | children [ 7 | Solid { 8 | translation 0 0 -0.015 9 | children [ 10 | DEF battery Shape { 11 | appearance PBRAppearance { 12 | baseColor 0.5 0.5 0.6 13 | metalness 0.1 14 | emissiveIntensity 0 15 | } 16 | geometry Mesh { 17 | url [ 18 | "meshes/battery.stl" 19 | ] 20 | } 21 | } 22 | DEF battery_holder Shape { 23 | appearance PBRAppearance { 24 | baseColor 0 0 0 25 | metalness 0.2 26 | } 27 | geometry Mesh { 28 | url [ 29 | "meshes/battery_holder.stl" 30 | ] 31 | } 32 | } 33 | DEF motors Shape { 34 | appearance PBRAppearance { 35 | baseColor 0.5 0.5 0.5 36 | emissiveColor 0.4 0.4 0.4 37 | } 38 | geometry Mesh { 39 | url [ 40 | "meshes/4_motors.stl" 41 | ] 42 | } 43 | } 44 | DEF motormounts Shape { 45 | appearance PBRAppearance { 46 | transparency 0.1 47 | metalness 0.2 48 | emissiveColor 0.5 0.5 0.5 49 | } 50 | geometry Mesh { 51 | url [ 52 | "meshes/4_motormounts.stl" 53 | ] 54 | } 55 | } 56 | DEF pinheader Shape { 57 | appearance DEF metal PBRAppearance { 58 | baseColor 0.5 0.5 0.5 59 | metalness 0.8 60 | emissiveColor 0.4 0.4 0.4 61 | } 62 | geometry Mesh { 63 | url [ 64 | "meshes/2_pinheaders.stl" 65 | ] 66 | } 67 | } 68 | DEF body Shape { 69 | appearance DEF PCB PBRAppearance { 70 | baseColor 0 0 0 71 | roughness 0.3 72 | metalness 0.5 73 | } 74 | geometry Mesh { 75 | url [ 76 | "meshes/cf_body.stl" 77 | ] 78 | } 79 | } 80 | ] 81 | name "body" 82 | } 83 | GPS { 84 | } 85 | Gyro { 86 | } 87 | InertialUnit { 88 | } 89 | DEF m1 Propeller { 90 | shaftAxis 0 0 1 91 | centerOfThrust 0.031 -0.031 0.008 92 | thrustConstants -4e-05 0 93 | torqueConstants 2.4e-06 0 94 | device RotationalMotor { 95 | name "m1_motor" 96 | maxVelocity 600 97 | maxTorque 30 98 | } 99 | slowHelix Solid { 100 | translation 0.031 -0.031 0.008 101 | children [ 102 | Shape { 103 | appearance DEF plastic PBRAppearance { 104 | baseColor 1 0 0 105 | metalness 0.3 106 | } 107 | geometry Mesh { 108 | url [ 109 | "meshes/ccw_prop.stl" 110 | ] 111 | } 112 | } 113 | ] 114 | } 115 | } 116 | DEF m2 Propeller { 117 | shaftAxis 0 0 1 118 | centerOfThrust -0.031 -0.031 0.008 119 | thrustConstants 4e-05 0 120 | torqueConstants 2.4e-06 0 121 | device RotationalMotor { 122 | name "m2_motor" 123 | maxVelocity 600 124 | maxTorque 30 125 | } 126 | slowHelix Solid { 127 | translation -0.031 -0.031 0.008 128 | children [ 129 | Shape { 130 | appearance USE plastic 131 | geometry Mesh { 132 | url [ 133 | "meshes/cw_prop.stl" 134 | ] 135 | } 136 | } 137 | ] 138 | } 139 | } 140 | DEF m3 Propeller { 141 | shaftAxis 0 0 1 142 | centerOfThrust -0.031 0.031 0.008 143 | thrustConstants -4e-05 0 144 | torqueConstants 2.4e-06 0 145 | device RotationalMotor { 146 | name "m3_motor" 147 | maxVelocity 600 148 | maxTorque 30 149 | } 150 | slowHelix Solid { 151 | translation -0.031 0.031 0.008 152 | children [ 153 | Shape { 154 | appearance USE plastic 155 | geometry Mesh { 156 | url [ 157 | "meshes/ccw_prop.stl" 158 | ] 159 | } 160 | } 161 | ] 162 | } 163 | } 164 | DEF m4 Propeller { 165 | shaftAxis 0 0 1 166 | centerOfThrust 0.031 0.031 0.008 167 | thrustConstants 4e-05 0 168 | torqueConstants 2.4e-06 0 169 | device RotationalMotor { 170 | name "m4_motor" 171 | maxVelocity 600 172 | maxTorque 30 173 | } 174 | slowHelix DEF prop Solid { 175 | translation 0.031 0.031 0.008 176 | children [ 177 | DEF prop Shape { 178 | appearance USE plastic 179 | geometry Mesh { 180 | url [ 181 | "meshes/cw_prop.stl" 182 | ] 183 | } 184 | } 185 | ] 186 | } 187 | } 188 | ] 189 | boundingObject Cylinder { 190 | height 0.03 191 | radius 0.05 192 | } 193 | physics Physics { 194 | density -1 195 | mass 0.038 196 | } 197 | } --------------------------------------------------------------------------------