diff --git a/README.md b/README.md index 3488c20..67b9cb4 100644 --- a/README.md +++ b/README.md @@ -30,7 +30,7 @@ git clone git@github.com:ardupilot/ardupilot_ros.git Install dependencies using rosdep: ```bash cd ~/ros2_ws -rosdep install --from-paths src --ignore-src -r +rosdep install --from-paths src --ignore-src -r --skip-keys gazebo-ros-pkgs ``` ## Build @@ -96,6 +96,47 @@ Then run the controller using, Now, using the keyboard keys you can control the drone. +### 2. Obstacle avoidance using Cartographer and Nav2 + +Using the same simulation as before, the nav2 node can be launched to control the copter once it is in the air. + +Launch the simulation: + +```bash +cd ~/ros2_ws +source install/setup.sh +ros2 launch ardupilot_gz_bringup iris_maze.launch.py rviz:=false +``` +Launch cartographer: + +```bash +cd ~/ros2_ws +source install/setup.sh +ros2 launch ardupilot_ros cartographer.launch.py rviz:=false +``` + +Launch nav2: + +```bash +cd ~/ros2_ws +source install/setup.sh +ros2 launch ardupilot_ros navigation.launch.py +``` + +Takeoff the Copter using `mavproxy` to an altitude of 2.5m: + +```bash +mavproxy.py --console --map --aircraft test --master=:14550 + +mode guided + +arm throttle + +takeoff 2.5 +``` + +You may now navigate while mapping using the `Nav2 Goal` tool in RVIZ! + ## Contribution Guideline * Ensure the [pre-commit](https://github.com/pre-commit/pre-commit) hooks pass locally before creating your pull request by installing the hooks before committing. diff --git a/config/navigation.yaml b/config/navigation.yaml new file mode 100644 index 0000000..5c7c7d5 --- /dev/null +++ b/config/navigation.yaml @@ -0,0 +1,180 @@ +bt_navigator: + ros__parameters: + use_sim_time: &default_use_sim_time True + global_frame: map + robot_base_frame: base_link + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_smooth_path_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_back_up_action_bt_node + - nav2_drive_on_heading_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_truncate_path_local_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_path_expiring_timer_condition + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + - nav2_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node + +bt_navigator_navigate_through_poses_rclcpp_node: + ros__parameters: + use_sim_time: *default_use_sim_time + +bt_navigator_navigate_to_pose_rclcpp_node: + ros__parameters: + use_sim_time: *default_use_sim_time + +controller_server: + ros__parameters: + use_sim_time: *default_use_sim_time + failure_tolerance: 0.3 + progress_checker_plugin: "progress_checker" + goal_checker_plugins: ["general_goal_checker"] + controller_plugins: ["FollowPath"] + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + general_goal_checker: + stateful: True + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + FollowPath: + plugin: "dwb_core::DWBLocalPlanner" + max_vel_x: 0.5 + max_vel_y: 0.5 + max_vel_theta: 1.0 + max_speed_xy: 0.707 + acc_lim_x: 2.5 + acc_lim_y: 0.0 + acc_lim_theta: 3.2 + decel_lim_x: -2.5 + decel_lim_y: 0.0 + decel_lim_theta: -3.2 + critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] + +global_costmap: + global_costmap: + ros__parameters: + global_frame: map + robot_base_frame: base_link + use_sim_time: *default_use_sim_time + robot_radius: 0.25 + resolution: 0.05 + lethal_cost_threshold: 50 + always_send_full_costmap: True + plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: scan + scan: + topic: /scan + clearing: True + marking: True + data_type: "LaserScan" + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + +local_costmap: + local_costmap: + ros__parameters: + global_frame: odom + robot_base_frame: base_link + use_sim_time: *default_use_sim_time + rolling_window: true + width: 5 + height: 5 + resolution: 0.05 + robot_radius: 0.25 + always_send_full_costmap: True + plugins: ["voxel_layer", "inflation_layer"] + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + +planner_server: + ros__parameters: + expected_planner_frequency: 20.0 + use_sim_time: *default_use_sim_time + planner_plugins: ["GridBased"] + GridBased: + plugin: "nav2_navfn_planner/NavfnPlanner" + +smoother_server: + ros__parameters: + use_sim_time: *default_use_sim_time + smoother_plugins: ["simple_smoother"] + enable_stamped_cmd_vel: True + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + +behavior_server: + ros__parameters: + local_frame: odom + global_frame: map + robot_base_frame: base_link + use_sim_time: *default_use_sim_time + enable_stamped_cmd_vel: True + behavior_plugins: ["spin", "backup", "drive_on_heading", "wait"] + spin: + plugin: "nav2_behaviors/Spin" + backup: + plugin: "nav2_behaviors/BackUp" + drive_on_heading: + plugin: "nav2_behaviors/DriveOnHeading" + wait: + plugin: "nav2_behaviors/Wait" + +velocity_smoother: + ros__parameters: + use_sim_time: *default_use_sim_time + feedback: "OPEN_LOOP" + max_velocity: [0.5, 0.5, 1.0] + min_velocity: [-0.5, -0.5, -1.0] + velocity_timeout: 1.0 diff --git a/launch/ap_nav.launch b/launch/ap_nav.launch deleted file mode 100644 index e28101e..0000000 --- a/launch/ap_nav.launch +++ /dev/null @@ -1,12 +0,0 @@ - - - - - - - - - - - - diff --git a/launch/main.launch b/launch/main.launch deleted file mode 100644 index 3e20697..0000000 --- a/launch/main.launch +++ /dev/null @@ -1,11 +0,0 @@ - - - - - - - diff --git a/launch/navigation.launch.py b/launch/navigation.launch.py new file mode 100644 index 0000000..79e1467 --- /dev/null +++ b/launch/navigation.launch.py @@ -0,0 +1,66 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, GroupAction +from launch.substitutions import LaunchConfiguration +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.conditions import IfCondition +from launch_ros.actions import SetRemap +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +from pathlib import Path + +"""Generate a launch description for the navigation example.""" + + +def generate_launch_description(): + # Navigation + navigation = GroupAction( + actions=[ + SetRemap(src="/cmd_vel", dst="/ap/cmd_vel"), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + str( + Path( + FindPackageShare("nav2_bringup").find("nav2_bringup"), + "launch", + "navigation_launch.py", + ) + ) + ), + launch_arguments={ + "use_sim_time": "true", + "params_file": FindPackageShare("ardupilot_ros").find( + "ardupilot_ros" + ) + + "/config" + + "/navigation.yaml", + }.items(), + ), + ] + ) + + # RViz. + rviz = Node( + package="rviz2", + executable="rviz2", + arguments=[ + "-d", + str( + Path( + FindPackageShare("ardupilot_ros").find("ardupilot_ros"), + "rviz", + "navigation.rviz", + ) + ), + ], + condition=IfCondition(LaunchConfiguration("rviz")), + ) + + return LaunchDescription( + [ + DeclareLaunchArgument( + "rviz", default_value="true", description="Open RViz." + ), + rviz, + navigation, + ] + ) diff --git a/package.xml b/package.xml index 7d75bde..b4c8538 100644 --- a/package.xml +++ b/package.xml @@ -17,6 +17,7 @@ ros_gz_sim robot_state_publisher topic_tools + navigation2 ament_copyright ament_flake8 ament_pep257 diff --git a/params/costmap_common_params.yaml b/params/costmap_common_params.yaml deleted file mode 100644 index 8c4de1e..0000000 --- a/params/costmap_common_params.yaml +++ /dev/null @@ -1,10 +0,0 @@ -obstacle_range: 6.0 -raytrace_range: 8.5 -footprint: [[0.12, 0.14], [0.12, -0.14], [-0.12, -0.14], [-0.12, 0.14]] -map_topic: /map -subscribe_to_updates: true -observation_sources: laser_scan_sensor -laser_scan_sensor: {sensor_frame: laser, data_type: LaserScan, topic: scan, marking: true, clearing: true} -global_frame: map -robot_base_frame: base_link -always_send_full_costmap: true diff --git a/params/global_costmap_params.yaml b/params/global_costmap_params.yaml deleted file mode 100644 index f9caba1..0000000 --- a/params/global_costmap_params.yaml +++ /dev/null @@ -1,14 +0,0 @@ -global_costmap: - update_frequency: 2.5 - publish_frequency: 2.5 - transform_tolerance: 0.5 - width: 15 - height: 15 - origin_x: -7.5 - origin_y: -7.5 - static_map: false - rolling_window: true - inflation_radius: 2.5 - resolution: 0.1 - global_frame: /map - robot_base_frame: base_link diff --git a/params/local_costmap_params.yaml b/params/local_costmap_params.yaml deleted file mode 100644 index cd91263..0000000 --- a/params/local_costmap_params.yaml +++ /dev/null @@ -1,14 +0,0 @@ -local_costmap: - update_frequency: 5 - publish_frequency: 5 - transform_tolerance: 0.25 - static_map: false - rolling_window: true - width: 3 - height: 3 - origin_x: -1.5 - origin_y: -1.5 - resolution: 0.1 - inflation_radius: 0.6 - global_frame: odom - robot_base_frame: base_link diff --git a/params/trajectory_planner.yaml b/params/trajectory_planner.yaml deleted file mode 100644 index 1807cd0..0000000 --- a/params/trajectory_planner.yaml +++ /dev/null @@ -1,19 +0,0 @@ -TrajectoryPlannerROS: - max_vel_x: 2.0 - min_vel_x: 0.0 - max_vel_theta: 1.0 - min_vel_theta: -1.0 - min_in_place_vel_theta: 0.25 - - acc_lim_theta: 0.25 - acc_lim_x: 2.5 - acc_lim_Y: 2.5 - - holonomic_robot: false - - escape_reset_dist: 5.0 - escape_reset_theta: 1.5707963267948966 - escape_vel: -2.0 - - goal_distance_bias: 0.8 - path_distance_bias: 0.6 diff --git a/rviz/navigation.rviz b/rviz/navigation.rviz new file mode 100644 index 0000000..f13ec89 --- /dev/null +++ b/rviz/navigation.rviz @@ -0,0 +1,489 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /TF1/Frames1 + - /TF1/Tree1 + Splitter Ratio: 0.5833333134651184 + Tree Height: 496 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: nav2_rviz_plugins/Navigation 2 + Name: Navigation 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: "" + Mass Properties: + Inertia: false + Mass: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: false + Visual Enabled: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: false + base_link: + Value: true + base_scan: + Value: true + imu_link: + Value: true + map: + Value: true + odom: + Value: true + rotor_0: + Value: true + rotor_1: + Value: true + rotor_2: + Value: true + rotor_3: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + map: + odom: + base_link: + base_scan: + {} + imu_link: + {} + rotor_0: + {} + rotor_1: + {} + rotor_2: + {} + rotor_3: + {} + Update Interval: 0 + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: LaserScan + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /scan + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: true + Enabled: true + Name: Map + Topic: + Depth: 1 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map_updates + Use Timestamp: false + Value: true + - Class: rviz_common/Group + Displays: + - Alpha: 0.30000001192092896 + Class: rviz_default_plugins/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: Global Costmap + Topic: + Depth: 1 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/costmap + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/costmap_updates + Use Timestamp: false + Value: true + - Alpha: 0.30000001192092896 + Class: rviz_default_plugins/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: Downsampled Costmap + Topic: + Depth: 1 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /downsampled_costmap + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /downsampled_costmap_updates + Use Timestamp: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 255; 0; 0 + Enabled: true + Head Diameter: 0.019999999552965164 + Head Length: 0.019999999552965164 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: Arrows + Radius: 0.029999999329447746 + Shaft Diameter: 0.004999999888241291 + Shaft Length: 0.019999999552965164 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /plan + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 125; 125; 125 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: VoxelGrid + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.05000000074505806 + Style: Boxes + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/voxel_marked_cloud + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Class: rviz_default_plugins/Polygon + Color: 25; 255; 0 + Enabled: false + Name: Polygon + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/published_footprint + Value: false + Enabled: true + Name: Global Planner + - Class: rviz_common/Group + Displays: + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: Local Costmap + Topic: + Depth: 1 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/costmap + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/costmap_updates + Use Timestamp: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 0; 12; 255 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Local Plan + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_plan + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Trajectories + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /marker + Value: false + - Alpha: 1 + Class: rviz_default_plugins/Polygon + Color: 25; 255; 0 + Enabled: true + Name: Polygon + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/published_footprint + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: VoxelGrid + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/voxel_marked_cloud + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Controller + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /waypoints + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + - Class: nav2_rviz_plugins/GoalTool + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Angle: -1.5850002765655518 + Class: rviz_default_plugins/TopDownOrtho + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Scale: 78.73542785644531 + Target Frame: + Value: TopDownOrtho (rviz_default_plugins) + X: 3.2515478134155273 + Y: -4.7466325759887695 + Saved: ~ +Window Geometry: + Displays: + collapsed: true + Height: 986 + Hide Left Dock: true + Hide Right Dock: true + Navigation 2: + collapsed: true + QMainWindow State: 000000ff00000000fd00000004000000000000016a00000380fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d0000022d000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018004e0061007600690067006100740069006f006e0020003200000002700000014d0000013900fffffffb0000001e005200650061006c00730065006e0073006500430061006d00650072006100000002c6000000c10000000000000000000000010000010f0000034afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000034a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000064a0000038000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1610 + X: 70 + Y: 27 diff --git a/scripts/set_origin.py b/scripts/set_origin.py deleted file mode 100644 index 846156c..0000000 --- a/scripts/set_origin.py +++ /dev/null @@ -1,119 +0,0 @@ -#!/usr/bin/env python - -## -# -# Send SET_GPS_GLOBAL_ORIGIN and SET_HOME_POSITION messages -# -## - -import rospy -from pymavlink.dialects.v10 import ardupilotmega as MAV_APM -from mavros.mavlink import convert_to_rosmsg -from mavros_msgs.msg import Mavlink - -# Global position of the origin -lat = 425633500 # Terni -lon = 126432900 # Terni -alt = 163000 - - -class fifo(object): - """A simple buffer""" - - def __init__(self): - self.buf = [] - - def write(self, data): - self.buf += data - return len(data) - - def read(self): - return self.buf.pop(0) - - -def send_message(msg, mav, pub): - """ - Send a mavlink message - """ - msg.pack(mav) - rosmsg = convert_to_rosmsg(msg) - pub.publish(rosmsg) - - print("sent message %s" % msg) - - -def set_global_origin(mav, pub): - """ - Send a mavlink SET_GPS_GLOBAL_ORIGIN message, which allows us - to use local position information without a GPS. - """ - target_system = mav.srcSystem - # target_system = 0 # 0 --> broadcast to everyone - lattitude = lat - longitude = lon - altitude = alt - - msg = MAV_APM.MAVLink_set_gps_global_origin_message( - target_system, lattitude, longitude, altitude - ) - - send_message(msg, mav, pub) - - -def set_home_position(mav, pub): - """ - Send a mavlink SET_HOME_POSITION message, which should allow - us to use local position information without a GPS - """ - target_system = mav.srcSystem - # target_system = 0 # broadcast to everyone - - lattitude = lat - longitude = lon - altitude = alt - - x = 0 - y = 0 - z = 0 - q = [1, 0, 0, 0] # w x y z - - approach_x = 0 - approach_y = 0 - approach_z = 1 - - msg = MAV_APM.MAVLink_set_home_position_message( - target_system, - lattitude, - longitude, - altitude, - x, - y, - z, - q, - approach_x, - approach_y, - approach_z, - ) - - send_message(msg, mav, pub) - - -if __name__ == "__main__": - try: - rospy.init_node("origin_publisher") - mavlink_pub = rospy.Publisher("/mavlink/to", Mavlink, queue_size=20) - - # Set up mavlink instance - f = fifo() - mav = MAV_APM.MAVLink(f, srcSystem=1, srcComponent=1) - - # wait to initialize - while mavlink_pub.get_num_connections() <= 0: - pass - - for _ in range(2): - rospy.sleep(1) - set_global_origin(mav, mavlink_pub) - set_home_position(mav, mavlink_pub) - except rospy.ROSInterruptException: - pass diff --git a/setup.py b/setup.py index 98b9f87..b5094c8 100644 --- a/setup.py +++ b/setup.py @@ -19,6 +19,10 @@ os.path.join("share", package_name, "config"), glob(os.path.join("config", "*.lua")), ), + ( + os.path.join("share", package_name, "config"), + glob(os.path.join("config", "*.yaml")), + ), ( os.path.join("share", package_name, "rviz"), glob(os.path.join("rviz", "*.rviz")),