Skip to content

Commit

Permalink
Merge pull request #5 from marrts/feature/go-home
Browse files Browse the repository at this point in the history 
Enable planning home
  • Loading branch information
@marip8
marip8 authored May 30, 2024
2 parents 55664db + 8d6acd0 commit 1c46c1b
Show file tree
Hide file tree
Showing 6 changed files with 167 additions and 32 deletions.
2 changes: 1 addition & 1 deletion .github/workflows/docker.yml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -55,7 +55,7 @@ jobs:
context: .
file: docker/Dockerfile
build-args: |
TAG=${{ matrix.distro }}-4.4
TAG=${{ matrix.distro }}-4.6
push: ${{ env.PUSH_DOCKER_IMAGE }}
tags: ${{ steps.meta.outputs.tags }}
labels: ${{ steps.meta.outputs.labels }}
2 changes: 1 addition & 1 deletion .github/workflows/ubuntu.yml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,7 +21,7 @@ jobs:
matrix:
distro: [humble]
container:
image: ghcr.io/ros-industrial-consortium/scan_n_plan_workshop:${{ matrix.distro }}-4.4
image: ghcr.io/ros-industrial-consortium/scan_n_plan_workshop:${{ matrix.distro }}-4.6
env:
CCACHE_DIR: ${{ github.workspace }}/${{ matrix.distro }}/.ccache
DEBIAN_FRONTEND: noninteractive
Expand Down
2 changes: 1 addition & 1 deletion docker/docker-compose.yml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,7 @@ services:
context: ..
dockerfile: docker/Dockerfile
args:
- TAG=humble-4.4
- TAG=humble-4.6
environment:
DISPLAY: $DISPLAY
XAUTHORITY: $XAUTHORITY
Expand Down
4 changes: 4 additions & 0 deletions snp_automate_2023/launch/start.launch.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -43,6 +43,10 @@
<param name="robot_description" value="$(var robot_description)"/>
<param name="robot_description_semantic" value="$(var robot_description_semantic)"/>
<param name="follow_joint_trajectory_action" value="$(var follow_joint_trajectory_action)"/>
<!-- Home -->
<param name="home_state_joint_values" value="[0.0, 0.0, 1.57, 0.0, 0.0, 0.785]"/>
<param name="home_state_joint_names" value="['joint_1_s', 'joint_2_l', 'joint_3_u', 'joint_4_r', 'joint_5_b', 'joint_6_t']"/>
<param name="freespace_tree" value="go_home_main"/>
<!-- Industrial Reconstruction parameters -->
<param name="ir.tsdf.min.x" value="0.21"/>
<param name="ir.tsdf.min.y" value="-0.45"/>
Expand Down
25 changes: 22 additions & 3 deletions snp_automate_application/config/snp_automate.btproj
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,7 +13,7 @@
<Action ID="CombineTrajectories" editable="true">
<input_port name="first"/>
<input_port name="second"/>
<input_port name="output"/>
<output_port name="output"/>
</Action>
<Action ID="ExecuteMotionPlanService" editable="true">
<input_port name="service_name" default="execute_motion_plan"/>
Expand All @@ -24,6 +24,12 @@
<input_port name="action_name" default="follow_joint_trajectory"/>
<input_port name="trajectory" default="{trajectory}"/>
</Action>
<Action ID="GenerateFreespaceMotionPlanService" editable="true">
<input_port name="service_name" default="generate_freespace_motion_plan"/>
<input_port name="start_joint_state" default="{start_joint_state}"/>
<input_port name="goal_joint_state" default="{goal_joint_state}"/>
<output_port name="trajectory" default="{trajectory}"/>
</Action>
<Action ID="GenerateMotionPlanService" editable="true">
<input_port name="service_name" default="generate_motion_plan"/>
<input_port name="tool_paths" default="{tool_paths}"/>
Expand All @@ -41,6 +47,10 @@
<input_port name="service_name" default="generate_tool_paths"/>
<input_port name="tool_paths" default="{tool_paths}"/>
</Action>
<Action ID="GetCurrentJointState" editable="true">
<input_port name="topic_name" default="/joint_states"/>
<output_port name="current_state" default="{current_state}"/>
</Action>
<Action ID="MotionPlanPub" editable="true">
<input_port name="topic_name"/>
<input_port name="trajectory" default="{trajectory}"/>
Expand All @@ -50,6 +60,10 @@
<input_port name="start" default="0"/>
<input_port name="end" default="100"/>
</Decorator>
<Action ID="ReverseTrajectory" editable="true">
<input_port name="input"/>
<output_port name="output"/>
</Action>
<Condition ID="RosSpinner" editable="true"/>
<Control ID="SNPSequenceWithMemory" editable="true"/>
<Decorator ID="SetPage" editable="true">
Expand All @@ -72,15 +86,20 @@
<input_port name="service_name"/>
</Action>
<Action ID="UpdateTrajectoryStartState" editable="true">
<input_port name="topic_name" default="/joint_states"/>
<input_port name="input" default="{trajectory}"/>
<input_port name="joint_state" default="{current_state}"/>
<input_port name="input_trajectory" default="{trajectory}"/>
<output_port name="output" default="{trajectory}"/>
</Action>
<SubTree ID="motion" editable="true">
<input_port name="approach" default="{approach}"/>
<input_port name="process" default="{process}"/>
<input_port name="departure" default="{departure}"/>
</SubTree>
<SubTree ID="process_pub" editable="true">
<input_port name="approach" default="{approach}"/>
<input_port name="process" default="{process}"/>
<input_port name="departure" default="{departure}"/>
</SubTree>
<SubTree ID="scan" editable="true">
<input_port name="approach" default="{approach}"/>
<input_port name="process" default="{process}"/>
Expand Down
164 changes: 138 additions & 26 deletions snp_automate_application/config/snp_automate.xml
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,80 @@
<?xml version="1.0" encoding="UTF-8"?>
<root BTCPP_format="4">
<BehaviorTree ID="freespace_motion">
<Sequence>
<StartPointQueueMode name="Enable Robot"
service_name="start_point_queue_mode"/>
<GetCurrentJointState topic_name="/joint_states"
current_state="{current_state}"/>
<UpdateTrajectoryStartState name="Update Trajectory Start State"
joint_state="{current_state}"
input_trajectory="{trajectory}"
output="{updated_trajectory}"/>
<FollowJointTrajectoryAction name="Execute Approach Motion"
action_name="{follow_joint_trajectory_action}"
trajectory="{updated_trajectory}"/>
</Sequence>
</BehaviorTree>

