diff --git a/CustomRobots/drone_cat_mouse/models/my_city_v3/meshes/city.glb b/CustomRobots/drone_cat_mouse/models/my_city_v3/meshes/city.glb new file mode 100644 index 000000000..079ca6ba7 Binary files /dev/null and b/CustomRobots/drone_cat_mouse/models/my_city_v3/meshes/city.glb differ diff --git a/CustomRobots/drone_cat_mouse/models/my_city_v3/model.config b/CustomRobots/drone_cat_mouse/models/my_city_v3/model.config new file mode 100644 index 000000000..05e8a10e6 --- /dev/null +++ b/CustomRobots/drone_cat_mouse/models/my_city_v3/model.config @@ -0,0 +1,10 @@ + + + my_city_v3 + 1.0 + model.sdf + + anish + + city mesh from blender, kept it low poly on purpose - about 65K faces, 7MB glb + diff --git a/CustomRobots/drone_cat_mouse/models/my_city_v3/model.sdf b/CustomRobots/drone_cat_mouse/models/my_city_v3/model.sdf new file mode 100644 index 000000000..05cf48dcc --- /dev/null +++ b/CustomRobots/drone_cat_mouse/models/my_city_v3/model.sdf @@ -0,0 +1,27 @@ + + + + + true + + 0 0 0 1.5707963 0 0 + + + + model://my_city_v3/meshes/city.glb + 1 1 1 + + + + + + + model://my_city_v3/meshes/city.glb + 1 1 1 + + + + + + diff --git a/Launchers/drone_cat_mouse/control_modes.yaml b/Launchers/drone_cat_mouse/control_modes.yaml new file mode 100644 index 000000000..c5c870ded --- /dev/null +++ b/Launchers/drone_cat_mouse/control_modes.yaml @@ -0,0 +1,34 @@ +#---------------------------------------------------------------- +# A complete control mode is an 8 bits flag +# (4bits control mode + 2 yaw mode bits + 2 reference frame bits) +# +# ------------- mode codification (4 bits) ---------------------- +# +# unset = 0 = 0b0000 +# hover = 1 = 0b0001 +# acro = 2 = 0b0010 +# attitude = 3 = 0b0011 +# speed = 4 = 0b0100 +# speed_in_a_plane = 5 = 0b0101 +# position = 6 = 0b0110 +# trajectory = 7 = 0b0111 +# +#-------------- yaw codification -------------------------------- +# +# angle = 0 = 0b00 +# speed = 1 = 0b01 +# +# frame codification +# +# local_frame_flu = 0 = 0b00 +# global_frame_enu = 1 = 0b01 +# global_frame_lla = 2 = 0b10 +# +#----------------------------------------------------------------- + +available_modes: + - 0b00010000 # HOVER + - 0b01000100 # SPEED with yaw SPEED in the LOCAL_FLU_FRAME + - 0b01000101 # SPEED with yaw SPEED in the GLOBAL_ENU_FRAME + - 0b01100001 # POSITION with yaw ANGLE in the GLOBAL_ENU_FRAME + - 0b01100101 # POSITION with yaw SPEED in the GLOBAL_ENU_FRAME diff --git a/Launchers/drone_cat_mouse/gazebo.launch.py b/Launchers/drone_cat_mouse/gazebo.launch.py new file mode 100644 index 000000000..9264c570c --- /dev/null +++ b/Launchers/drone_cat_mouse/gazebo.launch.py @@ -0,0 +1,286 @@ +""" +Launch file for the drone cat-mouse chase exercise. +maintainer: Anish Kumar (anishkumar59085@gmail.com) + +Starts gazebo harmonic with the city world, then brings up the full AS2 +stack for both drones (drone_cat + drone_mouse). + +how the data flows for each drone: + Gz OdometryPublisher -> ground_truth_bridge -> state_estimator -> TF tree + -> motion_controller (pid) -> platform_gazebo -> gz cmd_vel -> simulation + +to arm after launch (the motors auto-enable at t+8s): + ros2 service call /{drone}/set_arming_state data: true + ros2 service call /{drone}/set_offboard_mode data: true + ros2 action send_goal /{drone}/TakeoffBehavior ... +""" + +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + ExecuteProcess, + GroupAction, + IncludeLaunchDescription, + SetEnvironmentVariable, + TimerAction, + LogInfo, +) +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + + pkg_dir = get_package_share_directory('drone_cat_mouse_chase') + pkg_ros_gz_sim = get_package_share_directory('ros_gz_sim') + + world_file = os.path.join(pkg_dir, 'worlds', 'my_city_v3.world') + models_dir = os.path.join(pkg_dir, 'models') + config_dir = os.path.join(pkg_dir, 'config') + + + # tell gz where our custom models are (city mesh etc) + gz_resource_path = SetEnvironmentVariable( + name='GZ_SIM_RESOURCE_PATH', + value=[models_dir, ':', os.environ.get('GZ_SIM_RESOURCE_PATH', '')], + ) + + # =================== launch args =================== + verbose_arg = DeclareLaunchArgument( + 'verbose', default_value='4', + description='Gazebo verbosity (0-4)') + use_sim_time_arg = DeclareLaunchArgument( + 'use_sim_time', default_value='true') + + # =================== gazebo sim =================== + gz_sim = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(pkg_ros_gz_sim, 'launch', 'gz_sim.launch.py')), + launch_arguments={ + 'gz_args': ['-r -v ', LaunchConfiguration('verbose'), ' ', world_file], + }.items(), + ) + + # =================== clock bridge =================== + clock_bridge = Node( + package='ros_gz_bridge', + executable='parameter_bridge', + name='clock_bridge', + output='screen', + arguments=['/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock'], + parameters=[{'use_sim_time': True}], + ) + + # =================== per-drone setup =================== + drone_names = ['drone_cat', 'drone_mouse'] + world_name = 'my_city_world' + + # all drones share these config files + platform_config = os.path.join(config_dir, 'platform_config_file.yaml') + control_modes = os.path.join(config_dir, 'control_modes.yaml') + state_est_config = os.path.join(config_dir, 'state_estimator.yaml') + motion_ctrl_config = os.path.join(config_dir, 'motion_controller.yaml') + pid_config = os.path.join(config_dir, 'pid_speed_controller.yaml') + + all_drone_nodes = [] + + for drone in drone_names: + + # ground truth bridge — this is the only localization source + # reads gz odometry and publishes it as ROS pose+twist msgs + gt_bridge = Node( + package='as2_gazebo_assets', + executable='ground_truth_bridge', + name='ground_truth_bridge', + namespace=drone, + output='screen', + parameters=[{ + 'use_sim_time': True, + 'name_space': drone, + 'pose_frame_id': 'earth', + 'twist_frame_id': f'{drone}/base_link', + }], + ) + + # ros-gz parameter bridge for each drone + # handles cmd_vel, motor enable, imu, camera topics + bridge_args = [ + # velocity cmd goes ROS -> GZ + f'/model/{drone}/cmd_vel@geometry_msgs/msg/Twist]gz.msgs.Twist', + # arm/enable also ROS -> GZ + f'/model/{drone}/velocity_controller/enable@std_msgs/msg/Bool]gz.msgs.Boolean', + # imu comes back from GZ -> ROS (the imu sensor is nested inside the drone model) + f'/world/{world_name}/model/{drone}/model/imu/link/internal/sensor/imu/imu' + f'@sensor_msgs/msg/Imu[gz.msgs.IMU', + # camera image GZ -> ROS + f'/world/{world_name}/model/{drone}/link/base_link/sensor/camera/image' + f'@sensor_msgs/msg/Image[gz.msgs.Image', + # camera info GZ -> ROS + f'/world/{world_name}/model/{drone}/link/base_link/sensor/camera/camera_info' + f'@sensor_msgs/msg/CameraInfo[gz.msgs.CameraInfo', + ] + # remap the long gz topic names to somthing AS2 expects + bridge_remaps = [ + (f'/world/{world_name}/model/{drone}/model/imu/link/internal/sensor/imu/imu', + f'/{drone}/sensor_measurements/imu'), + (f'/world/{world_name}/model/{drone}/link/base_link/sensor/camera/image', + f'/{drone}/sensor_measurements/camera/image_raw'), + (f'/world/{world_name}/model/{drone}/link/base_link/sensor/camera/camera_info', + f'/{drone}/sensor_measurements/camera/camera_info'), + ] + gz_bridge = Node( + package='ros_gz_bridge', + executable='parameter_bridge', + name=f'{drone}_bridge', + output='screen', + arguments=bridge_args, + remappings=bridge_remaps, + parameters=[{'use_sim_time': True}], + ) + + # the full AS2 stack — platform, estimator, controller, behaviours + as2_stack = GroupAction([ + LogInfo(msg=f'[AS2] launching stack for {drone}'), + + # platform node — translates AS2 commands into gz cmd_vel + arm msgs + Node( + package='as2_platform_gazebo', + executable='as2_platform_gazebo_node', + name='platform', + namespace=drone, + output='screen', + parameters=[ + {'use_sim_time': True}, + {'cmd_vel_topic': f'/model/{drone}/cmd_vel'}, + {'arm_topic': f'/model/{drone}/velocity_controller/enable'}, + {'acro_topic': f'/gz/{drone}/acro'}, + {'control_modes_file': control_modes}, + platform_config, + ], + ), + + # state estimator — uses ground_truth plugin to read the bridge output + # publishes the TF tree and self_localization topics + Node( + package='as2_state_estimator', + executable='as2_state_estimator_node', + name='state_estimator', + namespace=drone, + output='screen', + parameters=[ + {'use_sim_time': True}, + {'plugin_name': 'ground_truth'}, + {'use_gps': False}, + {'set_origin_on_start': False}, + {'use_gazebo_tf': False}, + state_est_config, + ], + ), + + # motion controller — pid speed controller + Node( + package='as2_motion_controller', + executable='as2_motion_controller_node', + name='controller_manager', + namespace=drone, + output='screen', + parameters=[ + {'use_sim_time': True}, + {'plugin_name': 'pid_speed_controller'}, + motion_ctrl_config, + pid_config, + ], + ), + + # takeoff behaviour + Node( + package='as2_behaviors_motion', + executable='takeoff_behavior_node', + namespace=drone, + output='screen', + parameters=[ + {'use_sim_time': True}, + {'plugin_name': 'takeoff_plugin_speed'}, + {'takeoff_height': 3.0}, + {'takeoff_speed': 1.5}, + {'takeoff_threshold': 0.2}, + {'tf_timeout_threshold': 0.1}, + ], + ), + + # land behaviour + Node( + package='as2_behaviors_motion', + executable='land_behavior_node', + namespace=drone, + output='screen', + parameters=[ + {'use_sim_time': True}, + {'plugin_name': 'land_plugin_speed'}, + {'land_speed': 0.5}, + {'land_speed_condition_percentage': 0.2}, + {'land_condition_height': 0.2}, + {'land_height': -10.0}, + {'land_position_condition_time': 1.0}, + {'tf_timeout_threshold': 0.1}, + ], + ), + + # go-to — used for initial positioning + Node( + package='as2_behaviors_motion', + executable='go_to_behavior_node', + namespace=drone, + output='screen', + parameters=[ + {'use_sim_time': True}, + {'plugin_name': 'go_to_plugin_position'}, + {'go_to_speed': 2.0}, + {'go_to_threshold': 0.3}, + {'tf_timeout_threshold': 0.1}, + ], + ), + + # follow-reference — the cat uses this to chase the mouse + Node( + package='as2_behaviors_motion', + executable='follow_reference_behavior_node', + namespace=drone, + output='screen', + parameters=[ + {'use_sim_time': True}, + {'plugin_name': 'follow_reference_plugin_position'}, + {'follow_reference_max_speed_x': 3.0}, + {'follow_reference_max_speed_y': 3.0}, + {'follow_reference_max_speed_z': 1.5}, + {'tf_timeout_threshold': 0.1}, + ], + ), + ]) + + all_drone_nodes.extend([gt_bridge, gz_bridge, as2_stack]) + + # auto-enable the gz velocity controllers after 8s delay + # without this the motors wont spin even if you arm through ROS + motor_enables = [ + TimerAction(period=8.0, actions=[ + ExecuteProcess( + cmd=['gz', 'topic', '-t', + f'/model/{drone}/velocity_controller/enable', + '-m', 'gz.msgs.Boolean', '-p', 'data: true'], + output='screen', + ) + ]) + for drone in drone_names + ] + + return LaunchDescription([ + gz_resource_path, + verbose_arg, + use_sim_time_arg, + gz_sim, + clock_bridge, + ] + all_drone_nodes + motor_enables) diff --git a/Launchers/drone_cat_mouse/motion_controller.yaml b/Launchers/drone_cat_mouse/motion_controller.yaml new file mode 100644 index 000000000..c2457999b --- /dev/null +++ b/Launchers/drone_cat_mouse/motion_controller.yaml @@ -0,0 +1,8 @@ +/**: + ros__parameters: + cmd_freq: 100.0 # how fast the controller sends cmds (Hz) + info_freq: 10.0 # how often it publishes its internal state + odom_frame_id: "odom" # needs to match what state_estimator publishes + base_frame_id: "base_link" + use_bypass: true # bypass lets us send velocity cmds directly without waiting for position convergence + tf_timeout_threshold: 0.05 # if TF lookup takes longer than this something is probbaly wrong diff --git a/Launchers/drone_cat_mouse/pid_speed_controller.yaml b/Launchers/drone_cat_mouse/pid_speed_controller.yaml new file mode 100644 index 000000000..de66a0c88 --- /dev/null +++ b/Launchers/drone_cat_mouse/pid_speed_controller.yaml @@ -0,0 +1,78 @@ +/**: + ros__parameters: + pid_speed_controller: + proportional_limitation: true + use_bypass: true + position_control: + reset_integral: false + antiwindup_cte: 1.0 + alpha: 0.1 + kp: + x: 1.0 + y: 1.0 + z: 1.0 + kd: + x: 0.3 + y: 0.3 + z: 0.3 + ki: + x: 0.01 + y: 0.01 + z: 0.02 + speed_control: + reset_integral: false + antiwindup_cte: 1.0 + alpha: 0.1 + kp: + x: 3.0 + y: 3.0 + z: 5.0 + kd: + x: 0.1 + y: 0.1 + z: 0.1 + ki: + x: 0.01 + y: 0.01 + z: 0.05 + speed_in_a_plane_control: + reset_integral: false + antiwindup_cte: 1.0 + alpha: 0.1 + height: + kp: 5.0 + kd: 0.3 + ki: 0.02 + speed: + kp: + x: 3.0 + y: 3.0 + kd: + x: 0.1 + y: 0.1 + ki: + x: 0.01 + y: 0.01 + trajectory_control: + reset_integral: false + antiwindup_cte: 1.0 + alpha: 0.1 + kp: + x: 1.0 + y: 1.0 + z: 1.0 + kd: + x: 0.3 + y: 0.3 + z: 0.3 + ki: + x: 0.01 + y: 0.01 + z: 0.02 + yaw_control: + reset_integral: false + antiwindup_cte: 1.0 + alpha: 0.1 + kp: 3.0 + kd: 0.1 + ki: 0.0 diff --git a/Launchers/drone_cat_mouse/platform_config_file.yaml b/Launchers/drone_cat_mouse/platform_config_file.yaml new file mode 100644 index 000000000..7ff5a6698 --- /dev/null +++ b/Launchers/drone_cat_mouse/platform_config_file.yaml @@ -0,0 +1,6 @@ +/**: + ros__parameters: + cmd_freq: 100.0 # how fast platform sends commands + info_freq: 10.0 # telemetry publish rate + enable_takeoff_platform: false # these are only for debuging — dont enable in prod + enable_land_platform: false diff --git a/Launchers/drone_cat_mouse/simulation_config.yaml b/Launchers/drone_cat_mouse/simulation_config.yaml new file mode 100644 index 000000000..e4a7b3828 --- /dev/null +++ b/Launchers/drone_cat_mouse/simulation_config.yaml @@ -0,0 +1,35 @@ +# config for the cat mouse chase — two quads in the city world +# both use the same quadrotor_base model just with diffrent names + +world_name: my_city_v3 + +drones: + - model_type: quadrotor_base + model_name: drone_cat + xyz: + - 0.0 + - 5.0 + - 0.2 + rpy: + - 0.0 + - 0.0 + - 0.0 + flight_time: 60 # seconds + payload: + - model: hd_camera + pose: 0.0 0.0 -0.15 0.0 1.5707 0.0 # na dir cam pointing straight down + + - model_type: quadrotor_base + model_name: drone_mouse + xyz: + - 20.0 + - 5.0 + - 0.2 + rpy: + - 0.0 + - 0.0 + - 0.0 + flight_time: 60 # seconds + payload: + - model: hd_camera + pose: 0.0 0.0 -0.15 0.0 1.5707 0.0 # na dir cam pointing straight down diff --git a/Launchers/drone_cat_mouse/state_estimator.yaml b/Launchers/drone_cat_mouse/state_estimator.yaml new file mode 100644 index 000000000..80a64e7c5 --- /dev/null +++ b/Launchers/drone_cat_mouse/state_estimator.yaml @@ -0,0 +1,6 @@ +/**: + ros__parameters: + base_frame: "base_link" + global_ref_frame: "earth" + odom_frame: "odom" + map_frame: "map" diff --git a/Worlds/drone_cat_mouse_city.world b/Worlds/drone_cat_mouse_city.world new file mode 100644 index 000000000..e25bb45e1 --- /dev/null +++ b/Worlds/drone_cat_mouse_city.world @@ -0,0 +1,375 @@ + + + + + + + + + + + + ogre2 + + + + + 0.001 + 1.0 + + + + + true + 0 0 100 0 0 0 + 0.9 0.9 0.9 1 + 0.3 0.3 0.3 1 + -0.5 0.3 -1.0 + + + + 0.6 0.6 0.7 1 + 0.5 0.7 0.9 1 + true + + + + + true + 0 0 -0.05 0 0 0 + + + + + 0 0 1 + 500 500 + + + + + + + 0 0 1 + 500 500 + + + + 0.12 0.42 0.08 1 + 0.12 0.42 0.08 1 + + + + + + + + model://my_city_v3 + 0 0 0 0 0 0 + + + + + model://quadrotor_base + drone_cat + 0 5 0.2 0 0 0 + + + + model/drone_cat + cmd_vel + velocity_controller/enable + base_link + 2.7 2.7 2.7 + 2 3 0.15 + 0.4 0.52 0.18 + 1 1 2 + 5 5 5 + 3 3 3 + + + rotor_0_joint + 8.54858e-06 + 0.016 + 1 + + + rotor_1_joint + 8.54858e-06 + 0.016 + 1 + + + rotor_2_joint + 8.54858e-06 + 0.016 + -1 + + + rotor_3_joint + 8.54858e-06 + 0.016 + -1 + + + + + + + model/drone_cat + rotor_0_joint + rotor_0 + ccw + 0.0125 + 0.025 + 800.0 + 8.54858e-06 + 0.016 + command/motor_speed + 0 + 8.06428e-05 + 1e-06 + motor_speed/0 + 10 + velocity + + + model/drone_cat + rotor_1_joint + rotor_1 + ccw + 0.0125 + 0.025 + 800.0 + 8.54858e-06 + 0.016 + command/motor_speed + 1 + 8.06428e-05 + 1e-06 + motor_speed/1 + 10 + velocity + + + model/drone_cat + rotor_2_joint + rotor_2 + cw + 0.0125 + 0.025 + 800.0 + 8.54858e-06 + 0.016 + command/motor_speed + 2 + 8.06428e-05 + 1e-06 + motor_speed/2 + 10 + velocity + + + model/drone_cat + rotor_3_joint + rotor_3 + cw + 0.0125 + 0.025 + 800.0 + 8.54858e-06 + 0.016 + command/motor_speed + 3 + 8.06428e-05 + 1e-06 + motor_speed/3 + 10 + velocity + + + + + 3 + 100 + + + + + true + true + false + false + true + false + true + true + 100 + + + + + + model://quadrotor_base + drone_mouse + 20 5 0.2 0 0 0 + + + + model/drone_mouse + cmd_vel + velocity_controller/enable + base_link + 2.7 2.7 2.7 + 2 3 0.15 + 0.4 0.52 0.18 + 1 1 2 + 5 5 5 + 3 3 3 + + + rotor_0_joint + 8.54858e-06 + 0.016 + 1 + + + rotor_1_joint + 8.54858e-06 + 0.016 + 1 + + + rotor_2_joint + 8.54858e-06 + 0.016 + -1 + + + rotor_3_joint + 8.54858e-06 + 0.016 + -1 + + + + + + + model/drone_mouse + rotor_0_joint + rotor_0 + ccw + 0.0125 + 0.025 + 800.0 + 8.54858e-06 + 0.016 + command/motor_speed + 0 + 8.06428e-05 + 1e-06 + motor_speed/0 + 10 + velocity + + + model/drone_mouse + rotor_1_joint + rotor_1 + ccw + 0.0125 + 0.025 + 800.0 + 8.54858e-06 + 0.016 + command/motor_speed + 1 + 8.06428e-05 + 1e-06 + motor_speed/1 + 10 + velocity + + + model/drone_mouse + rotor_2_joint + rotor_2 + cw + 0.0125 + 0.025 + 800.0 + 8.54858e-06 + 0.016 + command/motor_speed + 2 + 8.06428e-05 + 1e-06 + motor_speed/2 + 10 + velocity + + + model/drone_mouse + rotor_3_joint + rotor_3 + cw + 0.0125 + 0.025 + 800.0 + 8.54858e-06 + 0.016 + command/motor_speed + 3 + 8.06428e-05 + 1e-06 + motor_speed/3 + 10 + velocity + + + + + 3 + 100 + + + + + true + true + false + false + true + false + true + true + 100 + + + + + + + 10 -30 30 0 0.5 1.57 + + + + +