robossembler/runtime
7
0
Fork 0
Code Issues 4 Pull requests Projects Releases Packages Wiki Activity Actions

Add minimal pick-and-place case with world embedded

Browse source
This commit is contained in:
Igor Brylyov 2023-03-31 20:24:56 +00:00
parent 209e99a4b3
commit a87fb8a7ec
64 changed files with 2419 additions and 275 deletions
Download patch file Download diff file Expand all files Collapse all files

15
README.md
View file

@ -21,15 +21,22 @@ These are the primary dependencies required to use this project.
Prepare workspace & install dependencies (So far only tested with UR robot arm) Prepare workspace & install dependencies (So far only tested with UR robot arm)
``` ```bash
mkdir -p ~/robossembler_ws/src && cd ~/robossembler_ws/src mkdir -p ~/robossembler_ws/src && cd ~/robossembler_ws/src
git clone https://gitlab.com/robosphere/robossembler-ros2 git clone https://gitlab.com/robosphere/robossembler-ros2
vcs import . < robossembler-ros2/rbs.repos vcs import . < robossembler-ros2/rbs.repos
cd .. cd ..
rosdep install -y -r -q --from-paths src --ignore-src --rosdistro humble rosdep install -y -r -q --from-paths src --ignore-src --rosdistro humble
colcon build colcon build --merge-install --symlink-install # args are optional
``` ```
### Set Gazebo enviroment variables
Replace `[WS_FOLDER]` with your workspace folder
```bash
echo "export IGN_GAZEBO_RESOURCE_PATH=${IGN_GAZEBO_RESOURCE_PATH}:~/robossembler_ws/install/share/rbs_simulation:~/robossembler_ws/install/share" >> ~/.bashrc
```
### Examples ### Examples
Add source to environment Add source to environment
``` ```
@ -40,12 +47,12 @@ Launch MoveIt2, Gazebo, RViz
```bash ```bash
ros2 launch rbs_simulation rbs_simulation.launch.py ros2 launch rbs_simulation rbs_simulation.launch.py
``` ```
#### Launch bt_tree #### Launch bt_tree (TBD)
It will execute `bt_tree` once from file `rbs_bt_executor/bt_trees/test_tree.xml` It will execute `bt_tree` once from file `rbs_bt_executor/bt_trees/test_tree.xml`
``` ```
ros2 launch rbs_executor rbs_executor.launch.py ros2 launch rbs_bt_executor rbs_executor.launch.py
``` ```
The robot arm should move to the point from config file in path The robot arm should move to the point from config file in path

8
rbs.repos
View file

@ -1,7 +1,7 @@
repositories: repositories:
ur_description: ur_description:
type: git type: git
url: https://github.com/UniversalRobots/Universal_Robots_ROS2_Description.git url: https://github.com/solid-sinusoid/Universal_Robots_ROS2_Description.git
version: ros2 version: ros2
ur_moveit_config: ur_moveit_config:
type: git type: git
@ -15,7 +15,11 @@ repositories:
type: git type: git
url: https://github.com/solid-sinusoid/behavior_tree.git url: https://github.com/solid-sinusoid/behavior_tree.git
version: master version: master
ros2_robotiq_gripper:
type: git
url: https://github.com/solid-sinusoid/ros2_robotiq_gripper.git
version: main
ros_gz: ros_gz:
type: git type: git
url: https://github.com/gazebosim/ros_gz.git url: https://github.com/gazebosim/ros_gz.git
version: humble version: humble

11
rbs_bt_executor/CMakeLists.txt
View file

@ -45,6 +45,17 @@ include_directories(include)
add_library(rbs_skill_move_topose_bt_action_client SHARED src/MoveToPose.cpp) add_library(rbs_skill_move_topose_bt_action_client SHARED src/MoveToPose.cpp)
list(APPEND plugin_libs rbs_skill_move_topose_bt_action_client) list(APPEND plugin_libs rbs_skill_move_topose_bt_action_client)
add_library(rbs_skill_gripper_move_bt_action_client SHARED src/gripper_move.cpp)
list(APPEND plugin_libs rbs_skill_gripper_move_bt_action_client)
add_library(rbs_skill_get_pick_place_pose_service_client SHARED src/GetPickPlacePoses.cpp)
list(APPEND plugin_libs rbs_skill_get_pick_place_pose_service_client)
add_library(rbs_skill_move_joint_state SHARED src/MoveToJointStates.cpp)
list(APPEND plugin_libs rbs_skill_move_joint_state)
add_library(rbs_add_planning_scene_object SHARED src/AddPlanningSceneObject.cpp)
list(APPEND plugin_libs rbs_add_planning_scene_object)
foreach(bt_plugin ${plugin_libs}) foreach(bt_plugin ${plugin_libs})
ament_target_dependencies(${bt_plugin} ${dependencies}) ament_target_dependencies(${bt_plugin} ${dependencies})

19
rbs_bt_executor/bt_trees/example_repeat.xml Normal file
View file

@ -0,0 +1,19 @@
<?xml version="1.0"?>
<root main_tree_to_execute="BehaviorTree">
<!-- ////////// -->
<BehaviorTree ID="BehaviorTree">
<Sequence>
<SetBlackboard output_key="" value=""/>
<Repeat num_cycles="3">
<Sequence>
<AlwaysSuccess/>
<AlwaysSuccess/>
<AlwaysFailure/>
</Sequence>
</Repeat>
</Sequence>
</BehaviorTree>
<!-- ////////// -->
<TreeNodesModel/>
<!-- ////////// -->
</root>

30
rbs_bt_executor/bt_trees/find_object.xml Normal file
View file

@ -0,0 +1,30 @@
<?xml version="1.0" encoding="UTF-8"?>
<root BTCPP_format="4">
<BehaviorTree ID="Untitled">
<Sequence>
<SetBlackboard name="InitBT"
value="param_name"
output_key="param_values"/>
<Fallback>
<ReactiveSequence>
<GoToPose PoseArray="{param}"
robot_name="{robot_name}"/>
<AproachObject/>
</ReactiveSequence>
<AlwaysSuccess name="DoSomethingElse"/>
</Fallback>
</Sequence>
</BehaviorTree>
<!-- Description of Node Models (used by Groot) -->
<TreeNodesModel>
<Condition ID="AproachObject"
editable="true"/>
<Action ID="GoToPose"
editable="true">
<input_port name="PoseArray">Load from outside</input_port>
<input_port name="robot_name"/>
</Action>
</TreeNodesModel>
</root>

19
rbs_bt_executor/bt_trees/gripper_tree.xml Normal file
View file

@ -0,0 +1,19 @@
<?xml version="1.0"?>
<root main_tree_to_execute="GripperTree">
<BehaviorTree ID="GripperTree">
<Sequence>
<Action ID = "GripperControl"
pose = "open"
server_name="/gripper_control"
server_timeout="10"
cancel_timeout="5" />
</Sequence>
</BehaviorTree>
<TreeNodesModel>
<Action ID = "GripperControl">
<input_port name = "pose"/>
</Action>
</TreeNodesModel>
</root>

61
rbs_bt_executor/bt_trees/pick_and_place_modified_test_xml.xml Normal file
View file

@ -0,0 +1,61 @@
<?xml version="1.0"?>
<root main_tree_to_execute="BehaviorTree">
<!-- ////////// -->
<BehaviorTree ID="BehaviorTree">
<Sequence>
<Action GraspPose="GraspPose" ID="GetPickPlacePoses" ObjectName="ObjectPose" PlacePose="PlacePose" PostGraspPose="PostGraspPose" PostPlacePose="PostPlacePose" PreGraspPose="PreGraspPose" PrePlacePose="PrePlacePose"/>
<ReactiveSequence>
<Sequence>
<SubTree ID="Pick"/>
<SubTree ID="Place"/>
</Sequence>
<Action ID="SetNextObject" new_object="ObjectName"/>
<Action GraspPose="GraspPose" ID="GetPickPlacePoses" ObjectName="ObjectPose" PlacePose="PlacePose" PostGraspPose="PostGraspPose" PostPlacePose="PostPlacePose" PreGraspPose="PreGraspPose" PrePlacePose="PrePlacePose"/>
</ReactiveSequence>
</Sequence>
</BehaviorTree>
<!-- ////////// -->
<BehaviorTree ID="Pick">
<Sequence>
<Action ID="MoveToPose" motion_type="ptp" pose="PreGraspPose"/>
<Action ID="GripperCommand" command="open"/>
<Action ID="MoveToPose" motion_type="cp" pose="GraspPosition"/>
<Action ID="GripperCommand" command="close"/>
<Action ID="MoveToPose" motion_type="cp" pose="PostGraspPose"/>
</Sequence>
</BehaviorTree>
<!-- ////////// -->
<BehaviorTree ID="Place">
<Sequence>
<Action ID="MoveToPose" motion_type="ptp" pose="PrePlacePose"/>
<Action ID="MoveToPose" motion_type="cp" pose="PlacePose"/>
<Action ID="GripperCommand" command="open"/>
<Action ID="MoveToPose" motion_type="cp" pose="PostPlacePose"/>
</Sequence>
</BehaviorTree>
<!-- ////////// -->
<TreeNodesModel>
<Action ID="GetPickPlacePoses">
<output_port default="GraspPose" name="GraspPose"/>
<inout_port default="ObjectPose" name="ObjectName"/>
<output_port default="PlacePose" name="PlacePose"/>
<output_port default="PostGraspPose" name="PostGraspPose"/>
<output_port default="PostPlacePose" name="PostPlacePose"/>
<output_port default="PreGraspPose" name="PreGraspPose"/>
<output_port default="PrePlacePose" name="PrePlacePose"/>
</Action>
<Action ID="GripperCommand">
<input_port default="open" name="command"/>
</Action>
<Action ID="MoveToPose">
<input_port default="cartesian" name="motion_type"/>
<input_port default="PreGraspPose" name="pose"/>
</Action>
<SubTree ID="Pick"/>
<SubTree ID="Place"/>
<Action ID="SetNextObject">
<inout_port name="new_object"/>
</Action>
</TreeNodesModel>
<!-- ////////// -->
</root>

50
rbs_bt_executor/bt_trees/pick_and_place_test.xml Normal file
View file

@ -0,0 +1,50 @@
<?xml version="1.0"?>
<root main_tree_to_execute="BehaviorTree">
<!-- ////////// -->
<BehaviorTree ID="BehaviorTree">
<ReactiveSequence>
<SubTree ID="Pick"/>
<SubTree ID="Place"/>
</ReactiveSequence>
</BehaviorTree>
<!-- ////////// -->
<BehaviorTree ID="Pick">
<Sequence>
<Action ID="GetPickPlacePoses" ObjectPose="ObjectPose" Pose="{GraspPose}" PostPose="{PostGraspPose}" PrePose="{PreGraspPose}" name="box1"/>
<Action ID="MoveToPose" motion_type="ptp" pose="PreGraspPose"/>
<Action ID="GripperCommand" command="open"/>
<Action ID="MoveToPose" motion_type="cp" pose="GraspPosition"/>
<Action ID="GripperCommand" command="close"/>
<Action ID="MoveToPose" motion_type="cp" pose="PostGraspPose"/>
</Sequence>
</BehaviorTree>
<!-- ////////// -->
<BehaviorTree ID="Place">
<Sequence>
<Action ID="GetPickPlacePoses" ObjectPose="ObjectPose" Pose="PlacePose" PostPose="PostPlacePose" PrePose="PrePlacePose" name="GetPlacePoses"/>
<Action ID="MoveToPose" motion_type="ptp" pose="PrePlacePose"/>
<Action ID="MoveToPose" motion_type="cp" pose="PlacePose"/>
<Action ID="GripperCommand" command="open"/>
<Action ID="MoveToPose" motion_type="cp" pose="PostPlacePose"/>
</Sequence>
</BehaviorTree>
<!-- ////////// -->
<TreeNodesModel>
<Action ID="GetPickPlacePoses">
<input_port default="ObjectPose" name="ObjectPose"/>
<output_port default="GraspPose" name="Pose"/>
<output_port default="PostGraspPose" name="PostPose"/>
<output_port default="PreGraspPose" name="PrePose"/>
</Action>
<Action ID="GripperCommand">
<input_port default="open" name="command"/>
</Action>
<Action ID="MoveToPose">
<input_port default="cartesian" name="motion_type"/>
<input_port default="PreGraspPose" name="pose"/>
</Action>
<SubTree ID="Pick"/>
<SubTree ID="Place"/>
</TreeNodesModel>
<!-- ////////// -->
</root>

74
rbs_bt_executor/bt_trees/test_tree.xml
View file

