Skip to content
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

Fix depend of moveit_studio_behavior #11

Merged
merged 3 commits into from
Nov 18, 2024
Merged

Fix depend of moveit_studio_behavior #11

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
6f581ce
Fix depend of moveit_studio_behavior
pac48 Nov 18, 2024
9b97cd4
Fix pick ApriTag object
pac48 Nov 18, 2024
d43221b
Merge pull request #12 from PickNikRobotics/fix-pick-apriTag-object
MikeWrock Nov 18, 2024
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
1 change: 1 addition & 0 deletions src/example_behaviors/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@
<depend>moveit_ros_planning_interface</depend>
<depend>moveit_studio_vision_msgs</depend>
<depend>moveit_studio_vision</depend>
<depend>moveit_studio_behavior</depend>

<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_cmake_gtest</test_depend>
Expand Down
6 changes: 3 additions & 3 deletions src/lab_sim/objectives/apriltag_detection_config.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,7 @@ DetectAprilTags:
apriltag_size: 0.04
max_hamming: 0
n_threads: 1
quad_decimate: 1
quad_sigma: 0.1
refine_edges: false
quad_decimate: 2
quad_sigma: 0.0
refine_edges: true
z_up: false
36 changes: 30 additions & 6 deletions src/lab_sim/objectives/apriltag_pick_object.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,3 +1,4 @@
<?xml version="1.0" encoding="UTF-8" ?>
<root BTCPP_format="4" main_tree_to_execute="Pick April Tag Labeled Object">
<!--//////////-->
<BehaviorTree
Expand All @@ -8,7 +9,7 @@
<Control ID="Sequence" name="TopLevelSequence">
<SubTree ID="Look at table" _collapsed="true" />
<Action ID="ClearSnapshot" />
<SubTree ID="Wrist camera snapshot" _collapsed="true" />
<SubTree ID="Take wrist camera snapshot" _collapsed="true" />
<Control ID="Sequence" name="Setup">
<SubTree ID="Open Gripper" />
</Control>
Expand All @@ -27,19 +28,39 @@
topic_name="/wrist_camera/color"
message_out="{image}"
/>
<SubTree
ID="Get AprilTag Pose from Image"
image="{image}"
camera_info="{camera_info}"
parameters="{parameters}"
_collapsed="true"
detection_pose="{detection_pose}"
/>
<Action
ID="DetectAprilTags"
ID="TransformPose"
output_pose="{view_pose}"
input_pose="{detection_pose}"
quaternion_xyzw="0;0;0;1"
translation_xyz="0;-0.1;-0.15"
/>
<SubTree
ID="Plan Move To Pose"
target_pose="{view_pose}"
move_to_pose_solution="{move_to_pose_solution}"
/>
<Action ID="ExecuteMTCTask" solution="{move_to_pose_solution}" />
<SubTree
ID="Get AprilTag Pose from Image"
image="{image}"
camera_info="{camera_info}"
parameters="{parameters}"
detections="{detections}"
_collapsed="true"
detection_pose="{detection_pose}"
/>
<Action ID="GetDetectionPose" />
<Action ID="TransformPoseFrame" input_pose="{detection_pose}" />
<Action
ID="TransformPose"
output_pose="{offset_pose}"
input_pose="{output_pose}"
input_pose="{detection_pose}"
quaternion_xyzw="0;0;0;1"
translation_xyz="0;0;-0.01"
/>
Expand Down Expand Up @@ -68,4 +89,7 @@
<Action ID="ExecuteMTCTask" solution="{pick_object_solution}" />
</Control>
</BehaviorTree>
<TreeNodesModel>
<SubTree ID="Pick April Tag Labeled Object" />
</TreeNodesModel>
</root>
41 changes: 41 additions & 0 deletions src/lab_sim/objectives/get_april_tag_pose_from_image.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,41 @@
<root BTCPP_format="4" main_tree_to_execute="Get AprilTag Pose from Image">
<!-- ////////// -->
<BehaviorTree ID="Get AprilTag Pose from Image">
<Control ID="Sequence">
<!--Get tag pose estimate in world frame-->
<Action
ID="DetectAprilTags"
image="{image}"
camera_info="{camera_info}"
parameters="{parameters}"
detections="{detections}"
/>
<Action
ID="GetImage"
topic_name="/wrist_camera/color"
message_out="{image}"
/>
<Action
ID="DetectAprilTags"
image="{image}"
camera_info="{camera_info}"
parameters="{parameters}"
detections="{detections}"
/>
<Action ID="GetDetectionPose" />
<Action
ID="TransformPoseFrame"
input_pose="{detection_pose}"
output_pose="{detection_pose}"
/>
</Control>
</BehaviorTree>
<TreeNodesModel>
<SubTree ID="Get AprilTag Pose from Image">
<input_port name="image" default="{image}" />
<input_port name="camera_info" default="{camera_info}" />
<input_port name="parameters" default="{parameters}" />
<output_port name="detection_pose" default="{detection_pose}" />
</SubTree>
</TreeNodesModel>
</root>
36 changes: 36 additions & 0 deletions src/lab_sim/objectives/plan_move_to_pose.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,36 @@
<root BTCPP_format="4" main_tree_to_execute="Plan Move To Pose">
<!-- ////////// -->
<BehaviorTree ID="Plan Move To Pose">
<Control ID="Sequence">
<Action
ID="InitializeMTCTask"
task_id="move_to_pose"
controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
task="{move_to_pose_task}"
/>
<Action ID="SetupMTCCurrentState" task="{move_to_pose_task}" />
<Action
ID="SetupMTCMoveToPose"
ik_frame="grasp_link"
planning_group_name="manipulator"
target_pose="{target_pose}"
task="{move_to_pose_task}"
planner_interface="moveit_default"
/>
<Action
ID="PlanMTCTask"
solution="{move_to_pose_solution}"
task="{move_to_pose_task}"
/>
</Control>
</BehaviorTree>
<TreeNodesModel>
<SubTree ID="Plan Move To Pose">
<input_port name="target_pose" default="{target_pose}" />
<output_port
name="move_to_pose_solution"
default="{move_to_pose_solution}"
/>
</SubTree>
</TreeNodesModel>
</root>
15 changes: 12 additions & 3 deletions src/lab_sim/objectives/wrist_snap.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,13 +1,22 @@
<root BTCPP_format="4" main_tree_to_execute="Wrist camera snapshot">
<?xml version="1.0" encoding="UTF-8" ?>
<root BTCPP_format="4" main_tree_to_execute=" Take wrist camera snapshot">
<!--//////////-->
<BehaviorTree ID="Wrist camera snapshot" _description="" _favorite="true">
<BehaviorTree
ID="Take wrist camera snapshot"
_description=""
_favorite="true"
>
<Control ID="Sequence" name="TopLevelSequence">
<!-- Do not clear snapshots first, so we can take multiple snapshots from different angles -->
<Action ID="GetPointCloud" topic_name="/wrist_camera/points" />
<Action ID="SendPointCloudToUI" />
</Control>
</BehaviorTree>
<TreeNodesModel>
<SubTree ID="Wrist camera snapshot" />
<SubTree ID="Take wrist camera snapshot">
<MetadataFields>
<Metadata subcategory="Application - Basic Examples" />
</MetadataFields>
</SubTree>
</TreeNodesModel>
</root>
Loading