Skip to content

Add new navigate with replanning Objective and only have front lidar … #560

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
bgill92 wants to merge 2 commits into
base: main
Choose a base branch
from
+49 −4
Open

Add new navigate with replanning Objective and only have front lidar … #560

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
b626f11
Add new navigate with replanning Objective and only have front lidar …
Apr 2, 2026
f922146
fix pre-commit
Apr 2, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
7 changes: 3 additions & 4 deletions src/hangar_sim/description/ur5e_ridgeback.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -248,7 +248,7 @@
91 beams x 3 deg = 270 deg FOV, sweeping CCW from beam-0 at +45 deg in robot frame.
Rear: phi=+45 deg -> R_base = Rot_Z(45)*Rot_Y(90) -> quat=(0.6533,-0.2706,0.6533,0.2706).
angle_min=0 in rangefinder user params so the plugin sets _ROS frame X = beam-0 direction. -->
<body name="lidar_rear_mount" pos="-0.45 0 0.15">
<!-- <body name="lidar_rear_mount" pos="-0.45 0 0.15">
<inertial
mass="0.25"
pos="0 0 0"
Expand All @@ -262,7 +262,6 @@
quat="0.6533 -0.2706 0.6533 0.2706"
/>
</replicate>
<!-- TIM571 body: dark cylinder, visual-only -->
<geom
name="lidar_rear_body"
type="cylinder"
Expand All @@ -273,7 +272,7 @@
conaffinity="0"
group="2"
/>
</body>
</body> -->
<geom
name="chassis_link"
mesh="chassis_link"
Expand Down Expand Up @@ -658,6 +657,6 @@
beam-0 at -135 deg and beam-90 at +135 deg in the robot frame. -->
<rangefinder site="lidar_front" user="0.05 0.0 4.7124 0.05236 0.05 25.0" />
<!-- Rear TIM571: same convention — angle_min=0, frame X points at beam-0 (+45 deg robot frame). -->
<rangefinder site="lidar_rear" user="0.05 0.0 4.7124 0.05236 0.05 25.0" />
<!-- <rangefinder site="lidar_rear" user="0.05 0.0 4.7124 0.05236 0.05 25.0" /> -->
</sensor>
</mujoco>
46 changes: 46 additions & 0 deletions src/hangar_sim/objectives/navigate_to_clicked_point_with_replanning.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,46 @@
<?xml version="1.0" encoding="UTF-8" ?>
<root
BTCPP_format="4"
main_tree_to_execute="Navigate to Clicked Point with Replanning"
>
<!--//////////-->
<BehaviorTree
ID="Navigate to Clicked Point with Replanning"
_description="Navigate to a point clicked by the user in the UI. This Objective will repeatedly replan the path as new obstacles appear on the local costmap."
_favorite="true"
>
<Control ID="Sequence" name="TopLevelSequence">
<Action
ID="CreateStampedPose"
reference_frame="base_link"
stamped_pose="{start}"
/>
<Action
ID="SwitchController"
activate_controllers="platform_velocity_controller_nav2"
deactivate_controllers="velocity_force_controller;joint_trajectory_controller;platform_velocity_controller"
/>
<Action
ID="GetPoseFromUser"
is_normal="true"
pose_prompt="Click point on UI"
user_pose="{goal}"
view_name="Visualization"
/>
<Action
ID="NavigateToPoseAction"
action_name="/navigate_to_pose"
behavior_tree_path="/opt/ros/humble/share/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml"
pose_stamped="{goal}"
/>
</Control>
</BehaviorTree>
<TreeNodesModel>
<SubTree ID="Navigate to Clicked Point with Replanning">
<MetadataFields>
<Metadata runnable="true" />
<Metadata subcategory="Navigation" />
</MetadataFields>
</SubTree>
</TreeNodesModel>
</root>
Loading