@ -2,36 +2,68 @@
<root main_tree_to_execute="MainTree"> <root main_tree_to_execute="MainTree">
<BehaviorTree ID="MainTree"> <BehaviorTree ID="MainTree">
<Sequence> <Sequence>
<Action ID="MoveToPose" <Action ID="GetPickPlacePoses"
robot_name="ur_manipulator" ObjectName="box1"
pose="pose1" GraspPose="{GraspPose}"
PostGraspPose="{PostGraspPose}"
PostGraspPose2="{PostGraspPose2}"
PreGraspPose="{PreGraspPose}"
PlacePose="{PlacePose}"
PrePlacePose="{PrePlacePose}"
PostPlacePose="{PostPlacePose}"
PrePlaceJointState="{PrePlaceJointState}"
NextObject = "{NextObject}"
server_name="/get_pick_place_pose_service"
server_timeout="1000"/>
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PreGraspPose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{GraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GripperControl" pose = "close" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostGraspPose2}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<!-- <Action ID="MoveToJointState" robot_name="ur_manipulator_gripper" PrePlaceJointState="{PrePlaceJointState}" server_name="/move_to_joint_states" server_timeout="10000" cancel_timeout="5000" /> -->
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PrePlacePose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<!-- <Action ID="AddPlanningSceneObject" ObjectName="{NextObject}" pose="{PlacePose}" server_name="/add_planing_scene_object" server_timeout="1000"/> -->
<Action ID="GripperControl" pose = "open" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Repeat num_cycles="5">
<Sequence>
<Action ID="GetPickPlacePoses"
ObjectName="{NextObject}"
GraspPose="{GraspPose}"
PostGraspPose="{PostGraspPose}"
PostGraspPose2="{PostGraspPose2}"
PreGraspPose="{PreGraspPose}"
PlacePose="{PlacePose}"
PrePlacePose="{PrePlacePose}"
PostPlacePose="{PostPlacePose}"
PrePlaceJointState="{PrePlaceJointState}"
NextObject = "{NextObject}"
server_name="/get_pick_place_pose_service"
server_timeout="1000"/>
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PreGraspPose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{GraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GripperControl" pose = "close" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostGraspPose2}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<!-- <Action ID="MoveToJointState" robot_name="ur_manipulator_gripper" PrePlaceJointState="{PrePlaceJointState}" server_name="/move_to_joint_states" server_timeout="10000" cancel_timeout="5000" /> -->
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PrePlacePose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GripperControl" pose = "open" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
</Sequence>
</Repeat>
server_name="/move_topose"
server_timeout="1000"
cancel_timeout="500" />
<Action ID="MoveToPose"
robot_name="ur_manipulator"
pose="pose2"
server_name="/move_topose"
server_timeout="1000"
cancel_timeout="500" />
<Action ID="MoveToPose"
robot_name="ur_manipulator"
pose="pose3"
server_name="/move_topose"
server_timeout="1000"
cancel_timeout="500" />
</Sequence> </Sequence>
</BehaviorTree> </BehaviorTree>
<TreeNodesModel> <TreeNodesModel>
<Action ID="MoveToPose"> <Action ID="MoveToPose">
<input_port name="robot_name"/> <input_port name="robot_name"/>
<input_port name="pose"/> <input_port name="pose_name"/>
</Action> </Action>
</TreeNodesModel> </TreeNodesModel>
</root> </root>

73
rbs_bt_executor/config/gripperPositions.yaml Normal file
View file

@ -0,0 +1,73 @@
box1:
PreGraspPose: [0.25, 0.55, 0.6515, -0.707, 0.0, 0.0, 0.707]
GraspPose: [0.25, 0.655, 0.6515, -0.707, 0.0, 0.0, 0.707]
PostGraspPose: [0.25, 0.655, 0.67, -0.707, 0.0, 0.0, 0.707]
PostGraspPose2: [0.25, 0.5, 0.67, -0.707, 0.0, 0.0, 0.707]
PrePlacePose: [-0.4500, -0.055, 0.6, 1.0, 0.0, 0.0, 0.0]
PlacePose: [-0.4500, -0.055, 0.46, 1.0, 0.0, 0.0, 0.0]
PostPlacePose: [-0.4500, -0.055, 0.6, 1.0, 0.0, 0.0, 0.0]
box2:
PreGraspPose: [0.0, 0.55, 0.6515, -0.707, 0.0, 0.0, 0.707]
GraspPose: [0.0, 0.655, 0.6515, -0.707, 0.0, 0.0, 0.707]
PostGraspPose: [0.0, 0.655, 0.67, -0.707, 0.0, 0.0, 0.707]
PostGraspPose2: [0.0, 0.5, 0.67, -0.707, 0.0, 0.0, 0.707]
PrePlacePose: [-0.4500, 0.0, 0.6, 1.0, 0.0, 0.0, 0.0]
PlacePose: [-0.4500, 0.0, 0.46, 1.0, 0.0, 0.0, 0.0]
PostPlacePose: [-0.4500, 0.0, 0.6, 1.0, 0.0, 0.0, 0.0]
box3:
PreGraspPose: [-0.25, 0.55, 0.6515, -0.707, 0.0, 0.0, 0.707]
GraspPose: [-0.25, 0.655, 0.6515, -0.707, 0.0, 0.0, 0.707]
PostGraspPose: [-0.25, 0.655, 0.67, -0.707, 0.0, 0.0, 0.707]
PostGraspPose2: [-0.25, 0.5, 0.67, -0.707, 0.0, 0.0, 0.707]
PrePlacePose: [-0.4500, 0.055, 0.6, 1.0, 0.0, 0.0, 0.0]
PlacePose: [-0.4500, 0.055, 0.46, 1.0, 0.0, 0.0, 0.0]
PostPlacePose: [-0.4500, 0.055, 0.6, 1.0, 0.0, 0.0, 0.0]
box4:
PreGraspPose: [0.25, 0.55, 0.3015, -0.707, 0.0, 0.0, 0.707]
GraspPose: [0.25, 0.655, 0.3015, -0.707, 0.0, 0.0, 0.707]
PostGraspPose: [0.25, 0.655, 0.32, -0.707, 0.0, 0.0, 0.707]
PostGraspPose2: [0.25, 0.5, 0.32, -0.707, 0.0, 0.0, 0.707]
PrePlacePose: [-0.4500, 0.0275, 0.6, 1.0, 0.0, 0.0, 0.0]
PlacePose: [-0.4500, 0.0275, 0.49, 1.0, 0.0, 0.0, 0.0]
PostPlacePose: [-0.4500, 0.0275, 0.6, 1.0, 0.0, 0.0, 0.0]
box5:
PreGraspPose: [0.0, 0.55, 0.3015, -0.707, 0.0, 0.0, 0.707]
GraspPose: [0.0, 0.655, 0.3015, -0.707, 0.0, 0.0, 0.707]
PostGraspPose: [0.0, 0.655, 0.32, -0.707, 0.0, 0.0, 0.707]
PostGraspPose2: [0.0, 0.5, 0.32, -0.707, 0.0, 0.0, 0.707]
PrePlacePose: [-0.4500, -0.0275, 0.6, 1.0, 0.0, 0.0, 0.0]
PlacePose: [-0.4500, -0.0275, 0.49, 1.0, 0.0, 0.0, 0.0]
PostPlacePose: [-0.4500, -0.0275, 0.6, 1.0, 0.0, 0.0, 0.0]
box6:
PreGraspPose: [-0.25, 0.55, 0.3015, -0.707, 0.0, 0.0, 0.707]
GraspPose: [-0.25, 0.655, 0.3015, -0.707, 0.0, 0.0, 0.707]
PostGraspPose: [-0.25, 0.655, 0.32, -0.707, 0.0, 0.0, 0.707]
PostGraspPose2: [-0.25, 0.5, 0.32, -0.707, 0.0, 0.0, 0.707]
PrePlacePose: [-0.4500, 0.0, 0.6, 1.0, 0.0, 0.0, 0.0]
PlacePose: [-0.4500, 0.0, 0.56, 1.0, 0.0, 0.0, 0.0]
PostPlacePose: [-0.4500, 0.0, 0.6, 1.0, 0.0, 0.0, 0.0]
pre_place_joint_states:
- 0.11030024152330242
- 0.8306162497371689
- -1.7680363334650195
- -0.6262361658856159
- 1.4713289344704141
- -0.11066274809566562
scene_objects:
- box1
- box2
- box3
- box4
- box5
- box6
- last

31
rbs_bt_executor/config/params.yaml
View file

@ -1,6 +1,33 @@
# Here is a nodes which calling from launch for bt_tree # Here is a nodes which calling from launch for bt_tree
simple_move_bt_nodes: simple_move_bt_nodes:
ros__parameters: ros__parameters:
plugins: # plugins:
- rbs_skill_move_topose_bt_action_client # - rbs_skill_move_topose_bt_action_client
pose1: [
0.11724797630931184, #X position
0.46766635768602544, #Y
0.5793862343094913, #Z
0.9987999001314066, #X orientation
0.041553846820940925, #Y
-0.004693314677006583, #Z
-0.025495295825239704 #W
]
pose2: [
-0.11661364861606047,
0.4492600889665261,
0.5591700913807053,
0.9962273179258467,
0.04057380155886888,
0.009225849745372298,
0.07615629548377048
]
pose3: [
-0.07133612047767886,
0.41038906578748613,
0.2844649846989331,
0.999078481789772,
0.04109234110437432,
0.006539754292177074,
0.010527978961032304
]

74
rbs_bt_executor/launch/rbs_executor.launch.py
View file

@ -1,7 +1,40 @@
from launch import LaunchDescription from launch import LaunchDescription
from launch_ros.actions import Node from launch_ros.actions import Node
from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
from launch_ros.substitutions import FindPackageShare
from ament_index_python.packages import get_package_share_directory from ament_index_python.packages import get_package_share_directory
import os import os
import yaml
import math
def construct_angle_radians(loader, node):
"""Utility function to construct radian values from yaml."""
value = loader.construct_scalar(node)
try:
return float(value)
except SyntaxError:
raise Exception("invalid expression: %s" % value)
def construct_angle_degrees(loader, node):
"""Utility function for converting degrees into radians from yaml."""
return math.radians(construct_angle_radians(loader, node))
def load_yaml(package_name, file_path):
package_path = get_package_share_directory(package_name)
absolute_file_path = os.path.join(package_path, file_path)
try:
yaml.SafeLoader.add_constructor("!radians", construct_angle_radians)
yaml.SafeLoader.add_constructor("!degrees", construct_angle_degrees)
except Exception:
raise Exception("yaml support not available; install python-yaml")
try:
with open(absolute_file_path) as file:
return yaml.safe_load(file)
except OSError: # parent of IOError, OSError *and* WindowsError where available
return None
def generate_launch_description(): def generate_launch_description():
@ -11,25 +44,46 @@ def generate_launch_description():
'config', 'config',
'params.yaml' 'params.yaml'
) )
points = os.path.join( points = load_yaml(
get_package_share_directory('rbs_bt_executor'), package_name="rbs_bt_executor",
'config', file_path="config/points.yaml"
'points.yaml'
) )
gripperPoints = load_yaml(
package_name="rbs_bt_executor",
file_path="config/gripperPositions.yaml")
robot_description_semantic_content = Command(
[
PathJoinSubstitution([FindExecutable(name="xacro")]),
" ",
PathJoinSubstitution(
[FindPackageShare("ur_moveit_config"), "srdf", "ur.srdf.xacro"]
),
" ",
"name:=",
"ur",
" ",
"prefix:=",
"",
" ",
]
)
robot_description_semantic = {"robot_description_semantic": robot_description_semantic_content}
return LaunchDescription([ return LaunchDescription([
Node( Node(
package='behavior_tree', package='behavior_tree',
namespace='', namespace='',
executable='bt_engine', executable='bt_engine',
name="bt_engine", #prefix=['xterm -e gdb -ex run --args'],
# Do not declare a node name otherwise it messes with the action node names and will result in
# duplicate nodes!
parameters=[ parameters=[
{'bt_file_path': os.path.join(get_package_share_directory('rbs_bt_executor'), 'bt_trees/test_tree.xml')}, {'bt_file_path': os.path.join(get_package_share_directory('rbs_bt_executor'), 'bt_trees/test_tree.xml')},
{'plugins': ['rbs_skill_move_topose_bt_action_client']}, {'plugins':['rbs_skill_move_topose_bt_action_client', "rbs_skill_get_pick_place_pose_service_client", "rbs_skill_gripper_move_bt_action_client", "rbs_skill_move_joint_state", "rbs_add_planning_scene_object"]},
points gripperPoints,
robot_description_semantic
] ]
), )
]) ])

47
rbs_bt_executor/src/AddPlanningSceneObject.cpp Normal file
View file

@ -0,0 +1,47 @@
#include "rbs_skill_interfaces/srv/add_planning_scene_object.hpp"
#include "behaviortree_cpp_v3/bt_factory.h"
#include "behavior_tree/BtService.hpp"
using namespace BT;
using AddPlanningSceneObjectClient = rbs_skill_interfaces::srv::AddPlanningSceneObject;
class GetPickPlacePose : public BtService<AddPlanningSceneObjectClient>
{
public:
GetPickPlacePose(const std::string & name, const BT::NodeConfiguration & config)
: BtService<AddPlanningSceneObjectClient>(name, config) {
RCLCPP_INFO(_node->get_logger(), "Start the node");
}
AddPlanningSceneObjectClient::Request::SharedPtr populate_request() override
{
auto request = std::make_shared<AddPlanningSceneObjectClient::Request>();
RCLCPP_INFO(_node->get_logger(), "Start populate_request()");
object_name_ = getInput<std::string>("ObjectName").value();
auto place_pose_ = getInput<std::vector<double>>("pose").value();
request->object_id = object_name_;
request->object_pose = place_pose_;
return request;
}
BT::NodeStatus handle_response(AddPlanningSceneObjectClient::Response::SharedPtr response) override
{
RCLCPP_INFO(_node->get_logger(), "Response %d", response->success);
return BT::NodeStatus::SUCCESS;
}
static PortsList providedPorts() {
return providedBasicPorts({
InputPort<std::string>("ObjectName"),
InputPort<std::vector<double>>("pose"),
});
}
private:
std::string object_name_{};
};
BT_REGISTER_NODES(factory) {
factory.registerNodeType<GetPickPlacePose>("AddPlanningSceneObject");
}

63
rbs_bt_executor/src/GetPickPlacePoses.cpp Normal file
View file

