diff --git a/src/hangar_sim/images/box.png b/src/hangar_sim/images/box.png new file mode 100644 index 000000000..d1a08c9b7 Binary files /dev/null and b/src/hangar_sim/images/box.png differ diff --git a/src/hangar_sim/images/box_2.png b/src/hangar_sim/images/box_2.png new file mode 100644 index 000000000..a5c63f9ad Binary files /dev/null and b/src/hangar_sim/images/box_2.png differ diff --git a/src/hangar_sim/objectives/count_boxes.xml b/src/hangar_sim/objectives/count_boxes.xml new file mode 100644 index 000000000..425fa52e7 --- /dev/null +++ b/src/hangar_sim/objectives/count_boxes.xml @@ -0,0 +1,33 @@ + + + + + + + + + + + + + + + + + + diff --git a/src/hangar_sim/objectives/count_boxes_objective.xml b/src/hangar_sim/objectives/count_boxes_objective.xml new file mode 100644 index 000000000..e7a286654 --- /dev/null +++ b/src/hangar_sim/objectives/count_boxes_objective.xml @@ -0,0 +1,130 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Absolute path to a directory for saving wrist camera pictures + + + Absolute path to a directory for saving scene camera pictures + + + Absolute path to a directory of pose YAML files + + + + diff --git a/src/hangar_sim/objectives/count_boxes_subtree.xml b/src/hangar_sim/objectives/count_boxes_subtree.xml new file mode 100644 index 000000000..df87749ce --- /dev/null +++ b/src/hangar_sim/objectives/count_boxes_subtree.xml @@ -0,0 +1,168 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Absolute path to a directory for saving scene camera pictures + + + Absolute path to a directory for saving wrist camera pictures + + + Number of boxes + + + + diff --git a/src/hangar_sim/objectives/createposevectorfromdirectory.xml b/src/hangar_sim/objectives/createposevectorfromdirectory.xml new file mode 100644 index 000000000..27d3ed6ac --- /dev/null +++ b/src/hangar_sim/objectives/createposevectorfromdirectory.xml @@ -0,0 +1,47 @@ + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/hangar_sim/objectives/order_poses.xml b/src/hangar_sim/objectives/order_poses.xml new file mode 100644 index 000000000..c7f395c34 --- /dev/null +++ b/src/hangar_sim/objectives/order_poses.xml @@ -0,0 +1,244 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/hangar_sim/objectives/verify_box_count.xml b/src/hangar_sim/objectives/verify_box_count.xml new file mode 100644 index 000000000..1abf1c709 --- /dev/null +++ b/src/hangar_sim/objectives/verify_box_count.xml @@ -0,0 +1,31 @@ + + + + + + + + + + + + + + + + + + diff --git a/src/hangar_sim/objectives/verify_box_count_subtree.xml b/src/hangar_sim/objectives/verify_box_count_subtree.xml new file mode 100644 index 000000000..b26da7c1b --- /dev/null +++ b/src/hangar_sim/objectives/verify_box_count_subtree.xml @@ -0,0 +1,89 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Absolute path to wrist camera images + + + + diff --git a/src/hangar_sim/params/nav2_params.yaml b/src/hangar_sim/params/nav2_params.yaml index d933fb51c..2097ff000 100644 --- a/src/hangar_sim/params/nav2_params.yaml +++ b/src/hangar_sim/params/nav2_params.yaml @@ -138,48 +138,99 @@ controller_server: plugin: "nav2_controller::SimpleGoalChecker" xy_goal_tolerance: 0.25 yaw_goal_tolerance: 0.25 - # DWB parameters FollowPath: - plugin: "dwb_core::DWBLocalPlanner" - debug_trajectory_details: True - min_vel_x: 0.0 - min_vel_y: 0.0 - max_vel_x: 0.26 - max_vel_y: 0.0 - max_vel_theta: 1.0 - min_speed_xy: 0.0 - max_speed_xy: 0.26 - min_speed_theta: 0.0 - # Add high threshold velocity for turtlebot 3 issue. - # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 - 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 - vx_samples: 20 - vy_samples: 5 - vtheta_samples: 20 - sim_time: 1.7 - linear_granularity: 0.05 - angular_granularity: 0.025 - transform_tolerance: 0.2 - xy_goal_tolerance: 0.25 - trans_stopped_velocity: 0.25 - short_circuit_trajectory_evaluation: True - stateful: True - critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] - BaseObstacle.scale: 0.02 - PathAlign.scale: 32.0 - PathAlign.forward_point_distance: 0.1 - GoalAlign.scale: 24.0 - GoalAlign.forward_point_distance: 0.1 - PathDist.scale: 32.0 - GoalDist.scale: 24.0 - RotateToGoal.scale: 32.0 - RotateToGoal.slowing_factor: 5.0 - RotateToGoal.lookahead_time: -1.0 + plugin: "nav2_mppi_controller::MPPIController" + time_steps: 56 + model_dt: 0.05 + batch_size: 2000 + vx_std: 0.2 + vy_std: 0.2 + wz_std: 0.4 + vx_max: 0.5 + vx_min: -0.35 + vy_max: 0.5 + wz_max: 1.9 + iteration_count: 1 + prune_distance: 1.7 + transform_tolerance: 0.1 + temperature: 0.3 + gamma: 0.015 + motion_model: "DiffDrive" + visualize: false + TrajectoryVisualizer: + trajectory_step: 5 + time_step: 3 + AckermannConstraints: + min_turning_r: 0.2 + critics: ["ConstraintCritic", "CostCritic", "GoalCritic", "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", "PathAngleCritic", "PreferForwardCritic"] + ConstraintCritic: + enabled: true + cost_power: 1 + cost_weight: 4.0 + GoalCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + threshold_to_consider: 1.4 + GoalAngleCritic: + enabled: true + cost_power: 1 + cost_weight: 3.0 + threshold_to_consider: 0.5 + PreferForwardCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + threshold_to_consider: 0.5 + # ObstaclesCritic: + # enabled: true + # cost_power: 1 + # repulsion_weight: 1.5 + # critical_weight: 20.0 + # consider_footprint: false + # collision_cost: 10000.0 + # collision_margin_distance: 0.1 + # near_goal_distance: 0.5 + CostCritic: + enabled: true + cost_power: 1 + cost_weight: 3.81 + critical_cost: 300.0 + consider_footprint: true + collision_cost: 1000000.0 + near_goal_distance: 1.0 + PathAlignCritic: + enabled: true + cost_power: 1 + cost_weight: 14.0 + max_path_occupancy_ratio: 0.05 + trajectory_point_step: 3 + threshold_to_consider: 0.5 + offset_from_furthest: 20 + use_path_orientations: false + PathFollowCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + offset_from_furthest: 5 + threshold_to_consider: 1.4 + PathAngleCritic: + enabled: true + cost_power: 1 + cost_weight: 2.0 + offset_from_furthest: 4 + threshold_to_consider: 0.5 + max_angle_to_furthest: 1.0 + forward_preference: true + # VelocityDeadbandCritic: + # enabled: true + # cost_power: 1 + # cost_weight: 35.0 + # deadband_velocities: [0.05, 0.05, 0.05] + # TwirlingCritic: + # enabled: true + # twirling_cost_power: 1 + # twirling_cost_weight: 10.0 local_costmap: local_costmap: @@ -324,7 +375,7 @@ behavior_server: transform_tolerance: 0.1 use_sim_time: true simulate_ahead_time: 2.0 - max_rotational_vel: 1.0 + max_rotational_vel: 3.6 min_rotational_vel: 0.4 rotational_acc_lim: 3.2 diff --git a/src/hangar_sim/poses/1.yaml b/src/hangar_sim/poses/1.yaml new file mode 100644 index 000000000..1f2d9da5c --- /dev/null +++ b/src/hangar_sim/poses/1.yaml @@ -0,0 +1,15 @@ +header: + stamp: + sec: 1746467553 + nanosec: 486561938 + frame_id: world +pose: + position: + x: -15.0 + y: -0.75 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.383 + w: 0.924 diff --git a/src/hangar_sim/poses/2.yaml b/src/hangar_sim/poses/2.yaml new file mode 100644 index 000000000..0ce2bd381 --- /dev/null +++ b/src/hangar_sim/poses/2.yaml @@ -0,0 +1,15 @@ +header: + stamp: + sec: 1746467553 + nanosec: 486561938 + frame_id: world +pose: + position: + x: -7.0 + y: -0.75 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.924 + w: 0.383 diff --git a/src/hangar_sim/poses/3.yaml b/src/hangar_sim/poses/3.yaml new file mode 100644 index 000000000..991fdca99 --- /dev/null +++ b/src/hangar_sim/poses/3.yaml @@ -0,0 +1,15 @@ +header: + stamp: + sec: 1746467553 + nanosec: 486561938 + frame_id: world +pose: + position: + x: -15.0 + y: 7.5 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.383 + w: 0.924 diff --git a/src/hangar_sim/poses/4.yaml b/src/hangar_sim/poses/4.yaml new file mode 100644 index 000000000..205980f54 --- /dev/null +++ b/src/hangar_sim/poses/4.yaml @@ -0,0 +1,15 @@ +header: + stamp: + sec: 1746467553 + nanosec: 486561938 + frame_id: world +pose: + position: + x: -7.0 + y: 7.5 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.924 + w: -0.383 diff --git a/src/hangar_sim/poses/random1.yaml b/src/hangar_sim/poses/random1.yaml new file mode 100644 index 000000000..fa88ac02a --- /dev/null +++ b/src/hangar_sim/poses/random1.yaml @@ -0,0 +1,15 @@ +header: + stamp: + sec: 1746467553 + nanosec: 486561938 + frame_id: world +pose: + position: + x: 20.0 + y: 10.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.383 + w: 0.924 diff --git a/src/hangar_sim/poses/random2.yaml b/src/hangar_sim/poses/random2.yaml new file mode 100644 index 000000000..6028227f9 --- /dev/null +++ b/src/hangar_sim/poses/random2.yaml @@ -0,0 +1,15 @@ +header: + stamp: + sec: 1746467553 + nanosec: 486561938 + frame_id: world +pose: + position: + x: 20.0 + y: 20.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.383 + w: 0.924 diff --git a/src/hangar_sim/poses/random3.yaml b/src/hangar_sim/poses/random3.yaml new file mode 100644 index 000000000..33cad35b0 --- /dev/null +++ b/src/hangar_sim/poses/random3.yaml @@ -0,0 +1,15 @@ +header: + stamp: + sec: 1746467553 + nanosec: 486561938 + frame_id: world +pose: + position: + x: 20.0 + y: 30.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.383 + w: 0.924 diff --git a/src/hangar_sim/poses/random4.yaml b/src/hangar_sim/poses/random4.yaml new file mode 100644 index 000000000..a6af148c2 --- /dev/null +++ b/src/hangar_sim/poses/random4.yaml @@ -0,0 +1,15 @@ +header: + stamp: + sec: 1746467553 + nanosec: 486561938 + frame_id: world +pose: + position: + x: 30.0 + y: 30.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.383 + w: 0.924 diff --git a/src/hangar_sim/poses/random5.yaml b/src/hangar_sim/poses/random5.yaml new file mode 100644 index 000000000..a6857058a --- /dev/null +++ b/src/hangar_sim/poses/random5.yaml @@ -0,0 +1,15 @@ +header: + stamp: + sec: 1746467553 + nanosec: 486561938 + frame_id: world +pose: + position: + x: -20.0 + y: -10.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.383 + w: 0.924 diff --git a/src/hangar_sim/poses/random6.yaml b/src/hangar_sim/poses/random6.yaml new file mode 100644 index 000000000..1e9702277 --- /dev/null +++ b/src/hangar_sim/poses/random6.yaml @@ -0,0 +1,15 @@ +header: + stamp: + sec: 1746467553 + nanosec: 486561938 + frame_id: world +pose: + position: + x: -20.0 + y: -20.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.383 + w: 0.924 diff --git a/src/hangar_sim/poses/random7.yaml b/src/hangar_sim/poses/random7.yaml new file mode 100644 index 000000000..3aafe4409 --- /dev/null +++ b/src/hangar_sim/poses/random7.yaml @@ -0,0 +1,15 @@ +header: + stamp: + sec: 1746467553 + nanosec: 486561938 + frame_id: world +pose: + position: + x: -20.0 + y: -30.0 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.383 + w: 0.924