<BehaviorTree ID="go_home_main">
<ForceSuccess>
<ReactiveSequence>
<RosSpinner/>
<ButtonMonitor name="Reset"
button="reset"
_description="Sequence has been reset to start state"/>
<KeepRunningUntilFailure>
<ForceSuccess>
<ReactiveSequence>
<ButtonMonitor name="Halt"
button="halt"
_description="Current task has been halted"/>
<SubTree ID="go_home_process"
_autoremap="true"/>
</ReactiveSequence>
</ForceSuccess>
</KeepRunningUntilFailure>
</ReactiveSequence>
</ForceSuccess>
</BehaviorTree>

<BehaviorTree ID="go_home_process">
<SequenceWithMemory>
<Progress start="0"
end="50">
<Sequence>
<GetCurrentJointState topic_name="/joint_states"
current_state="{current_state}"/>
<GenerateFreespaceMotionPlanService service_name="generate_freespace_motion_plan"
start_joint_state="{current_state}"
goal_joint_state="{home_state}"
trajectory="{trajectory}"/>
<MotionPlanPub topic_name="motion_plan"
trajectory="{trajectory}"/>
</Sequence>
</Progress>
<Progress start="50"
end="100">
<Sequence>
<SetPage index="3">
<ButtonApproval name="Approve Scan Execution"
approve_button="execute"
disapprove_button="back"
_description="Press next to approve the scan motion plan and proceed to scan motion execution"/>
</SetPage>
<SubTree ID="freespace_motion"
trajectory="{trajectory}"
_autoremap="true"/>
</Sequence>
</Progress>
<SetPage index="4">
<ButtonApproval name="Approve Restart"
approve_button=""
disapprove_button="back"/>
</SetPage>
</SequenceWithMemory>
</BehaviorTree>