@ -0,0 +1,63 @@
#include "rbs_skill_interfaces/srv/get_pick_place_poses.hpp"
#include "behaviortree_cpp_v3/bt_factory.h"
#include "behavior_tree/BtService.hpp"
using namespace BT;
using GetPickPlacePoseClient = rbs_skill_interfaces::srv::GetPickPlacePoses;
class GetPickPlacePose : public BtService<GetPickPlacePoseClient>
{
public:
GetPickPlacePose(const std::string & name, const BT::NodeConfiguration & config)
: BtService<GetPickPlacePoseClient>(name, config) {
RCLCPP_INFO(_node->get_logger(), "Start the node");
}
GetPickPlacePoseClient::Request::SharedPtr populate_request() override
{
auto request = std::make_shared<GetPickPlacePoseClient::Request>();
RCLCPP_INFO(_node->get_logger(), "Start populate_request()");
object_name_ = getInput<std::string>("ObjectName").value();
RCLCPP_INFO_STREAM(_node->get_logger(), "Starting process for object: " << object_name_);
request->object_name = object_name_;
return request;
}
BT::NodeStatus handle_response(GetPickPlacePoseClient::Response::SharedPtr response) override
{
RCLCPP_INFO(_node->get_logger(), "Start handle_response()");
setOutput<std::vector<double>>("GraspPose", response->grasp_pose);
setOutput<std::vector<double>>("PreGraspPose", response->pre_grasp_pose);
setOutput<std::vector<double>>("PostGraspPose", response->post_grasp_pose);
setOutput<std::vector<double>>("PostGraspPose2", response->post_grasp_pose_d);
setOutput<std::vector<double>>("PlacePose", response->place_pose);
setOutput<std::vector<double>>("PrePlacePose", response->pre_place_pose);
setOutput<std::vector<double>>("PostPlacePose", response->post_place_pose);
setOutput<std::vector<double>>("PrePlaceJointState", response->pre_place_joint_state);
setOutput<std::string>("NextObject", response->next_object);
return BT::NodeStatus::SUCCESS;
}
static PortsList providedPorts() {
return providedBasicPorts({
InputPort<std::string>("ObjectName"),
OutputPort<std::string>("ObjectName"),
OutputPort<std::vector<double>>("GraspPose"),
OutputPort<std::vector<double>>("PreGraspPose"),
OutputPort<std::vector<double>>("PostGraspPose"),
OutputPort<std::vector<double>>("PostGraspPose2"),
OutputPort<std::vector<double>>("PlacePose"),
OutputPort<std::vector<double>>("PrePlacePose"),
OutputPort<std::vector<double>>("PostPlacePose"),
OutputPort<std::vector<double>>("PrePlaceJointState"),
OutputPort<std::string>("NextObject")
});
}
private:
std::string object_name_{};
};
BT_REGISTER_NODES(factory) {
factory.registerNodeType<GetPickPlacePose>("GetPickPlacePoses");
}

62
rbs_bt_executor/src/MoveToJointStates.cpp Normal file
View file

@ -0,0 +1,62 @@
#include "rbs_skill_interfaces/action/moveit_send_joint_states.hpp"
#include "behaviortree_cpp_v3/bt_factory.h"
#include "behavior_tree/BtAction.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "moveit_msgs/action/move_group.h"
// #include "Eigen/Geometry"
#include "rclcpp/parameter.hpp"
using namespace BT;
using MoveToJointStateAction = rbs_skill_interfaces::action::MoveitSendJointStates;
static const rclcpp::Logger LOGGER = rclcpp::get_logger("MoveToJointStateActionClient");
class MoveToJointState : public BtAction<MoveToJointStateAction>
{
public:
MoveToJointState(const std::string & name, const BT::NodeConfiguration & config)
: BtAction<MoveToJointStateAction>(name, config)
{
RCLCPP_INFO(_node->get_logger(), "Start the node");
}
MoveToJointStateAction::Goal populate_goal() override
{
getInput<std::string>("robot_name", robot_name_);
getInput<std::vector<double>>("PrePlaceJointState", joint_values_);
auto goal = MoveToJointStateAction::Goal();
RCLCPP_INFO(_node->get_logger(), "Send goal for robot [%s]", robot_name_.c_str());
for (auto joint : joint_values_)
{
RCLCPP_INFO_STREAM(_node->get_logger(), "Joint value " << joint);
}
RCLCPP_INFO_STREAM(_node->get_logger(), "Joint value size " << joint_values_.size());
goal.robot_name = robot_name_;
goal.joint_values = joint_values_;
RCLCPP_INFO(_node->get_logger(), "Goal populated");
return goal;
}
static PortsList providedPorts()
{
return providedBasicPorts({
InputPort<std::string>("robot_name"),
InputPort<std::vector<double>>("PrePlaceJointState")
});
}
private:
std::string robot_name_{};
std::vector<double> joint_values_;
}; // class MoveToJointState
BT_REGISTER_NODES(factory)
{
factory.registerNodeType<MoveToJointState>("MoveToJointState");
}

65
rbs_bt_executor/src/MoveToPose.cpp
View file

@ -17,62 +17,21 @@ class MoveToPose : public BtAction<MoveToPoseAction>
MoveToPose(const std::string & name, const BT::NodeConfiguration & config) MoveToPose(const std::string & name, const BT::NodeConfiguration & config)
: BtAction<MoveToPoseAction>(name, config) : BtAction<MoveToPoseAction>(name, config)
{ {
if (!_node->has_parameter("pose1"))
{
_node->declare_parameter("pose1",std::vector<double>{ 0.11724797630931184,
0.46766635768602544,
0.5793862343094913,
0.9987999001314066,
0.041553846820940925,
-0.004693314677006583,
-0.025495295825239704
}
);
}
if (!_node->has_parameter("pose2"))
{
_node->declare_parameter("pose2",std::vector<double>{ -0.11661364861606047,
0.4492600889665261,
0.5591700913807053,
0.9962273179258467,
0.04057380155886888,
0.009225849745372298,
0.07615629548377048
}
);
}
if (!_node->has_parameter("pose3"))
{
_node->declare_parameter("pose3",std::vector<double>{ -0.07133612047767886,
0.41038906578748613,
0.2844649846989331,
0.999078481789772,
0.04109234110437432,
0.006539754292177074,
0.010527978961032304
}
);
}
RCLCPP_INFO(_node->get_logger(), "Start the node"); RCLCPP_INFO(_node->get_logger(), "Start the node");
} }
MoveToPoseAction::Goal populate_goal() override MoveToPoseAction::Goal populate_goal() override
{ {
getInput<std::string>("robot_name", robot_name_); getInput<std::string>("robot_name", robot_name_);
getInput<std::string>("pose", pose_); getInput<std::vector<double>>("pose", pose_);
rclcpp::Parameter arr = _node->get_parameter(pose_);
std::vector<double> possd = arr.as_double_array();
pose_des.position.x = possd[0]; pose_des.position.x = pose_[0];
pose_des.position.y = possd[1]; pose_des.position.y = pose_[1];
pose_des.position.z = possd[2]; pose_des.position.z = pose_[2];
pose_des.orientation.x = possd[3]; pose_des.orientation.x = pose_[3];
pose_des.orientation.y = possd[4]; pose_des.orientation.y = pose_[4];
pose_des.orientation.z = possd[5]; pose_des.orientation.z = pose_[5];
pose_des.orientation.w = possd[6]; pose_des.orientation.w = pose_[6];
RCLCPP_INFO(_node->get_logger(), "\n Send Pose: \n\t pose.x: %f \n\t pose.y: %f \n\t pose.z: %f \n\n\t opientation.x: %f \n\t orientation.y: %f \n\t orientation.z: %f \n\t orientation.w: %f", RCLCPP_INFO(_node->get_logger(), "\n Send Pose: \n\t pose.x: %f \n\t pose.y: %f \n\t pose.z: %f \n\n\t opientation.x: %f \n\t orientation.y: %f \n\t orientation.z: %f \n\t orientation.w: %f",
@ -84,8 +43,8 @@ class MoveToPose : public BtAction<MoveToPoseAction>
goal.robot_name = robot_name_; goal.robot_name = robot_name_;
goal.target_pose = pose_des; goal.target_pose = pose_des;
goal.end_effector_acceleration = 1.0; goal.end_effector_acceleration = 0.5;
goal.end_effector_velocity = 1.0; goal.end_effector_velocity = 0.5;
RCLCPP_INFO(_node->get_logger(), "Goal populated"); RCLCPP_INFO(_node->get_logger(), "Goal populated");
@ -96,13 +55,13 @@ class MoveToPose : public BtAction<MoveToPoseAction>
{ {
return providedBasicPorts({ return providedBasicPorts({
InputPort<std::string>("robot_name"), InputPort<std::string>("robot_name"),
InputPort<std::string>("pose") InputPort<std::vector<double>>("pose")
}); });
} }
private: private:
std::string robot_name_; std::string robot_name_;
std::string pose_; std::vector<double> pose_;
geometry_msgs::msg::Pose pose_des; geometry_msgs::msg::Pose pose_des;
std::map<std::string, geometry_msgs::msg::Pose> Poses; std::map<std::string, geometry_msgs::msg::Pose> Poses;

62
rbs_bt_executor/src/gripper_move.cpp Normal file
View file

@ -0,0 +1,62 @@
#include "rbs_skill_interfaces/action/gripper_command.hpp"
#include "behaviortree_cpp_v3/bt_factory.h"
#include "behavior_tree/BtAction.hpp"
#include "rclcpp/parameter.hpp"
using namespace BT;
using GripperCommand = rbs_skill_interfaces::action::GripperCommand;
class GripperControl: public BtAction<GripperCommand> {
public:
GripperControl(const std::string & name, const BT::NodeConfiguration & config): BtAction<GripperCommand>(name, config) {
if(!_node->has_parameter("open")) {
_node->declare_parameter("open",position.open);
}
if(!_node->has_parameter("close")) {
_node->declare_parameter("close",position.close);
}
if(!_node->has_parameter("midPosition")) {
_node->declare_parameter("midPosition",position.closeFull);
}
RCLCPP_INFO(_node->get_logger(),"Init the GripperControl Bt node");
}
GripperCommand::Goal populate_goal() override {
getInput<std::string>("pose",pose);
double targetPose = _node->get_parameter(pose).as_double();
RCLCPP_INFO(_node->get_logger(), "Send target pose: %f",targetPose);
auto goal = GripperCommand::Goal();
goal.position = targetPose;
RCLCPP_INFO(_node->get_logger(),"Goal send");
return goal;
}
static PortsList providedPorts() {
return providedBasicPorts({InputPort<std::string>("pose")});
}
private:
struct {
double open = 0.2;
double closeFull = 0.8;
double close = 0.4;
} position;
std::string pose;
};
BT_REGISTER_NODES(factory) {
factory.registerNodeType<GripperControl>("GripperControl");
}

4
rbs_simulation/CMakeLists.txt
View file

@ -12,10 +12,12 @@ find_package(ament_cmake REQUIRED)
# find_package(<dependency> REQUIRED) # find_package(<dependency> REQUIRED)
install( install(
DIRECTORY launch DIRECTORY launch worlds
DESTINATION share/${PROJECT_NAME} DESTINATION share/${PROJECT_NAME}
) )
add_subdirectory(models)
if(BUILD_TESTING) if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED) find_package(ament_lint_auto REQUIRED)
# the following line skips the linter which checks for copyrights # the following line skips the linter which checks for copyrights

154
rbs_simulation/launch/rbs_simulation.launch.py
View file

@ -3,18 +3,15 @@ from launch import LaunchDescription
from launch.actions import ( from launch.actions import (
DeclareLaunchArgument, DeclareLaunchArgument,
IncludeLaunchDescription, IncludeLaunchDescription,
RegisterEventHandler ExecuteProcess
) )
from ament_index_python.packages import get_package_share_directory, get_package_share_path from ament_index_python.packages import get_package_share_directory
from launch.conditions import IfCondition, UnlessCondition from launch.conditions import IfCondition, UnlessCondition
from launch.event_handlers import OnProcessExit, OnExecutionComplete
from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
from launch_ros.actions import Node from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare from launch_ros.substitutions import FindPackageShare
from ur_moveit_config.launch_common import load_yaml from ur_moveit_config.launch_common import load_yaml
def generate_launch_description(): def generate_launch_description():
declared_arguments = [] declared_arguments = []
# UR specific arguments # UR specific arguments
@ -26,6 +23,13 @@ def generate_launch_description():
default_value="ur5e", default_value="ur5e",
) )
) )
declared_arguments.append(
DeclareLaunchArgument(
"with_gripper",
default_value="true",
description="With gripper or not?",
)
)
declared_arguments.append( declared_arguments.append(
DeclareLaunchArgument( DeclareLaunchArgument(
"safety_limits", "safety_limits",
@ -63,6 +67,13 @@ def generate_launch_description():
description="YAML file with the controllers configuration.", description="YAML file with the controllers configuration.",
) )
) )
declared_arguments.append(
DeclareLaunchArgument(
"controllers_with_gripper_file",
default_value="ur_plus_gripper_controllers.yaml",
description="YAML file with the UR + gripper_controller configuration.",
)
)
declared_arguments.append( declared_arguments.append(
DeclareLaunchArgument( DeclareLaunchArgument(
"description_package", "description_package",
@ -101,6 +112,14 @@ def generate_launch_description():
description="Robot controller to start.", description="Robot controller to start.",
) )
) )
declared_arguments.append(
DeclareLaunchArgument(
"initial_gripper_controller",
default_value="gripper_controller",
description="Robot controller to start.",
)
)
declared_arguments.append( declared_arguments.append(
DeclareLaunchArgument( DeclareLaunchArgument(
@ -129,26 +148,31 @@ def generate_launch_description():
DeclareLaunchArgument("launch_rviz", default_value="true", description="Launch RViz?") DeclareLaunchArgument("launch_rviz", default_value="true", description="Launch RViz?")
) )
# Initialize Arguments # Initialize Arguments
# TODO тут всё переименовать и сделать боеле универсальным как под нашего робото так и под UR чтобы запускалось одинаково
rbs_robot_type = LaunchConfiguration("rbs_robot_type") rbs_robot_type = LaunchConfiguration("rbs_robot_type")
safety_limits = LaunchConfiguration("safety_limits") safety_limits = LaunchConfiguration("safety_limits")
safety_pos_margin = LaunchConfiguration("safety_pos_margin") safety_pos_margin = LaunchConfiguration("safety_pos_margin")
safety_k_position = LaunchConfiguration("safety_k_position") safety_k_position = LaunchConfiguration("safety_k_position")
# General arguments # General arguments
runtime_config_package = LaunchConfiguration("runtime_config_package") runtime_config_package = LaunchConfiguration("runtime_config_package")
controllers_file = LaunchConfiguration("controllers_file") with_gripper_condition = LaunchConfiguration("with_gripper")
controllers_file = LaunchConfiguration("controllers_with_gripper_file")
description_package = LaunchConfiguration("description_package") description_package = LaunchConfiguration("description_package")
description_file = LaunchConfiguration("description_file") description_file = LaunchConfiguration("description_file")
prefix = LaunchConfiguration("prefix") prefix = LaunchConfiguration("prefix")
start_joint_controller = LaunchConfiguration("start_joint_controller") start_joint_controller = LaunchConfiguration("start_joint_controller")
initial_joint_controller = LaunchConfiguration("initial_joint_controller") initial_joint_controller = LaunchConfiguration("initial_joint_controller")
initial_gripper_controller = LaunchConfiguration("initial_gripper_controller")
launch_rviz = LaunchConfiguration("launch_rviz") launch_rviz = LaunchConfiguration("launch_rviz")
moveit_config_package = LaunchConfiguration("moveit_config_package") moveit_config_package = LaunchConfiguration("moveit_config_package")
moveit_config_file = LaunchConfiguration("moveit_config_file") moveit_config_file = LaunchConfiguration("moveit_config_file")
use_sim_time = LaunchConfiguration("use_sim_time") use_sim_time = LaunchConfiguration("use_sim_time")
initial_joint_controllers = PathJoinSubstitution(
initial_joint_controllers_file_path = PathJoinSubstitution(
[FindPackageShare(runtime_config_package), "config", controllers_file] [FindPackageShare(runtime_config_package), "config", controllers_file]
) )
@ -157,7 +181,7 @@ def generate_launch_description():
) )
world_config_file = PathJoinSubstitution( world_config_file = PathJoinSubstitution(
[FindPackageShare("sdf_models"), "worlds", "empty_world.sdf"] # TODO тут пакет извне должен задаваться [FindPackageShare("rbs_simulation"), "worlds", "mir.sdf"]
) )
robot_description_content = Command( robot_description_content = Command(
@ -180,7 +204,7 @@ def generate_launch_description():
"name:=", "name:=",
"ur", "ur",
" ", " ",
"ur_type:=", # TODO сделать более универсальным "ur_type:=",
rbs_robot_type, rbs_robot_type,
" ", " ",
"prefix:=", "prefix:=",
@ -189,7 +213,10 @@ def generate_launch_description():
"sim_ignition:=true", "sim_ignition:=true",
" ", " ",
"simulation_controllers:=", "simulation_controllers:=",
initial_joint_controllers, initial_joint_controllers_file_path,
" ",
"with_gripper:=",
with_gripper_condition
] ]
) )
robot_description = {"robot_description": robot_description_content} robot_description = {"robot_description": robot_description_content}
@ -220,13 +247,20 @@ def generate_launch_description():
arguments=[initial_joint_controller, "--controller-manager", "/controller_manager", "--stopped"], arguments=[initial_joint_controller, "--controller-manager", "/controller_manager", "--stopped"],
condition=UnlessCondition(start_joint_controller), condition=UnlessCondition(start_joint_controller),
) )
gripper_controller = ExecuteProcess(
cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
"gripper_controller"],
output='screen',
condition=IfCondition(with_gripper_condition)
)
# Gazebo nodes # Gazebo nodes
gazebo = IncludeLaunchDescription( gazebo = IncludeLaunchDescription(
PythonLaunchDescriptionSource( PythonLaunchDescriptionSource(
os.path.join(get_package_share_path('ros_ign_gazebo'), 'launch', 'ign_gazebo.launch.py')), [os.path.join(get_package_share_directory('ros_ign_gazebo'),
launch_arguments={'ign_args': ['-r'," ", world_config_file]}.items(), 'launch', 'ign_gazebo.launch.py')]),
) launch_arguments=[('ign_args', [' -r ',world_config_file, " --physics-engine ignition-physics-dartsim-plugin --render-engine ogre2"])])
# Spawn robot # Spawn robot
gazebo_spawn_robot = Node(package='ros_ign_gazebo', executable='create', gazebo_spawn_robot = Node(package='ros_ign_gazebo', executable='create',
arguments=[ arguments=[
@ -252,6 +286,8 @@ def generate_launch_description():
"prefix:=", "prefix:=",
prefix, prefix,
" ", " ",
"with_gripper:=",
with_gripper_condition
] ]
) )
robot_description_semantic = {"robot_description_semantic": robot_description_semantic_content} robot_description_semantic = {"robot_description_semantic": robot_description_semantic_content}
@ -260,7 +296,6 @@ def generate_launch_description():
[FindPackageShare(moveit_config_package), "config", "kinematics.yaml"] [FindPackageShare(moveit_config_package), "config", "kinematics.yaml"]
) )
# Planning Configuration # Planning Configuration
ompl_planning_pipeline_config = { ompl_planning_pipeline_config = {
"move_group": { "move_group": {
@ -269,16 +304,10 @@ def generate_launch_description():
"start_state_max_bounds_error": 0.1, "start_state_max_bounds_error": 0.1,
} }
} }
ompl_planning_yaml = load_yaml("ur_moveit_config", "config/ompl_planning.yaml") # TODO сделать извне ompl_planning_yaml = load_yaml("ur_moveit_config", "config/ompl_planning.yaml")
ompl_planning_pipeline_config["move_group"].update(ompl_planning_yaml) ompl_planning_pipeline_config["move_group"].update(ompl_planning_yaml)
# Trajectory Execution Configuration
controllers_yaml = load_yaml("ur_moveit_config", "config/controllers.yaml") controllers_yaml = load_yaml("ur_moveit_config", "config/controllers.yaml")
# the scaled_joint_trajectory_controller does not work on fake hardware
change_controllers = True
if change_controllers == "true":
controllers_yaml["scaled_joint_trajectory_controller"]["default"] = False
controllers_yaml["joint_trajectory_controller"]["default"] = True
moveit_controllers = { moveit_controllers = {
"moveit_simple_controller_manager": controllers_yaml, "moveit_simple_controller_manager": controllers_yaml,
@ -287,7 +316,7 @@ def generate_launch_description():
trajectory_execution = { trajectory_execution = {
"moveit_manage_controllers": True, "moveit_manage_controllers": True,
"trajectory_execution.allowed_execution_duration_scaling": 1.2, "trajectory_execution.allowed_execution_duration_scaling": 100.0,
"trajectory_execution.allowed_goal_duration_margin": 0.5, "trajectory_execution.allowed_goal_duration_margin": 0.5,
"trajectory_execution.allowed_start_tolerance": 0.01, "trajectory_execution.allowed_start_tolerance": 0.01,
} }
@ -299,12 +328,10 @@ def generate_launch_description():
"publish_transforms_updates": True, "publish_transforms_updates": True,
} }
# Start the actual move_group node/action server
move_group_node = Node( move_group_node = Node(
package="moveit_ros_move_group", package="moveit_ros_move_group",
executable="move_group", executable="move_group",
output="screen", output="screen",
name="move_group",
parameters=[ parameters=[
robot_description, robot_description,
robot_description_semantic, robot_description_semantic,
@ -326,24 +353,40 @@ def generate_launch_description():
parameters=[ parameters=[
robot_description, robot_description,
robot_description_semantic, robot_description_semantic,
ompl_planning_pipeline_config,
robot_description_kinematics, robot_description_kinematics,
], ],
condition=IfCondition(launch_rviz), condition=IfCondition(launch_rviz),
) )
# TODO: Launch skill servers in other launch file
move_topose_action_server = Node(
package="rbs_skill_servers",
executable="move_topose_action_server",
parameters=[
robot_description,
robot_description_semantic,
robot_description_kinematics,
{"use_sim_time": use_sim_time},
]
)
gripper_control_node = Node(
package="rbs_skill_servers",
executable="gripper_control_action_server",
parameters= [
robot_description,
robot_description_semantic,
robot_description_kinematics,
{"use_sim_time": use_sim_time},
]
)
move_topose_action_server = Node( move_topose_action_server = Node(
package="rbs_skill_servers", package="rbs_skill_servers",
executable="move_topose_action_server", executable="move_topose_action_server",
name="move_to_pose_moveit",
parameters=[ parameters=[
robot_description, robot_description,
robot_description_semantic, robot_description_semantic,
robot_description_kinematics, robot_description_kinematics,
ompl_planning_pipeline_config,
trajectory_execution,
moveit_controllers,
planning_scene_monitor_parameters,
{"use_sim_time": use_sim_time}, {"use_sim_time": use_sim_time},
] ]
) )
@ -351,19 +394,49 @@ def generate_launch_description():
move_cartesian_path_action_server = Node( move_cartesian_path_action_server = Node(
package="rbs_skill_servers", package="rbs_skill_servers",
executable="move_cartesian_path_action_server", executable="move_cartesian_path_action_server",
name="move_cartesian_path_action_server",
parameters=[ parameters=[
robot_description, robot_description,
robot_description_semantic, robot_description_semantic,
robot_description_kinematics, robot_description_kinematics,
ompl_planning_pipeline_config,
trajectory_execution,
moveit_controllers,
planning_scene_monitor_parameters,
{"use_sim_time": use_sim_time}, {"use_sim_time": use_sim_time},
] ]
) )
move_joint_state_action_server = Node(
package="rbs_skill_servers",
executable="move_to_joint_states_action_server",
parameters=[
robot_description,
robot_description_semantic,
robot_description_kinematics,
{"use_sim_time": use_sim_time},
]
)
points_params = load_yaml("rbs_bt_executor", "config/gripperPositions.yaml")
grasp_pose_loader = Node(
package="rbs_skill_servers",
executable="pick_place_pose_loader",
output="screen",
emulate_tty=True,
parameters=[
points_params
]
)
# add_planning_scene_object = Node(
# package="rbs_skill_servers",
# executable="add_planning_scene_object_service",
# output="screen",
# parameters=[
# robot_description,
# robot_description_semantic,
# robot_description_kinematics,
# {"use_sim_time": use_sim_time},
# ]
# )
nodes_to_start = [ nodes_to_start = [
robot_state_publisher_node, robot_state_publisher_node,
joint_state_broadcaster_spawner, joint_state_broadcaster_spawner,
@ -373,8 +446,13 @@ def generate_launch_description():
gazebo, gazebo,
gazebo_spawn_robot, gazebo_spawn_robot,
move_group_node, move_group_node,
gripper_controller,
gripper_control_node,
move_topose_action_server, move_topose_action_server,
move_cartesian_path_action_server, move_cartesian_path_action_server,
move_joint_state_action_server,
grasp_pose_loader,
#add_planning_scene_object
] ]
return LaunchDescription(declared_arguments + nodes_to_start) return LaunchDescription(declared_arguments + nodes_to_start)

21
rbs_simulation/models/CMakeLists.txt Normal file
View file

@ -0,0 +1,21 @@
macro(list_subdirectories retval curdir return_relative)
file(GLOB sub-dir RELATIVE ${curdir} *)
set(list_of_dirs "")
foreach(dir ${sub-dir})
if(IS_DIRECTORY ${curdir}/${dir})
if (${return_relative})
set(list_of_dirs ${list_of_dirs} ${dir})
else()
set(list_of_dirs ${list_of_dirs} ${curdir}/${dir})
endif()
endif()
endforeach()
set(${retval} ${list_of_dirs})
endmacro()
list_subdirectories(SUBDIR ${CMAKE_CURRENT_LIST_DIR} TRUE)
install(
DIRECTORY ${SUBDIR}
DESTINATION share/${PROJECT_NAME}
)

0
sdf_models/models/box1/frames.json → rbs_simulation/models/box1/frames.json
View file

0
sdf_models/models/box1/model.config → rbs_simulation/models/box1/model.config
View file

58
rbs_simulation/models/box1/model.sdf Normal file
View file

@ -0,0 +1,58 @@
<?xml version="1.0" ?>
<sdf version="1.4">
<model name="box1">
<pose>0 0 0.015 0 0 0</pose>
<link name="link">
<inertial>
<inertia>
<ixx>0.00004167</ixx>
<ixy>0.0</ixy>
<ixz>0.0</ixz>
<iyy>0.00004167</iyy>
<iyz>0.0</iyz>
<izz>0.00004167</izz>
</inertia>
<mass>0.1</mass>
</inertial>
<!-- <gravity>0</gravity> -->
<collision name="collision">
<geometry>
<box>
<size>0.05 0.05 0.05</size>
</box>
</geometry>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode>
<slip>
0.3
</slip>
</ode>
</torsional>
<ode>
<mu>0.2</mu>
<mu2>0.2</mu2>
</ode>
</friction>
</surface>
</collision>
<visual name="visual">
<geometry>
<box>
<size>0.05 0.05 0.05</size>
</box>
</geometry>
<!-- <material>
<ambient>0.8 0.8 0.8 1</ambient>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.8 0.8 0.8 1</specular>
</material> -->
</visual>
</link>
</model>
</sdf>

0
sdf_models/models/box12/frames.json → rbs_simulation/models/box12/frames.json
View file

0
sdf_models/models/box12/model.config → rbs_simulation/models/box12/model.config
View file

0
sdf_models/models/box12/model.sdf → rbs_simulation/models/box12/model.sdf
View file

0
sdf_models/models/box2/frames.json → rbs_simulation/models/box2/frames.json
View file

0
sdf_models/models/box2/model.config → rbs_simulation/models/box2/model.config
View file

0
sdf_models/models/box2/model.sdf → rbs_simulation/models/box2/model.sdf
View file

16
rbs_simulation/models/rack/model.config Normal file
View file

@ -0,0 +1,16 @@
<?xml version="1.0"?>
<model>
<name>rack</name>
<version>1.0</version>
<sdf version="1.6">model.sdf</sdf>
<author>
<name>brothermechanic</name>
<email>brothermechanic@gmail.com</email>
</author>
<description>
rack
</description>
</model>

25
rbs_simulation/models/rack/model.sdf Normal file
View file

@ -0,0 +1,25 @@
<?xml version="1.0" ?>
<sdf version="1.6">
<model name="rack">
<pose>0 0 0 0 0 0</pose>
<static>true</static>
<link name="body">
<visual name="visual">
<geometry>
<mesh>
<scale>1.0 1.0 1.0</scale>
<uri>model://rack/model/rack.dae</uri>
</mesh>
</geometry>
</visual>
<collision name="collision">
<geometry>
<mesh>
<scale>1.0 1.0 1.0</scale>
<uri>model://rack/model/rack.stl</uri>
</mesh>
</geometry>
</collision>
</link>
</model>
</sdf>

BIN
rbs_simulation/models/rack/model/Metal.jpg Normal file
View file

Binary file not shown.
Side by side

After

Width:  |  Height:  |  Size: 1.6 MiB

115
rbs_simulation/models/rack/model/rack.dae Normal file
View file

File diff suppressed because one or more lines are too long

BIN
rbs_simulation/models/rack/model/rack.stl Normal file
View file

Binary file not shown.

35
rbs_simulation/models/rack/rack.xacro Normal file
View file

@ -0,0 +1,35 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="rack">
<xacro:macro name="rack" params="*origin parent">
<link name="rack_body">
<static>true</static>
<inertial>
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<mass value="0.0"/>
<inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0"/>
</inertial>
<visual>
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<geometry>
<mesh filename="package://rbs_simulation/rack/model/rack.dae" scale="0.35 0.35 0.35"/>
</geometry>
<material name="asd">
<color rgba="1.0 0.0 0.0 1.0"/>
<texture filename="package://rbs_simulation/rack/model/Metal.jpg"/>
</material>
</visual>
<collision>
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<geometry>
<mesh filename="package://rbs_simulation/rack/model/rack.stl" scale="0.35 0.35 0.35"/>
</geometry>
</collision>
</link>
<joint name="rack_base_joint" type="fixed">
<parent link="${parent}" />
<child link="rack_body" />
<xacro:insert_block name="origin" />
</joint>
</xacro:macro>
</robot>

16
rbs_simulation/models/table/model.config Normal file
View file

@ -0,0 +1,16 @@
<?xml version="1.0"?>
<model>
<name>table</name>
<version>1.0</version>
<sdf version="1.6">table.sdf</sdf>
<author>
<name>brothermechanic</name>
<email>brothermechanic@gmail.com</email>
</author>
<description>
table
</description>
</model>

25
rbs_simulation/models/table/model.sdf Normal file
View file

@ -0,0 +1,25 @@
<?xml version="1.0" ?>
<sdf version="1.6">
<model name="table">
<pose>0 0 0 0 0 0</pose>
<static>true</static>
<link name="body">
<visual name="visual">
<geometry>
<mesh>
<scale>1.0 1.0 1.0</scale>
<uri>model://table/model/table.dae</uri>
</mesh>
</geometry>
</visual>
<collision name="collision">
<geometry>
<mesh>
<scale>1.0 1.0 1.0</scale>
<uri>model://table/model/table.stl</uri>
</mesh>
</geometry>
</collision>
</link>
</model>
</sdf>

BIN
rbs_simulation/models/table/model/Metal.jpg Normal file
View file

Binary file not shown.
Side by side

After

Width:  |  Height:  |  Size: 1.6 MiB

155
rbs_simulation/models/table/model/table.dae Normal file
View file

File diff suppressed because one or more lines are too long

BIN
rbs_simulation/models/table/model/table.stl Normal file
View file

Binary file not shown.

BIN
rbs_simulation/models/table/model/textures/Reflexion.jpg Normal file
View file

Binary file not shown.
Side by side

After

Width:  |  Height:  |  Size: 74 KiB

BIN
rbs_simulation/models/table/model/textures/Wood_Table_C.jpg Normal file
View file

Binary file not shown.
Side by side

After

Width:  |  Height:  |  Size: 599 KiB

BIN
rbs_simulation/models/table/model/textures/Wood_Table_C_2.jpg Normal file
View file

Binary file not shown.
Side by side

After

Width:  |  Height:  |  Size: 1.4 MiB

BIN
rbs_simulation/models/table/model/textures/Wood_Table_N.jpg Normal file
View file

Binary file not shown.
Side by side

After

Width:  |  Height:  |  Size: 878 KiB

37
rbs_simulation/models/table/table.xacro Normal file
View file

@ -0,0 +1,37 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="table">
<xacro:macro name="table" params="*origin parent">
<link name="table_body">
<static>true</static>
<inertial>
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<mass value="0.0"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
<visual>
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<geometry>
<!-- <mesh filename="package://rbs_simulation/table/model/table.dae" scale="1 1 1"/> -->
<box size="0.8 0.8 0.4"/>
</geometry>
<material name="asd">
<color rgba="1.0 1.0 0.0 1.0"/>
<!-- <texture filename="package://rbs_simulation/table/model/Metal.jpg"/> -->
</material>
</visual>
<collision>
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<geometry>
<!-- <mesh filename="package://rbs_simulation/table/model/table.stl" scale="1 1 1"/> -->
<box size="0.8 0.8 0.4"/>
</geometry>
</collision>
</link>
<joint name="table_base_joint" type="fixed">
<parent link="${parent}" />
<child link="table_body" />
<xacro:insert_block name="origin" />
</joint>
</xacro:macro>
</robot>

30
sdf_models/worlds/empty_world.sdf → rbs_simulation/worlds/empty_world.sdf
View file

@ -66,6 +66,36 @@
<pose>0 0 0 0 -0 0</pose> <pose>0 0 0 0 -0 0</pose>
<self_collide>false</self_collide> <self_collide>false</self_collide>
</model> </model>
<include>
<uri>model://box1</uri>
<name>box1</name>
<pose>0.3 0.4 0 0 0 0</pose>
</include>
<include>
<uri>model://box1</uri>
<name>box2</name>
<pose>0.2 0.4 0 0 0 0</pose>
</include>
<include>
<uri>model://box1</uri>
<name>box3</name>
<pose>0.1 0.4 0 0 0 0</pose>
</include>
<include>
<uri>model://box1</uri>
<name>box4</name>
<pose>0 0.4 0 0 0 0</pose>
</include>
<include>
<uri>model://box1</uri>
<name>box5</name>
<pose>-0.1 0.4 0 0 0 0</pose>
</include>
<include>
<uri>model://box1</uri>
<name>box6</name>
<pose>-0.2 0.4 0 0 0 0</pose>
</include>
<light name='sun' type='directional'> <light name='sun' type='directional'>
<pose>0 0 10 0 -0 0</pose> <pose>0 0 10 0 -0 0</pose>
<cast_shadows>true</cast_shadows> <cast_shadows>true</cast_shadows>

243
rbs_simulation/worlds/mir.sdf Normal file
View file

@ -0,0 +1,243 @@
<?xml version="1.0"?>
<sdf version='1.9'>
<world name='mir'>
<physics name='1ms' type='ignored'>
<max_step_size>0.001</max_step_size>
<real_time_factor>1.0</real_time_factor>
<real_time_update_rate>1000</real_time_update_rate>
</physics>
<plugin name='ignition::gazebo::systems::Physics' filename='ignition-gazebo-physics-system'/>
<plugin name='ignition::gazebo::systems::UserCommands' filename='ignition-gazebo-user-commands-system'/>
<plugin name='ignition::gazebo::systems::SceneBroadcaster' filename='ignition-gazebo-scene-broadcaster-system'/>
<plugin name='ignition::gazebo::systems::Contact' filename='ignition-gazebo-contact-system'/>
<gravity>0 0 -9.8</gravity>
<magnetic_field>6e-06 2.3e-05 -4.2e-05</magnetic_field>
<atmosphere type='adiabatic'/>
<scene>
<ambient>0.4 0.4 0.4 1</ambient>
<background>0.7 0.7 0.7 1</background>
<shadows>false</shadows>
</scene>
<gui fullscreen="0">
<plugin filename="GzScene3D" name="3D View">
<ignition-gui>
<title>3D View</title>
<property type="bool" key="showTitleBar">false</property>
<property type="string" key="state">docked</property>
</ignition-gui>
<engine>ogre2</engine>
<scene>scene</scene>
<ambient_light>1.0 1.0 1.0</ambient_light>
<background_color>0.4 0.6 1.0</background_color>
<camera_pose>3.3 2.8 2.8 0 0.5 -2.4</camera_pose>
</plugin>
<plugin filename="WorldStats" name="World stats">
<ignition-gui>
<title>World stats</title>
<property type="bool" key="showTitleBar">false</property>
<property type="bool" key="resizable">false</property>
<property type="double" key="height">110</property>
<property type="double" key="width">290</property>
<property type="double" key="z">1</property>
<property type="string" key="state">floating</property>
<anchors target="3D View">
<line own="right" target="right"/>
<line own="bottom" target="bottom"/>
</anchors>
</ignition-gui>
<sim_time>true</sim_time>
<real_time>true</real_time>
<real_time_factor>true</real_time_factor>
<iterations>true</iterations>
</plugin>
</gui>
<light type="directional" name="sun">
<cast_shadows>true</cast_shadows>
<pose>0 0 10 0 0 0</pose>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.2 0.2 0.2 1</specular>
<attenuation>
<range>1000</range>
<constant>0.9</constant>
<linear>0.01</linear>
<quadratic>0.001</quadratic>
</attenuation>
<direction>-0.5 0.1 -0.9</direction>
</light>
<model name='ground'>
<static>true</static>
<link name="link">
<collision name="collision">
<geometry>
<plane>
<normal>0 0 1</normal>
</plane>
</geometry>
</collision>
<visual name="visual">
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
<material>
<ambient>0.8 0.8 0.8 1</ambient>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.8 0.8 0.8 1</specular>
</material>
</visual>
</link>
</model>
<model name='rack'>
<pose>0 0.7 0 0 0 0</pose>
<static>1</static>
<link name='body'>
<visual name='visual'>
<geometry>
<mesh>
<scale>0.35 0.35 0.35</scale>
<uri>model://rack/model/rack.dae</uri>
</mesh>
</geometry>
</visual>
<collision name='collision'>
<geometry>
<mesh>
<scale>0.35 0.35 0.35</scale>
<uri>model://rack/model/rack.stl</uri>
</mesh>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
</model>
<model name='table'>
<pose>-0.75 0 0.2 0 0 0</pose>
<static>1</static>
<link name='telo'>
<visual name='visual'>
<geometry>
<!-- <mesh>
<scale>1 1 1</scale>
<uri>model://table/model/table.dae</uri>
</mesh> -->
<box>
<size>0.8 0.8 0.4</size>
</box>
</geometry>
<material>
<ambient>0.8 0.8 0.8 1</ambient>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.8 0.8 0.8 1</specular>
</material>
</visual>
<collision name='collision'>
<geometry>
<!-- <mesh>
<scale>1 1 1</scale>
<uri>model://table/model/table.stl</uri>
</mesh> -->
<box>
<size>0.8 0.8 0.4</size>
</box>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
</model>
<include>
<uri>model://box1</uri>
<name>box1</name>
<pose>0.25 0.65 0.6515 0 0 0</pose>
</include>
<include>
<uri>model://box1</uri>
<name>box2</name>
<pose>0 0.65 0.6515 0 0 0</pose>
</include>
<include>
<uri>model://box1</uri>
<name>box3</name>
<pose>-0.25 0.65 0.6515 0 0 0</pose>
</include>
<include>
<uri>model://box1</uri>
<name>box4</name>
<pose>0.25 0.65 0.3015 0 0 0</pose>
</include>
<include>
<uri>model://box1</uri>
<name>box5</name>
<pose>0 0.65 0.3015 0 0 0</pose>
</include>
<include>
<uri>model://box1</uri>
<name>box6</name>
<pose>-0.25 0.65 0.3015 0 0 0</pose>
</include>
<!-- <include>
<uri>model://box1</uri>
<name>pyramid1</name>
<pose>-0.9000 -0.05 0.5250 0 0 0</pose>
</include>
<include>
<uri>model://box1</uri>
<name>pyramid2</name>
<pose>-0.9000 0.0 0.5250 0 0 0</pose>
</include>
<include>
<uri>model://box1</uri>
<name>pyramid3</name>
<pose>-0.9000 0.05 0.5250 0 0 0</pose>
</include>
<include>
<uri>model://box1</uri>
<name>pyramid4</name>
<pose>-0.9 -0.0243 0.5750 0 -0 0</pose>
</include>
<include>
<uri>model://box1</uri>
<name>pyramid5</name>
<pose>-0.9 0.0260 0.5750 0 -0 0</pose>
</include>
<include>
<uri>model://box1</uri>
<name>pyramid6</name>
<pose>-0.9 0 0.6250 0 0 0</pose>
</include> -->
</world>
</sdf>

493
rbs_simulation/worlds/rbs_world.sdf Normal file
View file

@ -0,0 +1,493 @@
<sdf version='1.9'>
<world name='empty'>
<physics name='1ms' type='ignored'>
<max_step_size>0.001</max_step_size>
<real_time_factor>1</real_time_factor>
<real_time_update_rate>1000</real_time_update_rate>
</physics>
<plugin name='ignition::gazebo::systems::Physics' filename='ignition-gazebo-physics-system'/>
<plugin name='ignition::gazebo::systems::UserCommands' filename='ignition-gazebo-user-commands-system'/>
<plugin name='ignition::gazebo::systems::SceneBroadcaster' filename='ignition-gazebo-scene-broadcaster-system'/>
<plugin name='ignition::gazebo::systems::Contact' filename='ignition-gazebo-contact-system'/>
<gravity>0 0 -9.8</gravity>
<magnetic_field>6e-06 2.3e-05 -4.2e-05</magnetic_field>
<atmosphere type='adiabatic'/>
<scene>
<ambient>0.4 0.4 0.4 1</ambient>
<background>0.7 0.7 0.7 1</background>
<shadows>true</shadows>
</scene>
<model name='ground_plane'>
<static>true</static>
<link name='link'>
<collision name='collision'>
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
<surface>
<friction>
<ode/>
</friction>
<bounce/>
<contact/>
</surface>
</collision>
<visual name='visual'>
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
<material>
<ambient>0.8 0.8 0.8 1</ambient>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.8 0.8 0.8 1</specular>
</material>
</visual>
<pose>0 0 0 0 -0 0</pose>
<inertial>
<pose>0 0 0 0 -0 0</pose>
<mass>1</mass>
<inertia>
<ixx>1</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>1</iyy>
<iyz>0</iyz>
<izz>1</izz>
</inertia>
</inertial>
<enable_wind>false</enable_wind>
</link>
<pose>0 0 0 0 -0 0</pose>
<self_collide>false</self_collide>
</model>
<model name='ur5e'>
<joint name='base_joint' type='fixed'>
<pose>0 0 0 0 -0 0</pose>
<parent>world</parent>
<child>base_link</child>
</joint>
<link name='base_link'>
<pose>0 0 0 0 -0 0</pose>
<inertial>
<pose>0 0 0 0 -0 0</pose>
<mass>4</mass>
<inertia>
<ixx>0.0044333300000000001</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.0044333300000000001</iyy>
<iyz>0</iyz>
<izz>0.0071999999999999998</izz>
</inertia>
</inertial>
<collision name='base_link_inertia_collision'>
<pose>0 0 0 0 0 -2.449293598294706e-16</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>file:///home/bill-finger/rasms_ws/install/share/ur_description/meshes/ur5e/collision/base.stl</uri>
</mesh>
</geometry>
</collision>
<visual name='base_link_inertia_visual'>
<pose>0 0 0 0 0 -2.449293598294706e-16</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>file:///home/bill-finger/rasms_ws/install/share/ur_description/meshes/ur5e/visual/base.dae</uri>
</mesh>
</geometry>
</visual>
<enable_wind>false</enable_wind>
</link>
<joint name='shoulder_pan_joint' type='revolute'>
<pose>0 0 0 0 -0 0</pose>
<parent>base_link</parent>
<child>shoulder_link</child>
<axis>
<xyz>0 0 1</xyz>
<limit>
<lower>-6.2831900000000003</lower>
<upper>6.2831900000000003</upper>
<effort>150</effort>
<velocity>3.1415899999999999</velocity>
<stiffness>100000000</stiffness>
<dissipation>1</dissipation>
</limit>
<dynamics>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
<damping>0</damping>
<friction>0</friction>
</dynamics>
</axis>
</joint>
<link name='shoulder_link'>
<pose>0 0 0.1625 0 -0 -3.14159</pose>
<inertial>
<pose>0 0 0 0 -0 0</pose>
<mass>3.7000000000000002</mass>
<inertia>
<ixx>0.010267500000000001</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.010267500000000001</iyy>
<iyz>0</iyz>
<izz>0.0066600000000000001</izz>
</inertia>
</inertial>
<collision name='shoulder_link_collision'>
<pose>0 0 0 0 0 3.141592653589793</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>file:///home/bill-finger/rasms_ws/install/share/ur_description/meshes/ur5e/collision/shoulder.stl</uri>
</mesh>
</geometry>
</collision>
<visual name='shoulder_link_visual'>
<pose>0 0 0 0 0 3.141592653589793</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>file:///home/bill-finger/rasms_ws/install/share/ur_description/meshes/ur5e/visual/shoulder.dae</uri>
</mesh>
</geometry>
</visual>
<enable_wind>false</enable_wind>
</link>
<joint name='shoulder_lift_joint' type='revolute'>
<pose>-0 0 0 0 -0 0</pose>
<parent>shoulder_link</parent>
<child>upper_arm_link</child>
<axis>
<xyz>0 0 1</xyz>
<limit>
<lower>-6.2831900000000003</lower>
<upper>6.2831900000000003</upper>
<effort>150</effort>
<velocity>3.1415899999999999</velocity>
<stiffness>100000000</stiffness>
<dissipation>1</dissipation>
</limit>
<dynamics>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
<damping>0</damping>
<friction>0</friction>
</dynamics>
</axis>
</joint>
<link name='upper_arm_link'>
<pose>0 -0 0.1625 1.5708 1.57 -3.14159</pose>
<inertial>
<pose>-0.2125 0 0.138 0 1.5708 0</pose>
<mass>8.3930000000000007</mass>
<inertia>
<ixx>0.13388600000000001</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.13388600000000001</iyy>
<iyz>0</iyz>
<izz>0.0151074</izz>
</inertia>
</inertial>
<collision name='upper_arm_link_collision'>
<pose>0 0 0.138 1.570796326794896 0 -1.570796326794896</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>file:///home/bill-finger/rasms_ws/install/share/ur_description/meshes/ur5e/collision/upperarm.stl</uri>
</mesh>
</geometry>
</collision>
<visual name='upper_arm_link_visual'>
<pose>0 0 0.138 1.570796326794896 0 -1.570796326794896</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>file:///home/bill-finger/rasms_ws/install/share/ur_description/meshes/ur5e/visual/upperarm.dae</uri>
</mesh>
</geometry>
</visual>
<enable_wind>false</enable_wind>
</link>
<joint name='elbow_joint' type='revolute'>
<pose>0 -0 0 0 -0 0</pose>
<parent>upper_arm_link</parent>
<child>forearm_link</child>
<axis>
<xyz>0 0 1</xyz>
<limit>
<lower>-3.1415899999999999</lower>
<upper>3.1415899999999999</upper>
<effort>150</effort>
<velocity>3.1415899999999999</velocity>
<stiffness>100000000</stiffness>
<dissipation>1</dissipation>
</limit>
<dynamics>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
<damping>0</damping>
<friction>0</friction>
</dynamics>
</axis>
</joint>
<link name='forearm_link'>
<pose>0.000338 0 0.5875 1.5708 1.57 -3.14159</pose>
<inertial>
<pose>-0.1961 0 0.007 0 1.5708 0</pose>
<mass>2.2749999999999999</mass>
<inertia>
<ixx>0.031209400000000002</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.031209400000000002</iyy>
<iyz>0</iyz>
<izz>0.0040949999999999997</izz>
</inertia>
</inertial>
<collision name='forearm_link_collision'>
<pose>0 0 0.007 1.570796326794896 0 -1.570796326794896</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>file:///home/bill-finger/rasms_ws/install/share/ur_description/meshes/ur5e/collision/forearm.stl</uri>
</mesh>
</geometry>
</collision>
<visual name='forearm_link_visual'>
<pose>0 0 0.007 1.570796326794896 0 -1.570796326794896</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>file:///home/bill-finger/rasms_ws/install/share/ur_description/meshes/ur5e/visual/forearm.dae</uri>
</mesh>
</geometry>
</visual>
<enable_wind>false</enable_wind>
</link>
<joint name='wrist_1_joint' type='revolute'>
<pose>0 0 0 0 -0 0</pose>
<parent>forearm_link</parent>
<child>wrist_1_link</child>
<axis>
<xyz>0 0 1</xyz>
<limit>
<lower>-6.2831900000000003</lower>
<upper>6.2831900000000003</upper>
<effort>28</effort>
<velocity>3.1415899999999999</velocity>
<stiffness>100000000</stiffness>
<dissipation>1</dissipation>
</limit>
<dynamics>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
<damping>0</damping>
<friction>0</friction>
</dynamics>
</axis>
</joint>
<link name='wrist_1_link'>
<pose>0.000651 0.1333 0.9797 -1.5708 0.001593 -0</pose>
<inertial>
<pose>0 0 0 0 -0 0</pose>
<mass>1.2190000000000001</mass>
<inertia>
<ixx>0.0025598999999999999</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.0025598999999999999</iyy>
<iyz>0</iyz>
<izz>0.0021941999999999999</izz>
</inertia>
</inertial>
<collision name='wrist_1_link_collision'>
<pose>0 0 -0.127 1.570796326794896 0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>file:///home/bill-finger/rasms_ws/install/share/ur_description/meshes/ur5e/collision/wrist1.stl</uri>
</mesh>
</geometry>
</collision>
<visual name='wrist_1_link_visual'>
<pose>0 0 -0.127 1.570796326794896 0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>file:///home/bill-finger/rasms_ws/install/share/ur_description/meshes/ur5e/visual/wrist1.dae</uri>
</mesh>
</geometry>
</visual>
<enable_wind>false</enable_wind>
</link>
<joint name='wrist_2_joint' type='revolute'>
<pose>0 0 0 0 0 -0</pose>
<parent>wrist_1_link</parent>
<child>wrist_2_link</child>
<axis>
<xyz>0 0 1</xyz>
<limit>
<lower>-6.2831900000000003</lower>
<upper>6.2831900000000003</upper>
<effort>28</effort>
<velocity>3.1415899999999999</velocity>
<stiffness>100000000</stiffness>
<dissipation>1</dissipation>
</limit>
<dynamics>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
<damping>0</damping>
<friction>0</friction>
</dynamics>
</axis>
</joint>
<link name='wrist_2_link'>
<pose>0.00081 0.1333 1.0794 -0 0.001593 -0</pose>
<inertial>
<pose>0 0 0 0 -0 0</pose>
<mass>1.2190000000000001</mass>
<inertia>
<ixx>0.0025598999999999999</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.0025598999999999999</iyy>
<iyz>0</iyz>
<izz>0.0021941999999999999</izz>
</inertia>
</inertial>
<collision name='wrist_2_link_collision'>
<pose>0 0 -0.0997 0 0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>file:///home/bill-finger/rasms_ws/install/share/ur_description/meshes/ur5e/collision/wrist2.stl</uri>
</mesh>
</geometry>
</collision>
<visual name='wrist_2_link_visual'>
<pose>0 0 -0.0997 0 0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>file:///home/bill-finger/rasms_ws/install/share/ur_description/meshes/ur5e/visual/wrist2.dae</uri>
</mesh>
</geometry>
</visual>
<enable_wind>false</enable_wind>
</link>
<joint name='wrist_3_joint' type='revolute'>
<pose>0 -0 -0 0 -0 0</pose>
<parent>wrist_2_link</parent>
<child>wrist_3_link</child>
<axis>
<xyz>0 0 1</xyz>
<limit>
<lower>-6.2831900000000003</lower>
<upper>6.2831900000000003</upper>
<effort>28</effort>
<velocity>3.1415899999999999</velocity>
<stiffness>100000000</stiffness>
<dissipation>1</dissipation>
</limit>
<dynamics>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
<damping>0</damping>
<friction>0</friction>
</dynamics>
</axis>
</joint>
<link name='wrist_3_link'>
<pose>0.00081 0.2329 1.0794 -1.5708 0.001593 -0</pose>
<inertial>
<pose>0 0 -0.0229 0 -0 0</pose>
<mass>0.18790000000000001</mass>
<inertia>
<ixx>9.8904099999999994e-05</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>9.8904099999999994e-05</iyy>
<iyz>0</iyz>
<izz>0.00013211700000000001</izz>
</inertia>
</inertial>
<collision name='wrist_3_link_collision'>
<pose>0 0 -0.0989 1.570796326794896 0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>file:///home/bill-finger/rasms_ws/install/share/ur_description/meshes/ur5e/collision/wrist3.stl</uri>
</mesh>
</geometry>
</collision>
<visual name='wrist_3_link_visual'>
<pose>0 0 -0.0989 1.570796326794896 0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>file:///home/bill-finger/rasms_ws/install/share/ur_description/meshes/ur5e/visual/wrist3.dae</uri>
</mesh>
</geometry>
</visual>
<enable_wind>false</enable_wind>
</link>
<plugin name='ign_ros2_control::IgnitionROS2ControlPlugin' filename='libign_ros2_control-system.so'>
<parameters>/home/bill-finger/rasms_ws/install/share/ur_moveit_config/config/ur_controllers.yaml</parameters>
<controller_manager_node_name>controller_manager</controller_manager_node_name>
</plugin>
<frame name='base_link-base_fixed_joint' attached_to='base_link'>
<pose>0 0 0 0 -0 3.14159</pose>
</frame>
<frame name='base' attached_to='base_link-base_fixed_joint'/>
<frame name='base_link-base_link_inertia' attached_to='base_link'>
<pose>0 0 0 0 -0 3.14159</pose>
</frame>
<frame name='base_link_inertia' attached_to='base_link-base_link_inertia'/>
<frame name='flange-tool0' attached_to='flange'>
<pose>0 0 0 1.5708 -0 1.5708</pose>
</frame>
<frame name='tool0' attached_to='flange-tool0'/>
<frame name='wrist_3-flange' attached_to='wrist_3_link'>
<pose>0 0 0 -1.5708 -1.5708 0</pose>
</frame>
<frame name='flange' attached_to='wrist_3-flange'/>
<frame name='wrist_3_link-ft_frame' attached_to='wrist_3_link'>
<pose>0 0 0 3.14159 -0 0</pose>
</frame>
<frame name='ft_frame' attached_to='wrist_3_link-ft_frame'/>
<pose>0 0 0 0 -0 0</pose>
<static>false</static>
<self_collide>false</self_collide>
</model>
<light name='sun' type='directional'>
<pose>0 0 10 0 -0 0</pose>
<cast_shadows>true</cast_shadows>
<intensity>1</intensity>
<direction>-0.5 0.1 -0.9</direction>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.2 0.2 0.2 1</specular>
<attenuation>
<range>1000</range>
<linear>0.01</linear>
<constant>0.90000000000000002</constant>
<quadratic>0.001</quadratic>
</attenuation>
<spot>
<inner_angle>0</inner_angle>
<outer_angle>0</outer_angle>
<falloff>0</falloff>
</spot>
</light>
</world>
</sdf>

6
rbs_skill_interfaces/CMakeLists.txt
View file

@ -9,6 +9,7 @@ endif()
find_package(ament_cmake REQUIRED) find_package(ament_cmake REQUIRED)
find_package(rosidl_default_generators REQUIRED) find_package(rosidl_default_generators REQUIRED)
find_package(geometry_msgs REQUIRED) find_package(geometry_msgs REQUIRED)
find_package(std_msgs REQUIRED)
# uncomment the following section in order to fill in # uncomment the following section in order to fill in
# further dependencies manually. # further dependencies manually.
# find_package(<dependency> REQUIRED) # find_package(<dependency> REQUIRED)
@ -20,8 +21,9 @@ rosidl_generate_interfaces(${PROJECT_NAME}
"msg/PropertyValuePair.msg" "msg/PropertyValuePair.msg"
"msg/ActionFeedbackStatusConstants.msg" "msg/ActionFeedbackStatusConstants.msg"
"msg/ActionResultStatusConstants.msg" "msg/ActionResultStatusConstants.msg"
"srv/BtInit.srv" "srv/GetPickPlacePoses.srv"
DEPENDENCIES geometry_msgs "srv/AddPlanningSceneObject.srv"
DEPENDENCIES geometry_msgs std_msgs
) )

4
rbs_skill_interfaces/action/GripperCommand.action
View file

@ -1,5 +1,5 @@
float64 value float32 position
string frame
--- ---
bool success bool success
--- ---
string result

2
rbs_skill_interfaces/action/MoveitSendJointStates.action
View file

@ -3,7 +3,7 @@
#goal definition #goal definition
string robot_name string robot_name
rbs_skill_interfaces/PropertyValuePair[] joint_pairs # list of joint_pairs (joint name + value) rbs_skill_interfaces/PropertyValuePair[] joint_pairs # list of joint_pairs (joint name + value)
float32 joint_value # send joint value to gripper float64[] joint_values
float32 joints_velocity_scaling_factor float32 joints_velocity_scaling_factor
float32 joints_acceleration_scaling_factor float32 joints_acceleration_scaling_factor
float32 timeout_seconds #if this action cannot be completed within this time period it should be considered failed. float32 timeout_seconds #if this action cannot be completed within this time period it should be considered failed.

4
rbs_skill_interfaces/srv/AddPlanningSceneObject.srv Normal file
View file

@ -0,0 +1,4 @@
float64[] object_pose
string object_id
---
bool success

12
rbs_skill_interfaces/srv/GetPickPlacePoses.srv Normal file
View file

@ -0,0 +1,12 @@
string object_name
string pose_name
---
float64[] pre_grasp_pose
float64[] grasp_pose
float64[] post_grasp_pose
float64[] post_grasp_pose_d
float64[] pre_place_pose
float64[] place_pose
float64[] post_place_pose
float64[] pre_place_joint_state
string next_object

38
rbs_skill_servers/CMakeLists.txt
View file

@ -41,41 +41,51 @@ set(deps
rclcpp_components rclcpp_components
rbs_skill_interfaces rbs_skill_interfaces
) )
#
# include_directories(include)
# install(DIRECTORY include
# DESTINATION include
# )
if(BUILD_TESTING) if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED) find_package(ament_lint_auto REQUIRED)
ament_lint_auto_find_test_dependencies() ament_lint_auto_find_test_dependencies()
endif() endif()
# add_library(gripper_cmd_action_server SHARED src/gripper_cmd.cpp) add_library(gripper_action_server SHARED src/gripper_control_action_server.cpp)
# ament_target_dependencies(gripper_cmd_action_server ${deps})
# target_include_directories(gripper_cmd_action_server PRIVATE target_include_directories(gripper_action_server PRIVATE
# $<BUILD_INTERFACE:${CMAKE_CURRENT_DIR}/include> $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
# $<INSTALL_INTERFACE:include>) $<INSTALL_INTERFACE:include>)
# target_compile_definitions(gripper_cmd_action_server PRIVATE "GRIPPER_CMD_ACTION_SERVER_CPP_BUILDING_DLL")
# rclcpp_components_register_node(gripper_cmd_action_server PLUGIN "robossembler_servers::GripperCmd" EXECUTABLE gripper_cmd_node) target_compile_definitions(gripper_action_server
PRIVATE "GRIPPER_ACTION_SERVER_CPP_BUILDING_DLL")
ament_target_dependencies(gripper_action_server ${deps})
rclcpp_components_register_node(gripper_action_server PLUGIN "rbs_skill_actions::GripperControlActionServer" EXECUTABLE gripper_control_action_server)
add_executable(move_to_joint_states_action_server src/move_to_joint_states_action_server.cpp) add_executable(move_to_joint_states_action_server src/move_to_joint_states_action_server.cpp)
ament_target_dependencies(move_to_joint_states_action_server ${deps}) ament_target_dependencies(move_to_joint_states_action_server ${deps})
add_executable(pick_place_pose_loader src/get_grasp_pick_pose.cpp)
ament_target_dependencies(pick_place_pose_loader ${deps})
add_executable(move_topose_action_server src/move_topose_action_server.cpp) add_executable(move_topose_action_server src/move_topose_action_server.cpp)
ament_target_dependencies(move_topose_action_server ${deps}) ament_target_dependencies(move_topose_action_server ${deps})
add_executable(move_cartesian_path_action_server src/move_cartesian_path_action_server.cpp) add_executable(move_cartesian_path_action_server src/move_cartesian_path_action_server.cpp)
ament_target_dependencies(move_cartesian_path_action_server ${deps}) ament_target_dependencies(move_cartesian_path_action_server ${deps})
add_executable(add_planning_scene_object_service src/add_planning_scene_objects_service.cpp)
ament_target_dependencies(add_planning_scene_object_service ${deps})
install( install(
TARGETS TARGETS
move_topose_action_server move_topose_action_server
gripper_action_server
pick_place_pose_loader
move_to_joint_states_action_server move_to_joint_states_action_server
move_cartesian_path_action_server move_cartesian_path_action_server
#gripper_cmd_action_server add_planning_scene_object_service
ARCHIVE DESTINATION lib ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib LIBRARY DESTINATION lib
RUNTIME DESTINATION lib/${PROJECT_NAME} RUNTIME DESTINATION lib/${PROJECT_NAME}

71
rbs_skill_servers/src/add_planning_scene_objects_service.cpp Normal file
View file

@ -0,0 +1,71 @@
#include <cinttypes>
#include <memory>
#include <algorithm>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include "rbs_skill_interfaces/srv/add_planning_scene_object.hpp"
#include "moveit/move_group_interface/move_group_interface.h"
#include "rclcpp/rclcpp.hpp"
using AddPlanningSceneObject = rbs_skill_interfaces::srv::AddPlanningSceneObject;
rclcpp::Node::SharedPtr g_node = nullptr;
void handle_service(
const std::shared_ptr<rmw_request_id_t> request_header,
const std::shared_ptr<AddPlanningSceneObject::Request> request,
const std::shared_ptr<AddPlanningSceneObject::Response> response)
{
(void)request_header;
// Create collision object for the robot to avoid
auto const collision_object = [frame_id =
"world", object_name=request->object_id, object_pose=request->object_pose] {
moveit_msgs::msg::CollisionObject collision_object;
collision_object.header.frame_id = frame_id;
collision_object.id = object_name;
shape_msgs::msg::SolidPrimitive primitive;
// Define the size of the box in meters
primitive.type = primitive.BOX;
primitive.dimensions.resize(3);
primitive.dimensions[primitive.BOX_X] = 0.05;
primitive.dimensions[primitive.BOX_Y] = 0.05;
primitive.dimensions[primitive.BOX_Z] = 0.05;
// Define the pose of the box (relative to the frame_id)
geometry_msgs::msg::Pose box_pose;
box_pose.position.x = object_pose[0];
box_pose.position.y = object_pose[1];
box_pose.position.z = object_pose[2];
box_pose.orientation.x = object_pose[3];
box_pose.orientation.y = object_pose[4];
box_pose.orientation.z = object_pose[5];
box_pose.orientation.w = object_pose[6];
collision_object.primitives.push_back(primitive);
collision_object.primitive_poses.push_back(box_pose);
collision_object.operation = collision_object.ADD;
return collision_object;
}();
// Add the collision object to the scene
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
planning_scene_interface.applyCollisionObject(collision_object);
response->success = true;
}
int main(int argc, char ** argv)
{
rclcpp::init(argc, argv);
rclcpp::NodeOptions node_options;
node_options.allow_undeclared_parameters(true).automatically_declare_parameters_from_overrides(true);
g_node = rclcpp::Node::make_shared("add_planing_scene_object", "", node_options);
auto server = g_node->create_service<AddPlanningSceneObject>("add_planing_scene_object_service", handle_service);
rclcpp::spin(g_node);
rclcpp::shutdown();
g_node = nullptr;
return 0;
}

71
rbs_skill_servers/src/get_grasp_pick_pose.cpp Normal file
View file

@ -0,0 +1,71 @@
#include <cinttypes>
#include <memory>
#include <algorithm>
#include "rbs_skill_interfaces/srv/get_pick_place_poses.hpp"
#include "rclcpp/rclcpp.hpp"
using GetGraspPlacePose = rbs_skill_interfaces::srv::GetPickPlacePoses;
rclcpp::Node::SharedPtr g_node = nullptr;
std::string pick_next_object(std::vector<std::string> scene_objects, std::string current_object_name);
void handle_service(
const std::shared_ptr<rmw_request_id_t> request_header,
const std::shared_ptr<GetGraspPlacePose::Request> request,
const std::shared_ptr<GetGraspPlacePose::Response> response)
{
(void)request_header;
RCLCPP_INFO(
g_node->get_logger(),"GetPoseCallback");
std::vector<std::string> param_names = {
request->object_name + ".GraspPose",
request->object_name + ".PreGraspPose",
request->object_name + ".PostGraspPose",
request->object_name + ".PlacePose",
request->object_name + ".PrePlacePose",
request->object_name + ".PostPlacePose",
request->object_name + ".PostGraspPose2",
"pre_place_joint_states",
"scene_objects"
};
std::vector<rclcpp::Parameter> params = g_node->get_parameters(param_names);
for (auto &param : params)
{
RCLCPP_INFO(g_node->get_logger(), "param name: %s, value: %s",
param.get_name().c_str(), param.value_to_string().c_str());
}
response->grasp_pose = params[0].as_double_array();
response->pre_grasp_pose = params[1].as_double_array();
response->post_grasp_pose = params[2].as_double_array();
response->place_pose = params[3].as_double_array();
response->pre_place_pose = params[4].as_double_array();
response->post_place_pose = params[5].as_double_array();
response->post_grasp_pose_d = params[6].as_double_array();
response->pre_place_joint_state = params[7].as_double_array();
response->next_object = pick_next_object(params[8].as_string_array(), request->object_name);
}
std::string pick_next_object(std::vector<std::string> scene_objects, std::string current_object_name)
{
std::string next_object_name{};
auto current_object_idx = std::find(scene_objects.begin(), scene_objects.end(), current_object_name);
if (!(current_object_idx == scene_objects.end()))
{
next_object_name = *std::next(current_object_idx, 1);
} else {
next_object_name = "It is last object";
}
return next_object_name;
}
int main(int argc, char ** argv)
{
rclcpp::init(argc, argv);
rclcpp::NodeOptions node_options;
node_options.allow_undeclared_parameters(true).automatically_declare_parameters_from_overrides(true);
g_node = rclcpp::Node::make_shared("get_grasp_pick_pose", "", node_options);
auto server = g_node->create_service<GetGraspPlacePose>("get_pick_place_pose_service", handle_service);
rclcpp::spin(g_node);
rclcpp::shutdown();
g_node = nullptr;
return 0;
}

114
rbs_skill_servers/src/gripper_control_action_server.cpp Normal file
View file

@ -0,0 +1,114 @@
#include <functional>
#include <memory>
#include <thread>
#include "rclcpp/rclcpp.hpp"
#include "rclcpp/timer.hpp"
#include "rclcpp_components/register_node_macro.hpp"
// action libs
#include "rclcpp_action/rclcpp_action.hpp"
#include "rbs_skill_interfaces/action/gripper_command.hpp"
#include "std_msgs/msg/float64_multi_array.hpp"
#include "sensor_msgs/msg/joint_state.hpp"
namespace rbs_skill_actions {
class GripperControlActionServer: public rclcpp::Node {
public:
using GripperCommand = rbs_skill_interfaces::action::GripperCommand;
explicit GripperControlActionServer(rclcpp::NodeOptions options): Node("gripper_control_action_server", options.allow_undeclared_parameters(true))
{
this->actionServer_ = rclcpp_action::create_server<GripperCommand>(
this,
"gripper_control",
std::bind(&GripperControlActionServer::goal_callback, this, std::placeholders::_1, std::placeholders::_2),
std::bind(&GripperControlActionServer::cancel_callback, this, std::placeholders::_1),
std::bind(&GripperControlActionServer::accepted_callback, this, std::placeholders::_1)
);
publisher = this->create_publisher<std_msgs::msg::Float64MultiArray>("/gripper_controller/commands", 10);
join_state_subscriber = this->create_subscription<sensor_msgs::msg::JointState>(
"/joint_states",10, std::bind(&GripperControlActionServer::joint_state_callback, this, std::placeholders::_1));
}
private:
rclcpp_action::Server<GripperCommand>::SharedPtr actionServer_;
rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr publisher;
rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr join_state_subscriber;
double gripper_joint_state{1.0};
using ServerGoalHandle = rclcpp_action::ServerGoalHandle<GripperCommand>;
void joint_state_callback(const sensor_msgs::msg::JointState & msg)
{
gripper_joint_state = msg.position.back();
}
rclcpp_action::GoalResponse goal_callback(const rclcpp_action::GoalUUID& uuid, std::shared_ptr<const GripperCommand::Goal> goal) {
RCLCPP_INFO(this->get_logger(),"goal request recieved for gripper [%.2f m]", goal->position);
(void)uuid;
if(goal->position > 0.9 or goal->position < 0) {
return rclcpp_action::GoalResponse::REJECT;
}
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}
rclcpp_action::CancelResponse cancel_callback(const std::shared_ptr<ServerGoalHandle> goal_handle) {
RCLCPP_INFO(this->get_logger(), "Received request to cancel goal");
(void)goal_handle;
return rclcpp_action::CancelResponse::ACCEPT;
}
void accepted_callback(const std::shared_ptr<ServerGoalHandle> goal_handle) {
std::thread{std::bind(&GripperControlActionServer::execute, this, std::placeholders::_1), goal_handle}.detach();
}
void execute(const std::shared_ptr<ServerGoalHandle> goal_handle){
const auto goal = goal_handle->get_goal();
auto result = std::make_shared<GripperCommand::Result>();
auto feedback = std::make_shared<GripperCommand::Feedback>();
if (goal_handle->is_canceling()) {
goal_handle->canceled(result);
RCLCPP_ERROR(this->get_logger(), "Goal Canceled");
return;
}
RCLCPP_INFO(this->get_logger(), "Current gripper state %f", gripper_joint_state);
std_msgs::msg::Float64MultiArray command;
using namespace std::chrono_literals;
command.data.push_back(goal->position);
RCLCPP_INFO(this->get_logger(),"Publishing goal to gripper");
publisher->publish(command);
std::this_thread::sleep_for(3s);
goal_handle->succeed(result);
RCLCPP_INFO(this->get_logger(), "Goal successfully completed");
}
};
}
RCLCPP_COMPONENTS_REGISTER_NODE(rbs_skill_actions::GripperControlActionServer);
// int main(int argc, char ** argv) {
// rclcpp::init(argc, argv);
// rbs_skill_actions::GripperControlActionServer server;
// rclcpp::spin(server);
// return 0;
// }

44
rbs_skill_servers/src/move_cartesian_path_action_server.cpp
View file

@ -86,9 +86,7 @@ private:
goal->target_pose.orientation.x, goal->target_pose.orientation.y, goal->target_pose.orientation.z, goal->target_pose.orientation.x, goal->target_pose.orientation.y, goal->target_pose.orientation.z,
goal->target_pose.orientation.w); goal->target_pose.orientation.w);
(void)uuid; (void)uuid;
if (false) {
return rclcpp_action::GoalResponse::REJECT;
}
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
} }
@ -103,6 +101,10 @@ private:
{ {
using namespace std::placeholders; using namespace std::placeholders;
std::thread(std::bind(&MoveCartesianActionServer::execute, this, _1), goal_handle).detach(); std::thread(std::bind(&MoveCartesianActionServer::execute, this, _1), goal_handle).detach();
// std::thread(
// [this, goal_handle]() {
// execute(goal_handle);
// }).detach();
} }
void execute(const std::shared_ptr<ServerGoalHandle> goal_handle) void execute(const std::shared_ptr<ServerGoalHandle> goal_handle)
@ -112,22 +114,15 @@ private:
auto result = std::make_shared<MoveitSendPose::Result>(); auto result = std::make_shared<MoveitSendPose::Result>();
moveit::planning_interface::MoveGroupInterface move_group_interface(node_, goal->robot_name); moveit::planning_interface::MoveGroupInterface move_group_interface(node_, goal->robot_name);
move_group_interface.startStateMonitor();
moveit::core::RobotState start_state(*move_group_interface.getCurrentState()); moveit::core::RobotState start_state(*move_group_interface.getCurrentState());
auto start_pose = move_group_interface.getCurrentPose();
std::vector<geometry_msgs::msg::Pose> waypoints; std::vector<geometry_msgs::msg::Pose> waypoints;
RCLCPP_INFO(this->get_logger(), "Current with Pose [%f, %f, %f, %f, %f, %f, %f]", auto current_pose = move_group_interface.getCurrentPose();
start_pose.pose.position.x, start_pose.pose.position.y, start_pose.pose.position.z, //waypoints.push_back(current_pose.pose);
start_pose.pose.orientation.x, start_pose.pose.orientation.y, start_pose.pose.orientation.z, geometry_msgs::msg::Pose target_pose = goal->target_pose;
start_pose.pose.orientation.w); //target_pose.position = goal->target_pose.position;
//waypoints.push_back(start_pose.pose);
geometry_msgs::msg::Pose target_pose = start_pose.pose;
//goal->target_pose
target_pose.position.z = target_pose.position.z + goal->target_pose.position.z;
target_pose.position.y = target_pose.position.y + goal->target_pose.position.y;
target_pose.position.x = target_pose.position.x + goal->target_pose.position.x;
waypoints.push_back(target_pose); waypoints.push_back(target_pose);
RCLCPP_INFO(this->get_logger(), "New cartesian target pose [%f, %f, %f]", RCLCPP_INFO(this->get_logger(), "New cartesian target pose [%f, %f, %f]",
@ -135,16 +130,25 @@ private:
//waypoints.push_back(start_pose.pose); //waypoints.push_back(start_pose.pose);
moveit_msgs::msg::RobotTrajectory trajectory; moveit_msgs::msg::RobotTrajectory trajectory;
const double jump_threshold = 0.0; const double jump_threshold = 0.0;
const double eef_step = 0.01; const double eef_step = 0.001;
double fraction = move_group_interface.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory); double fraction = move_group_interface.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory);
waypoints.clear();
if(fraction>0) if(fraction>0)
{ {
RCLCPP_INFO(this->get_logger(), "Planning success"); RCLCPP_INFO(this->get_logger(), "Planning success");
move_group_interface.execute(trajectory); moveit::core::MoveItErrorCode execute_err_code = move_group_interface.execute(trajectory);
if (execute_err_code == moveit::core::MoveItErrorCode::SUCCESS)
{
goal_handle->succeed(result);
RCLCPP_INFO(this->get_logger(), "Successfully executed action goal");
}else if (execute_err_code == moveit::core::MoveItErrorCode::FAILURE)
{
RCLCPP_ERROR(this->get_logger(), "Failure in move:");
}
//move_group_interface.move(); //move_group_interface.move();
}else{ }else{
RCLCPP_WARN(this->get_logger(), "Failed to generate plan"); RCLCPP_WARN(this->get_logger(), "Failed to generate plan");
goal_handle->abort(result);
} }
if (goal_handle->is_canceling()) { if (goal_handle->is_canceling()) {
@ -152,10 +156,6 @@ private:
RCLCPP_ERROR(this->get_logger(), "Action goal canceled"); RCLCPP_ERROR(this->get_logger(), "Action goal canceled");
return; return;
} }
goal_handle->succeed(result);
RCLCPP_INFO(this->get_logger(), "Successfully executed action goal");
} }
}; // class MoveCartesianActionServer }; // class MoveCartesianActionServer