<BehaviorTree ID="main">
<ForceSuccess>
<ReactiveSequence>
Expand All @@ -26,9 +101,11 @@
<Sequence>
<StartPointQueueMode name="Enable Robot"
service_name="start_point_queue_mode"/>
<GetCurrentJointState topic_name="/joint_states"
current_state="{current_state}"/>
<UpdateTrajectoryStartState name="Update Trajectory Start State"
topic_name="/joint_states"
input="{approach}"
joint_state="{current_state}"
input_trajectory="{approach}"
output="{updated_approach}"/>
<FollowJointTrajectoryAction name="Execute Approach Motion"
action_name="{follow_joint_trajectory_action}"
Expand Down Expand Up @@ -58,15 +135,11 @@
approach="{scan_approach}"
process="{scan_process}"
departure="{scan_departure}"/>
<CombineTrajectories first="{scan_approach}"
second="{scan_process}"
output="{scan_trajectory}"/>
<CombineTrajectories first="{scan_trajectory}"
second="{scan_departure}"
output="{scan_trajectory}"/>
<MotionPlanPub name="Publish Scan Motion Plan"
topic_name="motion_plan"
trajectory="{scan_trajectory}"/>
<SubTree ID="process_pub"
approach="{scan_approach}"
process="{scan_process}"
departure="{scan_departure}"
_autoremap="true"/>
</Sequence>
</Progress>
<Progress start="20"
Expand Down Expand Up @@ -117,15 +190,11 @@
approach="{approach}"
process="{process}"
departure="{departure}"/>
<CombineTrajectories first="{approach}"
second="{process}"
output="{trajectory}"/>
<CombineTrajectories first="{trajectory}"
second="{departure}"
output="{trajectory}"/>
<MotionPlanPub name="Publish Process Motion Plan"
topic_name="motion_plan"
trajectory="{trajectory}"/>
<SubTree ID="process_pub"
approach="{approach}"
process="{process}"
departure="{departure}"
_autoremap="true"/>
</Sequence>
</Progress>
<Progress start="80"
Expand Down Expand Up @@ -153,11 +222,27 @@
</SNPSequenceWithMemory>
</BehaviorTree>

<BehaviorTree ID="process_pub">
<Sequence>
<CombineTrajectories first="{approach}"
second="{process}"
output="{app_plus_proc}"/>
<CombineTrajectories first="{app_plus_proc}"
second="{departure}"
output="{full_traj}"/>
<MotionPlanPub name="Publish Scan Motion Plan"
topic_name="motion_plan"
trajectory="{full_traj}"/>
</Sequence>
</BehaviorTree>

<BehaviorTree ID="scan">
<Sequence>
<GetCurrentJointState topic_name="/joint_states"
current_state="{current_state}"/>
<UpdateTrajectoryStartState name="Update Trajectory Start State"
topic_name="/joint_states"
input="{approach}"
joint_state="{current_state}"
input_trajectory="{approach}"
output="{updated_approach}"/>
<StartPointQueueMode name="Enable Robot"
service_name="start_point_queue_mode"/>
Expand Down Expand Up @@ -192,7 +277,7 @@
editable="true">
<input_port name="first"/>
<input_port name="second"/>
<input_port name="output"/>
<output_port name="output"/>
</Action>
<Action ID="FollowJointTrajectoryAction"
editable="true">
Expand All @@ -201,6 +286,17 @@
<input_port name="trajectory"
default="{trajectory}"/>
</Action>
<Action ID="GenerateFreespaceMotionPlanService"
editable="true">
<input_port name="service_name"
default="generate_freespace_motion_plan"/>
<input_port name="start_joint_state"
default="{start_joint_state}"/>
<input_port name="goal_joint_state"
default="{goal_joint_state}"/>
<output_port name="trajectory"
default="{trajectory}"/>
</Action>
<Action ID="GenerateMotionPlanService"
editable="true">
<input_port name="service_name"
Expand Down Expand Up @@ -232,6 +328,13 @@
<input_port name="tool_paths"
default="{tool_paths}"/>
</Action>
<Action ID="GetCurrentJointState"
editable="true">
<input_port name="topic_name"
default="/joint_states"/>
<output_port name="current_state"
default="{current_state}"/>
</Action>
<Action ID="MotionPlanPub"
editable="true">
<input_port name="topic_name"/>
Expand Down Expand Up @@ -278,9 +381,9 @@
</Action>
<Action ID="UpdateTrajectoryStartState"
editable="true">
<input_port name="topic_name"
default="/joint_states"/>
<input_port name="input"
<input_port name="joint_state"
default="{current_state}"/>
<input_port name="input_trajectory"
default="{trajectory}"/>
<output_port name="output"
default="{trajectory}"/>
Expand All @@ -294,6 +397,15 @@
<input_port name="departure"
default="{departure}"/>
</SubTree>
<SubTree ID="process_pub"
editable="true">
<input_port name="approach"
default="{approach}"/>
<input_port name="process"
default="{process}"/>
<input_port name="departure"
default="{departure}"/>
</SubTree>
<SubTree ID="scan"
editable="true">
<input_port name="approach"
Expand Down

0 comments on commit 1c46c1b

Please sign in to comment.