23
rbs_skill_servers/src/move_to_joint_states_action_server.cpp
View file

@ -1,6 +1,8 @@
#include <functional> #include <functional>
#include <memory> #include <memory>
#include <thread> #include <thread>
#include <cinttypes>
#include "rclcpp/rclcpp.hpp" #include "rclcpp/rclcpp.hpp"
#include "rclcpp/timer.hpp" #include "rclcpp/timer.hpp"
@ -107,17 +109,16 @@ private:
auto result = std::make_shared<MoveitSendJointStates::Result>(); auto result = std::make_shared<MoveitSendJointStates::Result>();
moveit::planning_interface::MoveGroupInterface move_group_interface(node_, goal->robot_name); moveit::planning_interface::MoveGroupInterface move_group_interface(node_, goal->robot_name);
move_group_interface.startStateMonitor();
const moveit::core::JointModelGroup* joint_model_group = std::vector<double> joint_states = goal->joint_values;
move_group_interface.getCurrentState()->getJointModelGroup(goal->robot_name); for (auto &joint : joint_states)
moveit::core::RobotStatePtr current_state = move_group_interface.getCurrentState(10); {
RCLCPP_INFO(this->get_logger(), "Joint value %f", joint);
std::vector<double> joint_group_positions; }
current_state->copyJointGroupPositions(joint_model_group, joint_group_positions);
joint_group_positions[0] = goal->joint_value; move_group_interface.setJointValueTarget(goal->joint_values);
joint_group_positions[1] = goal->joint_value;
move_group_interface.setJointValueTarget(joint_group_positions);
move_group_interface.setMaxVelocityScalingFactor(goal->joints_velocity_scaling_factor); move_group_interface.setMaxVelocityScalingFactor(goal->joints_velocity_scaling_factor);
move_group_interface.setMaxAccelerationScalingFactor(goal->joints_acceleration_scaling_factor); move_group_interface.setMaxAccelerationScalingFactor(goal->joints_acceleration_scaling_factor);
@ -128,7 +129,7 @@ private:
{ {
RCLCPP_INFO(this->get_logger(), "Planning success"); RCLCPP_INFO(this->get_logger(), "Planning success");
move_group_interface.execute(plan); move_group_interface.execute(plan);
move_group_interface.move(); //move_group_interface.move();
}else{ }else{
RCLCPP_WARN(this->get_logger(), "Failed to generate plan"); RCLCPP_WARN(this->get_logger(), "Failed to generate plan");
} }

33
rbs_skill_servers/src/move_topose_action_server.cpp
View file

@ -19,6 +19,7 @@
#include "moveit/move_group_interface/move_group_interface.h" #include "moveit/move_group_interface/move_group_interface.h"
#include "moveit/planning_interface/planning_interface.h" #include "moveit/planning_interface/planning_interface.h"
#include "moveit/robot_model_loader/robot_model_loader.h" #include "moveit/robot_model_loader/robot_model_loader.h"
#include "moveit/planning_scene_interface/planning_scene_interface.h"
/* /*
#include <tf2/LinearMath/Quaternion.h> #include <tf2/LinearMath/Quaternion.h>
@ -27,6 +28,7 @@
*/ */
// For Visualization // For Visualization
//#include <eigen_conversions/eigen_msg.h> //#include <eigen_conversions/eigen_msg.h>
#include <moveit_msgs/msg/display_robot_state.hpp>
#include "moveit_msgs/msg/display_robot_state.hpp" #include "moveit_msgs/msg/display_robot_state.hpp"
#include "moveit_msgs/msg/display_trajectory.hpp" #include "moveit_msgs/msg/display_trajectory.hpp"
#include "moveit_msgs/msg/robot_trajectory.hpp" #include "moveit_msgs/msg/robot_trajectory.hpp"
@ -112,21 +114,34 @@ private:
auto result = std::make_shared<MoveitSendPose::Result>(); auto result = std::make_shared<MoveitSendPose::Result>();
moveit::planning_interface::MoveGroupInterface move_group_interface(node_, goal->robot_name); moveit::planning_interface::MoveGroupInterface move_group_interface(node_, goal->robot_name);
move_group_interface.startStateMonitor();
move_group_interface.setStartState(*move_group_interface.getCurrentState());
move_group_interface.setPoseTarget(goal->target_pose); move_group_interface.setPoseTarget(goal->target_pose);
move_group_interface.setMaxVelocityScalingFactor(goal->end_effector_velocity); move_group_interface.setMaxVelocityScalingFactor(goal->end_effector_velocity);
move_group_interface.setMaxAccelerationScalingFactor(goal->end_effector_acceleration); move_group_interface.setMaxAccelerationScalingFactor(goal->end_effector_acceleration);
moveit::planning_interface::MoveGroupInterface::Plan plan; moveit::planning_interface::MoveGroupInterface::Plan plan;
bool success = (move_group_interface.plan(plan) == moveit::core::MoveItErrorCode::SUCCESS); moveit::core::MoveItErrorCode plan_err_code = move_group_interface.plan(plan);
if(success) if (plan_err_code == moveit::core::MoveItErrorCode::INVALID_MOTION_PLAN){
move_group_interface.plan(plan);
}
if(plan_err_code == moveit::core::MoveItErrorCode::SUCCESS)
{ {
RCLCPP_INFO(this->get_logger(), "Planning success"); RCLCPP_INFO(this->get_logger(), "Planning success");
//move_group_interface.execute(plan); //move_group_interface.execute(plan);
move_group_interface.move(); moveit::core::MoveItErrorCode move_err_code = move_group_interface.execute(plan);
if (move_err_code == moveit::core::MoveItErrorCode::SUCCESS)
{
goal_handle->succeed(result);
RCLCPP_INFO(this->get_logger(), "Successfully executed action goal");
} else if (move_err_code == moveit::core::MoveItErrorCode::FAILURE)
{
RCLCPP_ERROR(this->get_logger(), "Failure in move:");
}
}else{ }else{
RCLCPP_WARN(this->get_logger(), "Failed to generate plan"); RCLCPP_WARN_STREAM(this->get_logger(), "Failed to generate plan because " << error_code_to_string(plan_err_code));
goal_handle->abort(result);
} }
if (goal_handle->is_canceling()) { if (goal_handle->is_canceling()) {
@ -134,10 +149,6 @@ private:
RCLCPP_ERROR(this->get_logger(), "Action goal canceled"); RCLCPP_ERROR(this->get_logger(), "Action goal canceled");
return; return;
} }
goal_handle->succeed(result);
RCLCPP_INFO(this->get_logger(), "Successfully executed action goal");
} }
}; // class MoveToPoseActionServer }; // class MoveToPoseActionServer

0
robonomics/COLCON_IGNORE Normal file
View file

34
sdf_models/CMakeLists.txt
View file

@ -1,34 +0,0 @@
cmake_minimum_required(VERSION 3.5)
project(sdf_models)
# Default to C99
if(NOT CMAKE_C_STANDARD)
set(CMAKE_C_STANDARD 99)
endif()
# Default to C++17
if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 17)
endif()
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
# find dependencies
find_package(ament_cmake REQUIRED)
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
# the following line skips the linter which checks for copyrights
# uncomment the line when a copyright and license is not present in all source files
#set(ament_cmake_copyright_FOUND TRUE)
# the following line skips cpplint (only works in a git repo)
# uncomment the line when this package is not in a git repo
#set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
endif()
install(DIRECTORY models worlds DESTINATION share/${PROJECT_NAME})
ament_package()

29
sdf_models/models/box1/model.sdf
View file

@ -1,29 +0,0 @@
<?xml version="1.0" ?>
<sdf version="1.4">
<model name="box1">
<pose>0 0 0.015 0 0 0</pose>
<link name="link">
<gravity>0</gravity>
<collision name="collision">
<geometry>
<box>
<size>0.03 0.03 0.03</size>
</box>
</geometry>
</collision>
<visual name="visual">
<geometry>
<box>
<size>0.03 0.03 0.03</size>
</box>
</geometry>
<material>
<ambient>0.5 0.75 0 1</ambient>
<diffuse>0.7 0.9 0 1</diffuse>
<specular>0.2 0.2 0.2 64</specular>
<emissive>0.1 0 0.1 1</emissive>
</material>
</visual>
</link>
</model>
</sdf>

18
sdf_models/package.xml
View file

@ -1,18 +0,0 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>sdf_models</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="rom.andrianov1984@gmail.com">Splinter1984</maintainer>
<license>TODO: License declaration</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>