diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml
index 3cbd117..3ae66d8 100644
--- a/.gitlab-ci.yml
+++ b/.gitlab-ci.yml
@@ -1,4 +1,4 @@
-image: ros:foxy-ros-base
+image: ros:humble-ros-base
workflow:
rules:
@@ -17,8 +17,8 @@ build-colcon-job:
- mv * .src/robossembler-ros2
- mv .git .src/robossembler-ros2
- mv .src src
- - vcs import src < src/robossembler-ros2/rasms.repos
- - rosdep install -y -r -q --from-paths src --ignore-src --rosdistro foxy
- - colcon build --merge-install --symlink-install --cmake-args '-DCMAKE_BUILD_TYPE=Release' -Wall -Wextra -Wpedantic
+ - vcs import src < src/robossembler-ros2/rbs.repos
+ - rosdep install -y -r -q --from-paths src --ignore-src --rosdistro humble
+ - colcon build --merge-install --cmake-args '-DCMAKE_BUILD_TYPE=Release' -Wall -Wextra -Wpedantic
rules:
- if: $CI_COMMIT_REF_NAME != $CI_DEFAULT_BRANCH
diff --git a/README.md b/README.md
index 91ef89a..4fc6f31 100644
--- a/README.md
+++ b/README.md
@@ -4,77 +4,55 @@ Repo for ROS2 packages related to Robossembler
## Instructions
### Requirements
-* OS: Ubuntu 20.04
-* Other distributions might work (not tested).
+* OS: Ubuntu 22.04
+* ROS 2 Humble
### Dependencies
These are the primary dependencies required to use this project.
-* ROS 2 Foxy
* MoveIt 2
> Install/build a version based on the selected ROS 2 release
-* Gazebo
+* Gazebo Fortress
### Build
1. Clone the repository
-2. Build packages
+2. Build packages `colcon build`
-For visualization install ```colcon``` with ```mixin``` (it's required to install ```moveit_visual_tools```):
-```bash
-sudo apt install python3-colcon-common-extensions python3-colcon-mixin python3-vcstool
-colcon mixin add default https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml
-colcon mixin update default
-```
-Prepare workspace & install dependencies
+Prepare workspace & install dependencies (So far only tested with UR robot arm)
```
mkdir -p ~/robossembler_ws/src && cd ~/robossembler_ws/src
git clone https://gitlab.com/robosphere/robossembler-ros2
-vcs import . < robossembler-ros2/rasms.repos
+vcs import . < robossembler-ros2/rbs.repos
cd ..
-rosdep install -y -r -q --from-paths src --ignore-src --rosdistro foxy
-colcon build --symlink-install --mixin release
+rosdep install -y -r -q --from-paths src --ignore-src --rosdistro humble
+colcon build
```
-but at the moment the visualization is not implemented
-
### Examples
Add source to environment
```
-source install/setup.bash
-```
-Add robot model to Gazebo models directory
-
-```
-cp ~/robossembler_ws/src/robossembler-ros2/rasmt_support/ ~/.gazebo/models/
+. install/setup.bash
```
-Launch MoveIt2, Gazebo, RViz and PlanSys2 with domain from `pddl/domain.pddl`
+Launch MoveIt2, Gazebo, RViz
```bash
-ros2 launch robossembler robossembler_bringup.launch.py
+ros2 launch rbs_simulation rbs_simulation.launch.py
```
-#### With PlanSys2 Terminal
+#### Launch bt_tree
+
+It will execute `bt_tree` once from file `rbs_bt_executor/bt_trees/test_tree.xml`
-Launch Plansys2 Terminal
-```bash
-ros2 run plansys2_terminal plansys2_terminal
```
-Then into plansys2_terminal paste command (see updates into pddl/commands)
-```bash
-set instance rasmt_arm_group robot
-set instance one zone
-set goal (and (robot_move rasmt_arm_group one))
-run
+ros2 launch rbs_executor rbs_executor.launch.py
```
-#### With C++ node
-
-```bash
-ros2 run robossembler move_controller_node
+The robot arm should move to the point from config file in path
+```
+rbs_bt_executor/config/points.yaml
```
### Links
-* [plansys2_bt](https://intelligentroboticslab.gsyc.urjc.es/ros2_planning_system.github.io/tutorials/docs/bt_actions.html)
* [bt_v3_cpp](https://www.behaviortree.dev)
-* [moveit2](https://moveit.picknik.ai/foxy/index.html)
\ No newline at end of file
+* [moveit2](https://moveit.picknik.ai/humble/index.html)
\ No newline at end of file
diff --git a/rasms.repos b/rasms.repos
deleted file mode 100644
index a799541..0000000
--- a/rasms.repos
+++ /dev/null
@@ -1,21 +0,0 @@
-repositories:
- rviz_visual_tools:
- type: git
- url: https://github.com/PickNikRobotics/rviz_visual_tools
- version: foxy-devel
- moveit_visual_tools:
- type: git
- url: https://github.com/ros-planning/moveit_visual_tools
- version: foxy
- plansys2:
- type: git
- url: https://github.com/IntelligentRoboticsLabs/ros2_planning_system
- version: master
- behavior_tree:
- type: git
- url: https://github.com/berkeleyauv/behavior_tree
- version: master
- gazebo_ros_link_attacher:
- type: git
- url: https://github.com/davidorchansky/gazebo_ros_link_attacher.git
- version: foxy-devel
diff --git a/rasmt_moveit_config/config/joint_limits.yaml b/rasmt_moveit_config/config/joint_limits.yaml
index 111f79e..4ddfe45 100644
--- a/rasmt_moveit_config/config/joint_limits.yaml
+++ b/rasmt_moveit_config/config/joint_limits.yaml
@@ -39,6 +39,11 @@ joint_limits:
has_acceleration_limits: false
max_acceleration: 0
rasmt_Slide_1:
+ has_velocity_limits: true
+ max_velocity: 0.2
+ has_acceleration_limits: false
+ max_acceleration: 0
+ rasmt_Slide_2:
has_velocity_limits: true
max_velocity: 0.2
has_acceleration_limits: false
diff --git a/rasmt_moveit_config/config/rasmt_moveit_controllers.yaml b/rasmt_moveit_config/config/rasmt_moveit_controllers.yaml
index 50bb1ba..afe920c 100644
--- a/rasmt_moveit_config/config/rasmt_moveit_controllers.yaml
+++ b/rasmt_moveit_config/config/rasmt_moveit_controllers.yaml
@@ -1,6 +1,6 @@
controller_names:
- rasmt_arm_controller
-# - rasmt_hand_controller
+ - rasmt_hand_controller
rasmt_arm_controller:
action_ns: follow_joint_trajectory
@@ -14,10 +14,10 @@ rasmt_arm_controller:
- rasmt_Rot_Z_3
- rasmt_Rot_Y_4
-#rasmt_hand_controller:
-# action_ns: follow_joint_trajectory
-# type: FollowJointTrajectory
-# default: true
-# joints:
-# - rasmt_Slide_1
-# - rasmt_Slide_2
\ No newline at end of file
+rasmt_hand_controller:
+ action_ns: follow_joint_trajectory
+ type: FollowJointTrajectory
+ default: true
+ joints:
+ - rasmt_Slide_1
+ - rasmt_Slide_2
\ No newline at end of file
diff --git a/rasmt_moveit_config/launch/rasmt_moveit.launch.py b/rasmt_moveit_config/launch/rasmt_moveit.launch.py
index ae52ea4..c4c789c 100644
--- a/rasmt_moveit_config/launch/rasmt_moveit.launch.py
+++ b/rasmt_moveit_config/launch/rasmt_moveit.launch.py
@@ -97,8 +97,12 @@ def generate_launch_description():
}
# Trajectory execution
- trajectory_execution = {"allow_trajectory_execution": True,
- "moveit_manage_controllers": True}
+ trajectory_execution = {
+ "moveit_manage_controllers": True,
+ "trajectory_execution.allowed_execution_duration_scaling": 1.2,
+ "trajectory_execution.allowed_goal_duration_margin": 0.5,
+ "trajectory_execution.allowed_start_tolerance": 0.01,
+ }
# Controllers
controllers_yaml = load_yaml(
@@ -156,47 +160,47 @@ def generate_launch_description():
]
)
- move_topose_action_server = Node(
- package="robossembler_servers",
- executable="move_topose_action_server",
- name="move_to_pose_moveit",
- parameters=[
- robot_description,
- robot_description_semantic,
- kinematics_yaml,
- robot_description_planning,
- ompl_yaml,
- planning,
- trajectory_execution,
- moveit_controllers,
- planning_scene_monitor_parameters,
- use_sim_time
- ]
- )
+ # move_topose_action_server = Node(
+ # package="robossembler_servers",
+ # executable="move_topose_action_server",
+ # name="move_to_pose_moveit",
+ # parameters=[
+ # robot_description,
+ # robot_description_semantic,
+ # kinematics_yaml,
+ # robot_description_planning,
+ # ompl_yaml,
+ # planning,
+ # trajectory_execution,
+ # moveit_controllers,
+ # planning_scene_monitor_parameters,
+ # use_sim_time
+ # ]
+ # )
- move_cartesian_path_action_server = Node(
- package="robossembler_servers",
- executable="move_cartesian_path_action_server",
- name="move_cartesian_path_action_server",
- parameters=[
- robot_description,
- robot_description_semantic,
- kinematics_yaml,
- robot_description_planning,
- ompl_yaml,
- planning,
- trajectory_execution,
- moveit_controllers,
- planning_scene_monitor_parameters,
- use_sim_time
- ]
- )
+ # move_cartesian_path_action_server = Node(
+ # package="robossembler_servers",
+ # executable="move_cartesian_path_action_server",
+ # name="move_cartesian_path_action_server",
+ # parameters=[
+ # robot_description,
+ # robot_description_semantic,
+ # kinematics_yaml,
+ # robot_description_planning,
+ # ompl_yaml,
+ # planning,
+ # trajectory_execution,
+ # moveit_controllers,
+ # planning_scene_monitor_parameters,
+ # use_sim_time
+ # ]
+ # )
launch_nodes = []
launch_nodes.append(rviz)
launch_nodes.append(move_group_node)
- launch_nodes.append(move_topose_action_server)
- launch_nodes.append(move_cartesian_path_action_server)
+ # launch_nodes.append(move_topose_action_server)
+ # launch_nodes.append(move_cartesian_path_action_server)
# launch_nodes.append(move_to_joint_state_action_server)
diff --git a/rasmt_support/README.md b/rasmt_support/README.md
index 6c3ace6..2d0f545 100644
--- a/rasmt_support/README.md
+++ b/rasmt_support/README.md
@@ -6,7 +6,7 @@ This package consists of files describing the AQUA model of the robot, the entir
.
├── CMakeLists.txt
├── config
-│ ├── rasmt_ros2_controllers.yaml # File describing the robot controllers
+│ ├── rasmt_effort_controllers.yaml # File describing the robot controllers
│ └── rasmt.rviz
├── launch
│ ├── rasmt_control.launch.py # Launch file for running robot controlles
diff --git a/rasmt_support/config/rasmt_effort_controllers.yaml b/rasmt_support/config/rasmt_effort_controllers.yaml
new file mode 100644
index 0000000..0485066
--- /dev/null
+++ b/rasmt_support/config/rasmt_effort_controllers.yaml
@@ -0,0 +1,80 @@
+controller_manager:
+ ros__parameters:
+ update_rate: 250
+
+ joint_state_broadcaster:
+ type: joint_state_broadcaster/JointStateBroadcaster
+
+ rasmt_arm_controller:
+ type: effort_controllers/JointGroupEffortController
+
+ rasmt_hand_controller:
+ type: effort_controllers/JointGroupEffortController
+
+rasmt_arm_controller:
+ ros__parameters:
+ joints:
+ - rasmt_Rot_Z_1
+ - rasmt_Rot_Y_1
+ - rasmt_Rot_Z_2
+ - rasmt_Rot_Y_2
+ - rasmt_Rot_Z_3
+ - rasmt_Rot_Y_4
+ command_interfaces:
+ - effort
+ state_interfaces:
+ - position
+ - velocity
+ gains:
+ rasmt_Rot_Z_1:
+ p: 4000.0
+ d: 10.0
+ i: 250.0
+ i_clamp: 15.0
+ rasmt_Rot_Y_1:
+ p: 10000.0
+ d: 25.0
+ i: 600.0
+ i_clamp: 45.0
+ rasmt_Rot_Z_2:
+ p: 8000.0
+ d: 20.0
+ i: 450.0
+ i_clamp: 30.0
+ rasmt_Rot_Y_2:
+ p: 6000.0
+ d: 15.0
+ i: 300.0
+ i_clamp: 30.0
+ rasmt_Rot_Z_3:
+ p: 3000.0
+ d: 5.0
+ i: 175.0
+ i_clamp: 7.0
+ rasmt_Rot_Y_4:
+ p: 2500.0
+ d: 3.0
+ i: 150.0
+ i_clamp: 6.0
+
+rasmt_hand_controller:
+ ros__parameters:
+ joints:
+ - rasmt_Slide_1
+ - rasmt_Slide_2
+ command_interfaces:
+ - effort
+ state_interfaces:
+ - position
+ - velocity
+ gains:
+ rasmt_Slide_1:
+ p: 225.0
+ d: 0.001
+ i: 0.4
+ i_clamp: 4.0
+ rasmt_Slide_2:
+ p: 225.0
+ d: 0.001
+ i: 0.4
+ i_clamp: 4.0
\ No newline at end of file
diff --git a/rasmt_support/config/rasmt_ros2_controllers.yaml b/rasmt_support/config/rasmt_position_controllers.yaml
similarity index 78%
rename from rasmt_support/config/rasmt_ros2_controllers.yaml
rename to rasmt_support/config/rasmt_position_controllers.yaml
index 9e62a9f..12c0b0b 100644
--- a/rasmt_support/config/rasmt_ros2_controllers.yaml
+++ b/rasmt_support/config/rasmt_position_controllers.yaml
@@ -6,7 +6,7 @@ controller_manager:
type: joint_trajectory_controller/JointTrajectoryController
rasmt_hand_controller:
- type: forward_command_controller/ForwardCommandController
+ type: joint_trajectory_controller/JointTrajectoryController
joint_state_broadcaster:
type: joint_state_broadcaster/JointStateBroadcaster
@@ -14,11 +14,6 @@ controller_manager:
rasmt_arm_controller:
ros__parameters:
- command_interfaces:
- - position
- state_interfaces:
- - position
- - velocity
joints:
- rasmt_Rot_Z_1
- rasmt_Rot_Y_1
@@ -26,10 +21,19 @@ rasmt_arm_controller:
- rasmt_Rot_Y_2
- rasmt_Rot_Z_3
- rasmt_Rot_Y_4
+ command_interfaces:
+ - position
+ state_interfaces:
+ - position
+ - velocity
rasmt_hand_controller:
ros__parameters:
- joints:
+ joints:
- rasmt_Slide_1
- rasmt_Slide_2
- interface_name: position
\ No newline at end of file
+ command_interfaces:
+ - position
+ state_interfaces:
+ - position
+ - velocity
\ No newline at end of file
diff --git a/rasmt_support/config/rasmt_position_velocity_controllers.yaml b/rasmt_support/config/rasmt_position_velocity_controllers.yaml
new file mode 100644
index 0000000..bebd134
--- /dev/null
+++ b/rasmt_support/config/rasmt_position_velocity_controllers.yaml
@@ -0,0 +1,41 @@
+controller_manager:
+ ros__parameters:
+ update_rate: 1000 # Hz
+
+ rasmt_arm_controller:
+ type: joint_trajectory_controller/JointTrajectoryController
+
+ rasmt_hand_controller:
+ type: joint_trajectory_controller/JointTrajectoryController
+
+ joint_state_broadcaster:
+ type: joint_state_broadcaster/JointStateBroadcaster
+
+
+rasmt_arm_controller:
+ ros__parameters:
+ joints:
+ - rasmt_Rot_Z_1
+ - rasmt_Rot_Y_1
+ - rasmt_Rot_Z_2
+ - rasmt_Rot_Y_2
+ - rasmt_Rot_Z_3
+ - rasmt_Rot_Y_4
+ command_interfaces:
+ - position
+ - velocity
+ state_interfaces:
+ - position
+ - velocity
+
+rasmt_hand_controller:
+ ros__parameters:
+ joints:
+ - rasmt_Slide_1
+ - rasmt_Slide_2
+ command_interfaces:
+ - position
+ - velocity
+ state_interfaces:
+ - position
+ - velocity
\ No newline at end of file
diff --git a/rasmt_support/launch/rasmt_control.launch.py b/rasmt_support/launch/rasmt_control.launch.py
index 4c51c55..9d5e276 100644
--- a/rasmt_support/launch/rasmt_control.launch.py
+++ b/rasmt_support/launch/rasmt_control.launch.py
@@ -1,6 +1,7 @@
+from numpy import load
from launch.launch_description import LaunchDescription
-from launch.actions import DeclareLaunchArgument
-from launch.conditions import UnlessCondition
+from launch.actions import DeclareLaunchArgument, ExecuteProcess
+from launch.conditions import UnlessCondition, IfCondition
from launch.substitutions import PathJoinSubstitution, LaunchConfiguration
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
@@ -27,22 +28,23 @@ def generate_launch_description():
# Configure robot_description
robot_description = {"robot_description": LaunchConfiguration("robot_description")}
-
# Load controllers from YAML configuration file
controller_configurations = PathJoinSubstitution([
FindPackageShare("rasmt_support"),
"config",
- "rasmt_ros2_controllers.yaml"
+ "rasmt_position_velocity_controllers.yaml"
])
# Prepare controller manager and other required nodes
- controller_manager = Node(
- package="controller_manager",
- executable="ros2_control_node",
- parameters=[robot_description, controller_configurations],
- output="screen",
- condition=UnlessCondition(LaunchConfiguration("sim"))
- )
+ # ros2_control_node = Node(
+ # package="controller_manager",
+ # executable="ros2_control_node",
+ # parameters=[robot_description, controller_configurations],
+ # output={
+ # "stdout": "screen",
+ # "stderr": "screen",
+ # },
+ # )
robot_state_publisher = Node(
package="robot_state_publisher",
@@ -51,33 +53,32 @@ def generate_launch_description():
parameters=[robot_description]
)
- joint_state_broadcaster = Node(
- package="controller_manager",
- executable="spawner.py",
- arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"],
- )
+ # action_server_controller_hand_node = Node(
+ # package="robossembler_servers",
+ # executable="gripper_cmd_node"
+ # )
+
+
+ # Load controllers
+ load_controllers = []
+ for controller in [
+ "rasmt_arm_controller",
+ "rasmt_hand_controller",
+ "joint_state_broadcaster",
+ ]:
+ load_controllers += [
+ ExecuteProcess(
+ cmd=["ros2 control load_controller --set-state start {}".format(controller)],
+ shell=True,
+ output="screen",
+ )
+ ]
- controller_arm = Node(
- package="controller_manager",
- executable="spawner.py",
- arguments=["rasmt_arm_controller", "--controller-manager", "/controller_manager"],
- )
- controller_hand = Node(
- package="controller_manager",
- executable="spawner.py",
- arguments=["rasmt_hand_controller", "--controller-manager", "/controller_manager"],
- )
- action_server_controller_hand_node = Node(
- package="robossembler_servers",
- executable="gripper_cmd_node"
- )
return LaunchDescription(
- launch_args + [
- controller_manager,
- robot_state_publisher,
- joint_state_broadcaster,
- controller_arm,
- controller_hand,
- action_server_controller_hand_node
- ])
\ No newline at end of file
+ [
+ robot_state_publisher
+ ]
+ + load_controllers
+ + launch_args
+ )
diff --git a/rasmt_support/launch/rasmt_gazebo.launch.py b/rasmt_support/launch/rasmt_gazebo.launch.py
index 1f3daca..3fa7c3e 100644
--- a/rasmt_support/launch/rasmt_gazebo.launch.py
+++ b/rasmt_support/launch/rasmt_gazebo.launch.py
@@ -21,22 +21,20 @@ def generate_launch_description():
world_file = os.path.join(get_package_share_directory("rasmt_support"), "world", "robossembler.world")
#test_world_file = "/home/bill-finger/gazebo_pkgs_ws/gazebo-pkgs/gazebo_grasp_plugin/test_world/test_world_full.world"
- gzserver = IncludeLaunchDescription(
+ # gzserver = IncludeLaunchDescription(
+ # PythonLaunchDescriptionSource(
+ # os.path.join(get_package_share_directory("gazebo_ros"), 'launch', 'gzserver.launch.py'),
+ # ), launch_arguments={'world':world_file}.items()
+ # )
+ # gzclient = IncludeLaunchDescription(
+ # PythonLaunchDescriptionSource(
+ # os.path.join(get_package_share_directory("gazebo_ros"), 'launch', 'gzclient.launch.py'),
+ # )
+ # )
+
+ gazebo = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
- PathJoinSubstitution([
- FindPackageShare("gazebo_ros"),
- "launch",
- "gzserver.launch.py"
- ])
- ), launch_arguments={'world':world_file, 'extra_gazebo_args':'--verbose'}.items()
- )
- gzclient = IncludeLaunchDescription(
- PythonLaunchDescriptionSource(
- PathJoinSubstitution([
- FindPackageShare("gazebo_ros"),
- "launch",
- "gzclient.launch.py"
- ])
+ os.path.join(get_package_share_directory("gazebo_ros"), 'launch', 'gazebo.launch.py'),
)
)
@@ -66,9 +64,9 @@ def generate_launch_description():
)"""
launch_nodes = []
- #launch_nodes.append(gazebo)
- launch_nodes.append(gzserver)
- launch_nodes.append(gzclient)
+ launch_nodes.append(gazebo)
+ #launch_nodes.append(gzserver)
+ #launch_nodes.append(gzclient)
launch_nodes.append(spawn_entity)
#launch_nodes.append(cube_spawn)
diff --git a/rasmt_support/launch/rasmt_ignition.launch.py b/rasmt_support/launch/rasmt_ignition.launch.py
new file mode 100644
index 0000000..abe796d
--- /dev/null
+++ b/rasmt_support/launch/rasmt_ignition.launch.py
@@ -0,0 +1,71 @@
+from launch import LaunchDescription
+from launch.actions import (
+ DeclareLaunchArgument,
+ IncludeLaunchDescription,
+ OpaqueFunction,
+ RegisterEventHandler,
+)
+from launch.conditions import IfCondition, UnlessCondition
+from launch.event_handlers import OnProcessExit
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+import xacro
+import os
+from ament_index_python import get_package_share_directory
+
+
+def generate_launch_description():
+
+ launch_args = []
+ launch_args.append(DeclareLaunchArgument(
+ name="robot_name",
+ default_value="rasmt",
+ description="Set robot name in gazebo env"
+ )
+ )
+ launch_args.append(DeclareLaunchArgument(
+ name="robot_description_content",
+ description="Robot XML file"
+ )
+ )
+
+ #robot_description_content = {"robot_description":LaunchConfiguration("robot_description_content")}
+ robot_description_content = LaunchConfiguration("robot_description_content")
+ # launch Ignition Gazebo
+ pkg_ros_ign_gazebo = get_package_share_directory('ros_ign_gazebo')
+ gazebo = IncludeLaunchDescription(
+ PythonLaunchDescriptionSource(
+ os.path.join(pkg_ros_ign_gazebo, 'launch', 'ign_gazebo.launch.py')),
+ launch_arguments={'ign_args': '-r empty.sdf'}.items(),
+ )
+
+ ros2_ign_bridge = IncludeLaunchDescription(
+ PythonLaunchDescriptionSource(
+ PathJoinSubstitution(
+ [
+ FindPackageShare("rasmt_support"),
+ "launch",
+ "rasmt_ignition_bridge.launch.py"
+ ]
+ )
+ )
+ )
+
+ # Spawn
+ spawn = Node(package='ros_ign_gazebo', executable='create',
+ arguments=[
+ '-string', robot_description_content,
+ '-name', 'rasmt',
+ '-allow_renaming', 'true'],
+ output='screen')
+
+
+ launch_nodes = []
+ launch_nodes.append(gazebo)
+ launch_nodes.append(spawn)
+ launch_nodes.append(ros2_ign_bridge)
+
+
+ return LaunchDescription(launch_args + launch_nodes)
\ No newline at end of file
diff --git a/rasmt_support/launch/rasmt_ignition_bridge.launch.py b/rasmt_support/launch/rasmt_ignition_bridge.launch.py
new file mode 100644
index 0000000..38c3b10
--- /dev/null
+++ b/rasmt_support/launch/rasmt_ignition_bridge.launch.py
@@ -0,0 +1,62 @@
+from launch.launch_description import LaunchDescription
+from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
+from launch.substitutions import PathJoinSubstitution, LaunchConfiguration, FindExecutable, Command
+from launch_ros.substitutions import FindPackageShare
+from launch_ros.actions import Node
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+import xacro
+import os
+from ament_index_python import get_package_share_directory
+
+
+
+def generate_launch_description():
+
+ use_sim_time = LaunchConfiguration('use_sim_time', default=True)
+ log_level = LaunchConfiguration('log_level', default='fatal')
+
+
+ launch_args = []
+ launch_args.append(
+ DeclareLaunchArgument(
+ name='use_sim_time',
+ default_value=use_sim_time,
+ description="If true, use simulated clock"
+ )
+ )
+ launch_args.append(
+ DeclareLaunchArgument(
+ name='log_level',
+ default_value=log_level,
+ description="Log level of all nodes launched by this script"
+ )
+ )
+
+ # JointTrajectory bridge for gripper (ROS2 -> IGN)
+ joint_trajectory_controller_bridge = Node(package='ros_ign_bridge',
+ executable='parameter_bridge',
+ name='parameter_bridge_gripper_trajectory',
+ output='screen',
+ arguments=['/gripper_trajectory@trajectory_msgs/msg/JointTrajectory]ignition.msgs.JointTrajectory',
+ '--ros-args', '--log-level', log_level],
+ parameters=[{'use_sim_time': use_sim_time}])
+
+ # ros_ign_bridge (clock -> ROS 2)
+ ros2_ign_clock_bridge = Node(
+ package="ros_ign_bridge",
+ executable="parameter_bridge",
+ output="log",
+ arguments=[
+ "/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock",
+ "--ros-args",
+ "--log-level",
+ log_level,
+ ],
+ parameters=[{"use_sim_time": use_sim_time}],
+ )
+
+ launch_nodes = []
+ #launch_nodes.append(joint_trajectory_controller_bridge)
+ launch_nodes.append(ros2_ign_clock_bridge)
+
+ return LaunchDescription(launch_nodes + launch_args)
\ No newline at end of file
diff --git a/rasmt_support/package.xml b/rasmt_support/package.xml
index 08bd341..7922ee3 100644
--- a/rasmt_support/package.xml
+++ b/rasmt_support/package.xml
@@ -17,5 +17,8 @@
ament_cmake
+
+
\ No newline at end of file
diff --git a/rasmt_support/urdf/ras.sdf b/rasmt_support/urdf/ras.sdf
new file mode 100644
index 0000000..9c754c2
--- /dev/null
+++ b/rasmt_support/urdf/ras.sdf
@@ -0,0 +1,89 @@
+
+
+
+ 0.001
+ 1.0
+ 1000
+
+
+
+
+
+ 0 0 -9.8
+ 5.5645e-6 22.8758e-6 -42.3884e-6
+
+
+ 0.4 0.4 0.4 1.0
+ .7 .7 .7 1
+ true
+
+
+ 1
+
+
+
+
+ 0 0 1
+ 100 100
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 0 0 1
+ 100 100
+
+
+
+ 0.8 0.8 0.8 1
+ 0.8 0.8 0.8 1
+ 0.8 0.8 0.8 1
+
+
+ 0 0 0 0 -0 0
+
+ 0 0 0 0 -0 0
+ 1
+
+ 1
+ 0
+ 0
+ 1
+ 0
+ 1
+
+
+ 0
+
+ 0 0 0 0 -0 0
+ 0
+
+
+ 0 0 10 0 -0 0
+ 1
+ 1
+ -0.5 0.1 -0.9
+ 0.8 0.8 0.8 1
+ 0.2 0.2 0.2 1
+
+ 1000
+ 0.01
+ 0.9
+ 0.001
+
+
+ 0
+ 0
+ 0
+
+
+
+
diff --git a/rasmt_support/urdf/rasmt.sdf b/rasmt_support/urdf/rasmt.sdf
new file mode 100644
index 0000000..998a6ff
--- /dev/null
+++ b/rasmt_support/urdf/rasmt.sdf
@@ -0,0 +1,512 @@
+
+
+
+ 0 0 0 0 0 0
+ world
+ rasmt_Base_Link
+
+
+ 0 0 0 0 0 0
+
+ -0.0030651 -3.2739e-05 0.082353 0 0 0
+ 5.2929
+
+ 0.0076169
+ 1.0121e-05
+ -0.00010622
+ 0.0076597
+ 6.511699999999999e-05
+ 0.01165
+
+
+
+ 0 0 0 0 0 0
+
+
+ 1 1 1
+ model://rasmt_support/meshes/collision/Base_Link.STL
+
+
+
+
+ 0 0 0 0 0 0
+
+
+ 1 1 1
+ model://rasmt_support/meshes/visual/Base_Link.dae
+
+
+
+ 0.9411749988794327 0.9411749988794327 0.9411749988794327 1
+ 0.9411749988794327 0.9411749988794327 0.9411749988794327 1
+
+
+
+
+ 0 0 0.2533 3.141585307179587 0 0
+ rasmt_Base_Link
+ rasmt_Fork_1
+
+ 0 0 1
+
+ -3.1416
+ 3.1416
+ 5.149
+ 0.99903
+
+
+ 0
+ 0
+
+
+
+
+ 0 0 0 0 0 0
+
+ 0.043764 -0.0066984 -0.032285 0 0 0
+ 0.67797
+
+ 0.0014091
+ -6.2674e-05
+ 0.00057897
+ 0.0017329
+ 4.7826e-05
+ 0.0019056
+
+
+
+ 0 0 0 0 0 0
+
+
+ 1 1 1
+ model://rasmt_support/meshes/collision/Fork_1.STL
+
+
+
+
+ 0 0 0 0 0 0
+
+
+ 1 1 1
+ model://rasmt_support/meshes/visual/Fork_1.dae
+
+
+
+ 0.9411749988794327 0.9411749988794327 0.9411749988794327 1
+ 0.9411749988794327 0.9411749988794327 0.9411749988794327 1
+
+
+
+
+ 0.1045 0 -0.0795 -3.141585307179587 0 0
+ rasmt_Fork_1
+ rasmt_Link_1
+
+ 0 1 0
+
+ -1.5707
+ 2.2863
+ 5.149
+ 0.99903
+
+
+ 0
+ 0
+
+
+
+
+ 0 0 0 0 0 0
+
+ -0.00042264 0 0.075171 0 0 0
+ 1.8959
+
+ 0.0029317
+ -9.417e-06
+ 5.5248e-05
+ 0.0031666
+ -9.3067e-05
+ 0.0015477
+
+
+
+ 0 0 0 0 0 0
+
+
+ 1 1 1
+ model://rasmt_support/meshes/collision/Link_1.STL
+
+
+
+
+ 0 0 0 0 0 0
+
+
+ 1 1 1
+ model://rasmt_support/meshes/visual/Link_1.dae
+
+
+
+ 0.9411749988794327 0.9411749988794327 0.9411749988794327 1
+ 0.9411749988794327 0.9411749988794327 0.9411749988794327 1
+
+
+
+
+ 0 0 0.17502 0 0 0
+ rasmt_Link_1
+ rasmt_Fork_2
+
+ 0 0 1
+
+ -3.1416
+ 3.1416
+ 5.149
+ 0.99903
+
+
+ 0
+ 0
+
+
+
+
+ 0 0 0 0 0 0
+
+ 0.043764 0.0066984 0.032285 0 0 0
+ 0.67797
+
+ 0.0014091
+ 6.2674e-05
+ -0.00057897
+ 0.0017329
+ 4.7826e-05
+ 0.0019056
+
+
+
+ 0 0 0 0 0 0
+
+
+ 1 1 1
+ model://rasmt_support/meshes/collision/Fork_2.STL
+
+
+
+
+ 0 0 0 0 0 0
+
+
+ 1 1 1
+ model://rasmt_support/meshes/visual/Fork_2.dae
+
+
+
+ 0.9411749988794327 0.9411749988794327 0.9411749988794327 1
+ 0.9411749988794327 0.9411749988794327 0.9411749988794327 1
+
+
+
+
+ 0.1045 0 0.0795 0 0 0
+ rasmt_Fork_2
+ rasmt_Link_2
+
+ 0 1 0
+
+ -1.5707
+ 2.2863
+ 5.149
+ 0.99903
+
+
+ 0
+ 0
+
+
+
+
+ 0 0 0 0 0 0
+
+ -0.00042264 0 0.075171 0 0 0
+ 1.8959
+
+ 0.0029317
+ -9.4171e-06
+ 5.5248e-05
+ 0.0031666
+ -9.3067e-05
+ 0.0015477
+
+
+
+ 0 0 0 0 0 0
+
+
+ 1 1 1
+ model://rasmt_support/meshes/collision/Link_2.STL
+
+
+
+
+ 0 0 0 0 0 0
+
+
+ 1 1 1
+ model://rasmt_support/meshes/visual/Link_2.dae
+
+
+
+ 0.9411749988794327 0.9411749988794327 0.9411749988794327 1
+ 0.9411749988794327 0.9411749988794327 0.9411749988794327 1
+
+
+
+
+ 0 0 0.175 0 0 0
+ rasmt_Link_2
+ rasmt_Fork_3
+
+ 0 0 1
+
+ -3.14159
+ 3.14159
+ 5.149
+ 0.99903
+
+
+ 0
+ 0
+
+
+
+
+ 0 0 0 0 0 0
+
+ 0.0437644555284691 0.00669835350471771 0.0322846229336455 0 0 0
+ 0.677972982551887
+
+ 0.00140908677953665
+ 6.267434924451639e-05
+ -0.000578970009504215
+ 0.00173285340301686
+ 4.78263030621606e-05
+ 0.00190561919128035
+
+
+
+ 0 0 0 0 0 0
+
+
+ 1 1 1
+ model://rasmt_support/meshes/collision/Fork_3.STL
+
+
+
+
+ 0 0 0 0 0 0
+
+
+ 1 1 1
+ model://rasmt_support/meshes/visual/Fork_3.dae
+
+
+
+ 0.9411764889955521 0.9411764889955521 0.9411764889955521 1
+ 0.9411764889955521 0.9411764889955521 0.9411764889955521 1
+
+
+
+
+ 0.1045 0 0.0795 0 0 0
+ rasmt_Fork_3
+ rasmt_Dock_Link
+
+ 0 1 0
+
+ -1.5707
+ 2.2863
+ 5.149
+ 0.99903
+
+
+ 0
+ 0
+
+
+
+
+ 0 0 0 0 0 0
+
+ 0.000487278098416338 0.0001516907797566372 0.08254557371325712 0 0 0
+ 2.53391750781824
+
+ 0.006064508328644976
+ -2.015356731520154e-06
+ -7.294101423638933e-05
+ 0.006094829365861896
+ -0.0001246350455024134
+ 0.002069011842474237
+
+
+
+ 0 0 0 0 0 0
+
+
+ 1 1 1
+ model://rasmt_support/meshes/collision/Dock_Link.STL
+
+
+
+
+ 0 0 0.12324 -3.14159265358979 0 0
+
+
+ 1 1 1
+ model://rasmt_support/meshes/collision/Grip_Body.STL
+
+
+
+
+ 0 0 0 0 0 0
+
+
+ 1 1 1
+ model://rasmt_support/meshes/visual/Dock_Link.dae
+
+
+
+ 0.9411764889955521 0.9411764889955521 0.9411764889955521 1
+ 0.9411764889955521 0.9411764889955521 0.9411764889955521 1
+
+
+
+ 0 0 0.12324 -3.14159265358979 0 0
+
+
+ 1 1 1
+ model://rasmt_support/meshes/visual/Grip_Body.dae
+
+
+
+ 0.6274499744176865 1 0.6274499744176865 1
+ 0.6274499744176865 1 0.6274499744176865 1
+
+
+
+
+ 0 -3.006528453019044e-16 0.21629 7.346410206709656e-06 -3.231089339497412e-15 1.5708
+ rasmt_Dock_Link
+ rasmt_Grip_L
+
+ 1 0 0
+
+ 0
+ 0.06
+ 20
+ 0.2
+
+
+ 0
+ 0
+
+
+
+
+ 0 0 0 0 0 0
+
+ -0.010118 0.010281 -0.0038252 0 0 0
+ 0.021223
+
+ 6.5436e-06
+ -3.114e-06
+ 2.8479e-06
+ 1.9726e-05
+ 1.6432e-06
+ 1.6355e-05
+
+
+
+ -0.02 0 0 0 0 0
+
+
+ 1 1 1
+ model://rasmt_support/meshes/collision/Grip_L.STL
+
+
+
+
+ -0.02 0 0 0 0 0
+
+
+ 1 1 1
+ model://rasmt_support/meshes/visual/Grip_L.dae
+
+
+
+ 0.9411749988794327 1 1 1
+ 0.9411749988794327 1 1 1
+
+
+
+
+ 0 -3.006528453019044e-16 0.21629 7.346410206709656e-06 3.231089339497412e-15 -1.5708
+ rasmt_Dock_Link
+ rasmt_Grip_R
+
+ 1 0 0
+
+ 0
+ 0.06
+ 20
+ 0.2
+
+
+ 0
+ 0
+
+
+
+
+ 0 0 0 0 0 0
+
+ -0.010118 0.010281 -0.0038252 0 0 0
+ 0.021223
+
+ 6.5436e-06
+ -3.114e-06
+ 2.8479e-06
+ 1.9726e-05
+ 1.6432e-06
+ 1.6355e-05
+
+
+
+ -0.02 0 0 0 0 0
+
+
+ 1 1 1
+ model://rasmt_support/meshes/collision/Grip_R.STL
+
+
+
+
+ -0.02 0 0 0 0 0
+
+
+ 1 1 1
+ model://rasmt_support/meshes/visual/Grip_R.dae
+
+
+
+ 0.9411749988794327 1 1 1
+ 0.9411749988794327 1 1 1
+
+
+
+ false
+
+ /home/bill-finger/rasms_ws/install/share/rasmt_support/config/rasmt_effort_controllers.yaml
+
+
+
diff --git a/rasmt_support/urdf/rasmt.urdf b/rasmt_support/urdf/rasmt.urdf
new file mode 100644
index 0000000..f1967bd
--- /dev/null
+++ b/rasmt_support/urdf/rasmt.urdf
@@ -0,0 +1,486 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ /home/bill-finger/rasms_ws/install/share/rasmt_support/config/rasmt_effort_controllers.yaml
+
+
+
+
+
+
+ ign_ros2_control/IgnitionSystem
+
+
+
+
+ -1
+ 1
+
+
+
+
+
+
+
+ -1
+ 1
+
+
+
+
+
+
+
+ -1
+ 1
+
+
+
+
+
+
+
+ -1
+ 1
+
+
+
+
+
+
+
+ -1
+ 1
+
+
+
+
+
+
+
+ -1
+ 1
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ ign_ros2_control/IgnitionSystem
+
+
+
+
+ 0
+ 10
+
+
+
+
+
+
+
+
+ 0
+ 10
+
+
+
+
+
+
+
+
+
+
diff --git a/rasmt_support/urdf/rasmt.xacro b/rasmt_support/urdf/rasmt.xacro
index 04b5321..e32be7f 100644
--- a/rasmt_support/urdf/rasmt.xacro
+++ b/rasmt_support/urdf/rasmt.xacro
@@ -1,21 +1,25 @@
-
-
+
+
-
+
-
-
+
+
+
+
+
+
diff --git a/rasmt_support/urdf/robot/rasmt_single_control.xacro b/rasmt_support/urdf/robot/rasmt_single_control.xacro
index 31813c3..61fed41 100644
--- a/rasmt_support/urdf/robot/rasmt_single_control.xacro
+++ b/rasmt_support/urdf/robot/rasmt_single_control.xacro
@@ -4,74 +4,60 @@
-
+
+
+
+ ign_ros2_control/IgnitionSystem
+
-
+
fake_components/GenericSystem
-
+
-
-
- -1
- 1
-
+
-
- -1
- 1
-
+
-
- -1
- 1
-
+
-
- -1
- 1
-
+
-
- -1
- 1
-
+
-
- -1
- 1
-
+
diff --git a/rasmt_support/urdf/robot/rasmt_single_gazebo.xacro b/rasmt_support/urdf/robot/rasmt_single_gazebo.xacro
index 2b2eb2e..50bea04 100644
--- a/rasmt_support/urdf/robot/rasmt_single_gazebo.xacro
+++ b/rasmt_support/urdf/robot/rasmt_single_gazebo.xacro
@@ -1,63 +1,26 @@
-
+
-
-
-
- $(find rasmt_support)/config/rasmt_ros2_controllers.yaml
- /${prefix}
-
-
+
-
-
- 0.2
- 0.2
- true
-
+
+
+
+
+ $(find rasmt_support)/config/rasmt_effort_controllers.yaml
+
+
+
+
-
-
- 0.2
- 0.2
- true
-
-
-
-
- 0.2
- 0.2
- true
-
-
-
-
- 0.2
- 0.2
- true
-
-
-
-
- 0.2
- 0.2
- true
-
-
-
-
- 0.2
- 0.2
- true
-
-
-
-
- 0.2
- 0.2
- true
-
\ No newline at end of file
diff --git a/rasmt_support/urdf/robot/rasmt_single_macro.xacro b/rasmt_support/urdf/robot/rasmt_single_macro.xacro
index e1bde10..20f7b76 100644
--- a/rasmt_support/urdf/robot/rasmt_single_macro.xacro
+++ b/rasmt_support/urdf/robot/rasmt_single_macro.xacro
@@ -398,7 +398,7 @@
effort="5.149"
velocity="0.99903" />
-
+
diff --git a/rasmt_support/urdf/tools/rasmt_hand_control.xacro b/rasmt_support/urdf/tools/rasmt_hand_control.xacro
index 0a0a254..9b96f44 100644
--- a/rasmt_support/urdf/tools/rasmt_hand_control.xacro
+++ b/rasmt_support/urdf/tools/rasmt_hand_control.xacro
@@ -4,36 +4,32 @@
-
+
gazebo_ros2_control/GazeboSystem
+
+
+ ign_ros2_control/IgnitionSystem
+
+
-
+
-
-
- 0
- 10
-
+
-
-
- 0
- 10
-
+
diff --git a/rasmt_support/urdf/tools/rasmt_hand_gazebo.xacro b/rasmt_support/urdf/tools/rasmt_hand_gazebo.xacro
index e9004f4..b41bd8f 100644
--- a/rasmt_support/urdf/tools/rasmt_hand_gazebo.xacro
+++ b/rasmt_support/urdf/tools/rasmt_hand_gazebo.xacro
@@ -11,26 +11,23 @@
-
-
+
0.2
0.2
true
-
0.2
0.2
true
-
+ -->
\ No newline at end of file
diff --git a/rbs.repos b/rbs.repos
new file mode 100644
index 0000000..4c83886
--- /dev/null
+++ b/rbs.repos
@@ -0,0 +1,17 @@
+repositories:
+ ur_description:
+ type: git
+ url: https://github.com/UniversalRobots/Universal_Robots_ROS2_Description.git
+ version: ros2
+ ur_moveit_config:
+ type: git
+ url: https://github.com/solid-sinusoid/ur_moveit_config.git
+ version: main
+ gz_ros2_control:
+ type: git
+ url: https://github.com/solid-sinusoid/gz_ros2_control.git
+ version: humble-fix
+ behavior_tree:
+ type: git
+ url: https://github.com/solid-sinusoid/behavior_tree.git
+ version: master
\ No newline at end of file
diff --git a/rbs_bt_executor/CMakeLists.txt b/rbs_bt_executor/CMakeLists.txt
new file mode 100644
index 0000000..fd792f4
--- /dev/null
+++ b/rbs_bt_executor/CMakeLists.txt
@@ -0,0 +1,80 @@
+cmake_minimum_required(VERSION 3.8)
+project(rbs_bt_executor)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+ add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_action REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+find_package(moveit_msgs REQUIRED)
+find_package(moveit_core REQUIRED)
+find_package(moveit_ros_planning REQUIRED)
+find_package(moveit_ros_planning_interface REQUIRED)
+find_package(ament_index_cpp REQUIRED)
+find_package(rbs_skill_interfaces REQUIRED)
+find_package(behavior_tree REQUIRED)
+find_package(control_msgs REQUIRED)
+
+if (NOT CMAKE_CXX_STANDARD)
+ set(CMAKE_CXX_STANDARD 17)
+endif()
+
+set(dependencies
+ rclcpp
+ rclcpp_action
+ geometry_msgs
+ tf2_geometry_msgs
+ moveit_msgs
+ moveit_core
+ moveit_ros_planning
+ moveit_ros_planning_interface
+ ament_index_cpp
+ rbs_skill_interfaces
+ behavior_tree
+ std_msgs
+ control_msgs
+)
+
+include_directories(include)
+
+add_library(rbs_skill_move_topose_bt_action_client SHARED src/MoveToPose.cpp)
+list(APPEND plugin_libs rbs_skill_move_topose_bt_action_client)
+
+
+foreach(bt_plugin ${plugin_libs})
+ ament_target_dependencies(${bt_plugin} ${dependencies})
+ target_compile_definitions(${bt_plugin} PRIVATE BT_PLUGIN_EXPORT)
+endforeach()
+
+
+
+install(DIRECTORY launch bt_trees config DESTINATION share/${PROJECT_NAME})
+
+install(TARGETS
+ ${plugin_libs}
+ ARCHIVE DESTINATION lib
+ LIBRARY DESTINATION lib
+ RUNTIME DESTINATION lib/${PROJECT_NAME}
+)
+
+
+if(BUILD_TESTING)
+ find_package(ament_lint_auto REQUIRED)
+ # the following line skips the linter which checks for copyrights
+ # comment the line when a copyright and license is added to all source files
+ set(ament_cmake_copyright_FOUND TRUE)
+ # the following line skips cpplint (only works in a git repo)
+ # comment the line when this package is in a git repo and when
+ # a copyright and license is added to all source files
+ set(ament_cmake_cpplint_FOUND TRUE)
+ ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_export_dependencies(${dependencies})
+
+ament_package()
diff --git a/robossembler/behavior_trees_xml/atomic_skills_xml/assemble.xml b/rbs_bt_executor/bt_trees/atomic_skills_xml/assemble.xml
similarity index 100%
rename from robossembler/behavior_trees_xml/atomic_skills_xml/assemble.xml
rename to rbs_bt_executor/bt_trees/atomic_skills_xml/assemble.xml
diff --git a/robossembler/behavior_trees_xml/atomic_skills_xml/get-part-state.xml b/rbs_bt_executor/bt_trees/atomic_skills_xml/get-part-state.xml
similarity index 100%
rename from robossembler/behavior_trees_xml/atomic_skills_xml/get-part-state.xml
rename to rbs_bt_executor/bt_trees/atomic_skills_xml/get-part-state.xml
diff --git a/robossembler/behavior_trees_xml/atomic_skills_xml/grasp_part.xml b/rbs_bt_executor/bt_trees/atomic_skills_xml/grasp_part.xml
similarity index 100%
rename from robossembler/behavior_trees_xml/atomic_skills_xml/grasp_part.xml
rename to rbs_bt_executor/bt_trees/atomic_skills_xml/grasp_part.xml
diff --git a/robossembler/behavior_trees_xml/atomic_skills_xml/grasp_skill.xml b/rbs_bt_executor/bt_trees/atomic_skills_xml/grasp_skill.xml
similarity index 100%
rename from robossembler/behavior_trees_xml/atomic_skills_xml/grasp_skill.xml
rename to rbs_bt_executor/bt_trees/atomic_skills_xml/grasp_skill.xml
diff --git a/robossembler/behavior_trees_xml/atomic_skills_xml/move.xml b/rbs_bt_executor/bt_trees/atomic_skills_xml/move.xml
similarity index 100%
rename from robossembler/behavior_trees_xml/atomic_skills_xml/move.xml
rename to rbs_bt_executor/bt_trees/atomic_skills_xml/move.xml
diff --git a/robossembler/behavior_trees_xml/atomic_skills_xml/move_gripper.xml b/rbs_bt_executor/bt_trees/atomic_skills_xml/move_gripper.xml
similarity index 100%
rename from robossembler/behavior_trees_xml/atomic_skills_xml/move_gripper.xml
rename to rbs_bt_executor/bt_trees/atomic_skills_xml/move_gripper.xml
diff --git a/robossembler/behavior_trees_xml/atomic_skills_xml/print.xml b/rbs_bt_executor/bt_trees/atomic_skills_xml/print.xml
similarity index 100%
rename from robossembler/behavior_trees_xml/atomic_skills_xml/print.xml
rename to rbs_bt_executor/bt_trees/atomic_skills_xml/print.xml
diff --git a/robossembler/behavior_trees_xml/atomic_skills_xml/remove.xml b/rbs_bt_executor/bt_trees/atomic_skills_xml/remove.xml
similarity index 100%
rename from robossembler/behavior_trees_xml/atomic_skills_xml/remove.xml
rename to rbs_bt_executor/bt_trees/atomic_skills_xml/remove.xml
diff --git a/rbs_bt_executor/bt_trees/test_tree.xml b/rbs_bt_executor/bt_trees/test_tree.xml
new file mode 100644
index 0000000..73d072c
--- /dev/null
+++ b/rbs_bt_executor/bt_trees/test_tree.xml
@@ -0,0 +1,37 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/rbs_bt_executor/config/params.yaml b/rbs_bt_executor/config/params.yaml
new file mode 100644
index 0000000..2c73ece
--- /dev/null
+++ b/rbs_bt_executor/config/params.yaml
@@ -0,0 +1,6 @@
+# Here is a nodes which calling from launch for bt_tree
+simple_move_bt_nodes:
+ ros__parameters:
+ plugins:
+ - rbs_skill_move_topose_bt_action_client
+
diff --git a/rbs_bt_executor/config/points.yaml b/rbs_bt_executor/config/points.yaml
new file mode 100644
index 0000000..b33651c
--- /dev/null
+++ b/rbs_bt_executor/config/points.yaml
@@ -0,0 +1,29 @@
+bt_engine:
+ ros__parameters:
+ 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
+ ]
\ No newline at end of file
diff --git a/rbs_bt_executor/launch/rbs_executor.launch.py b/rbs_bt_executor/launch/rbs_executor.launch.py
new file mode 100644
index 0000000..5f222a3
--- /dev/null
+++ b/rbs_bt_executor/launch/rbs_executor.launch.py
@@ -0,0 +1,35 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from ament_index_python.packages import get_package_share_directory
+import os
+
+
+def generate_launch_description():
+
+ bt_config = os.path.join(
+ get_package_share_directory('rbs_bt_executor'),
+ 'config',
+ 'params.yaml'
+ )
+ points = os.path.join(
+ get_package_share_directory('rbs_bt_executor'),
+ 'config',
+ 'points.yaml'
+ )
+
+
+ return LaunchDescription([
+ Node(
+ package='behavior_tree',
+ namespace='',
+ executable='bt_engine',
+ name="bt_engine",
+ # Do not declare a node name otherwise it messes with the action node names and will result in
+ # duplicate nodes!
+ parameters=[
+ {'bt_file_path': os.path.join(get_package_share_directory('rbs_bt_queue'), 'bt_xmls/test_tree.xml')},
+ {'plugins': ['rbs_skill_move_topose_bt_action_client']},
+ points
+ ]
+ ),
+ ])
diff --git a/robossembler_scene_monitor/scene_monitor_interfaces/package.xml b/rbs_bt_executor/package.xml
similarity index 58%
rename from robossembler_scene_monitor/scene_monitor_interfaces/package.xml
rename to rbs_bt_executor/package.xml
index 49f90e8..fce45a2 100644
--- a/robossembler_scene_monitor/scene_monitor_interfaces/package.xml
+++ b/rbs_bt_executor/package.xml
@@ -1,25 +1,20 @@
- scene_monitor_interfaces
+ rbs_bt_executor
0.0.0
TODO: Package description
- Splinter1984
+ root
TODO: License declaration
ament_cmake
- rosidl_default_generators
-
- rosidl_default_runtime
- geometry_msgs
- std_msgs
+ rbs_skill_interfaces
+ behavior_tree
ament_lint_auto
ament_lint_common
- rosidl_interface_packages
-
ament_cmake
-
\ No newline at end of file
+
diff --git a/robossembler/src/behavior_tree_nodes/atomic_skills/MoveToPose.cpp b/rbs_bt_executor/src/MoveToPose.cpp
similarity index 56%
rename from robossembler/src/behavior_tree_nodes/atomic_skills/MoveToPose.cpp
rename to rbs_bt_executor/src/MoveToPose.cpp
index 74a9d3c..fc1f91d 100644
--- a/robossembler/src/behavior_tree_nodes/atomic_skills/MoveToPose.cpp
+++ b/rbs_bt_executor/src/MoveToPose.cpp
@@ -1,4 +1,4 @@
-#include "robossembler_interfaces/action/moveit_send_pose.hpp"
+#include "rbs_skill_interfaces/action/moveit_send_pose.hpp"
#include "behaviortree_cpp_v3/bt_factory.h"
#include "behavior_tree/BtAction.hpp"
@@ -8,7 +8,7 @@
#include "rclcpp/parameter.hpp"
using namespace BT;
-using MoveToPoseAction = robossembler_interfaces::action::MoveitSendPose;
+using MoveToPoseAction = rbs_skill_interfaces::action::MoveitSendPose;
static const rclcpp::Logger LOGGER = rclcpp::get_logger("MoveToPoseActionClient");
class MoveToPose : public BtAction
@@ -17,22 +17,33 @@ class MoveToPose : public BtAction
MoveToPose(const std::string & name, const BT::NodeConfiguration & config)
: BtAction(name, config)
{
+ //auto params = _node->declare_parameters("Poses")
+ RCLCPP_INFO(_node->get_logger(), "Start the node");
+ pose_des.position.x = 0.11724797630931184;
+ pose_des.position.y = 0.46766635768602544;
+ pose_des.position.z = 0.5793862343094913;
+ pose_des.orientation.x = 0.9987999001314066;
+ pose_des.orientation.y = 0.041553846820940925;
+ pose_des.orientation.z = -0.004693314677006583;
+ pose_des.orientation.w = -0.025495295825239704;
}
MoveToPoseAction::Goal populate_goal() override
{
getInput("robot_name", robot_name_);
- getInput("pose", pose_);
+ getInput("pose", pose_);
+
+ //auto pose = _node->get_parameter(pose_).get_parameter_value().get();
RCLCPP_INFO(_node->get_logger(), "GrasPose pose.x: %f pose.y: %f pose.z: \n%f opientation.x: %f orientation.y: %f orientation.z: %f orientation.w: %f",
- pose_.position.x, pose_.position.y, pose_.position.z,
- pose_.orientation.x, pose_.orientation.y, pose_.orientation.z, pose_.orientation.w);
+ pose_des.position.x, pose_des.position.y, pose_des.position.z,
+ pose_des.orientation.x, pose_des.orientation.y, pose_des.orientation.z, pose_des.orientation.w);
auto goal = MoveToPoseAction::Goal();
RCLCPP_INFO(_node->get_logger(), "Send goal to robot [%s]", robot_name_.c_str());
goal.robot_name = robot_name_;
- goal.target_pose = pose_;
+ goal.target_pose = pose_des;
goal.end_effector_acceleration = 1.0;
goal.end_effector_velocity = 1.0;
@@ -45,13 +56,16 @@ class MoveToPose : public BtAction
{
return providedBasicPorts({
InputPort("robot_name"),
- InputPort("pose")
+ InputPort("pose")
});
}
private:
std::string robot_name_;
- geometry_msgs::msg::Pose pose_;
+ std::string pose_;
+ geometry_msgs::msg::Pose pose_des;
+ std::map Poses;
+
}; // class MoveToPose
diff --git a/rbs_simulation/CMakeLists.txt b/rbs_simulation/CMakeLists.txt
new file mode 100644
index 0000000..9d3931d
--- /dev/null
+++ b/rbs_simulation/CMakeLists.txt
@@ -0,0 +1,31 @@
+cmake_minimum_required(VERSION 3.8)
+project(rbs_simulation)
+
+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)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package( REQUIRED)
+
+install(
+ DIRECTORY launch
+ DESTINATION share/${PROJECT_NAME}
+)
+
+if(BUILD_TESTING)
+ find_package(ament_lint_auto REQUIRED)
+ # the following line skips the linter which checks for copyrights
+ # comment the line when a copyright and license is added to all source files
+ set(ament_cmake_copyright_FOUND TRUE)
+ # the following line skips cpplint (only works in a git repo)
+ # comment the line when this package is in a git repo and when
+ # a copyright and license is added to all source files
+ set(ament_cmake_cpplint_FOUND TRUE)
+ ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()
diff --git a/rbs_simulation/launch/rbs_simulation.launch.py b/rbs_simulation/launch/rbs_simulation.launch.py
new file mode 100644
index 0000000..2d4033c
--- /dev/null
+++ b/rbs_simulation/launch/rbs_simulation.launch.py
@@ -0,0 +1,389 @@
+import os
+from launch import LaunchDescription
+from launch.actions import (
+ DeclareLaunchArgument,
+ IncludeLaunchDescription,
+ OpaqueFunction,
+ RegisterEventHandler,
+)
+from ament_index_python.packages import get_package_share_directory, get_package_share_path
+from launch.conditions import IfCondition, UnlessCondition
+from launch.event_handlers import OnProcessExit
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+from ur_moveit_config.launch_common import load_yaml
+
+
+def generate_launch_description():
+ declared_arguments = []
+ # UR specific arguments
+ declared_arguments.append(
+ DeclareLaunchArgument(
+ "rbs_robot_type",
+ description="Type of robot by name",
+ choices=["ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e"],
+ default_value="ur5e",
+ )
+ )
+ declared_arguments.append(
+ DeclareLaunchArgument(
+ "safety_limits",
+ default_value="true",
+ description="Enables the safety limits controller if true.",
+ )
+ )
+ declared_arguments.append(
+ DeclareLaunchArgument(
+ "safety_pos_margin",
+ default_value="0.15",
+ description="The margin to lower and upper limits in the safety controller.",
+ )
+ )
+ declared_arguments.append(
+ DeclareLaunchArgument(
+ "safety_k_position",
+ default_value="20",
+ description="k-position factor in the safety controller.",
+ )
+ )
+ # General arguments
+ declared_arguments.append(
+ DeclareLaunchArgument(
+ "runtime_config_package",
+ default_value="ur_moveit_config",
+ description='Package with the controller\'s configuration in "config" folder. \
+ Usually the argument is not set, it enables use of a custom setup.',
+ )
+ )
+ declared_arguments.append(
+ DeclareLaunchArgument(
+ "controllers_file",
+ default_value="ur_controllers.yaml",
+ description="YAML file with the controllers configuration.",
+ )
+ )
+ declared_arguments.append(
+ DeclareLaunchArgument(
+ "description_package",
+ default_value="ur_description",
+ description="Description package with robot URDF/XACRO files. Usually the argument \
+ is not set, it enables use of a custom description.",
+ )
+ )
+ declared_arguments.append(
+ DeclareLaunchArgument(
+ "description_file",
+ default_value="ur.urdf.xacro",
+ description="URDF/XACRO description file with the robot.",
+ )
+ )
+ declared_arguments.append(
+ DeclareLaunchArgument(
+ "prefix",
+ default_value='""',
+ description="Prefix of the joint names, useful for \
+ multi-robot setup. If changed than also joint names in the controllers' configuration \
+ have to be updated.",
+ )
+ )
+ declared_arguments.append(
+ DeclareLaunchArgument(
+ "start_joint_controller",
+ default_value="true",
+ description="Enable headless mode for robot control",
+ )
+ )
+ declared_arguments.append(
+ DeclareLaunchArgument(
+ "initial_joint_controller",
+ default_value="joint_trajectory_controller",
+ description="Robot controller to start.",
+ )
+ )
+
+ declared_arguments.append(
+ DeclareLaunchArgument(
+ "moveit_config_package",
+ default_value="ur_moveit_config",
+ description="MoveIt config package with robot SRDF/XACRO files. Usually the argument \
+ is not set, it enables use of a custom moveit config.",
+ )
+ )
+ declared_arguments.append(
+ DeclareLaunchArgument(
+ "moveit_config_file",
+ default_value="ur.srdf.xacro",
+ description="MoveIt SRDF/XACRO description file with the robot.",
+ )
+ )
+ declared_arguments.append(
+ DeclareLaunchArgument(
+ "use_sim_time",
+ default_value="true",
+ description="Make MoveIt to use simulation time. This is needed for the trajectory planing in simulation.",
+ )
+ )
+
+ declared_arguments.append(
+ DeclareLaunchArgument("launch_rviz", default_value="true", description="Launch RViz?")
+ )
+
+ # Initialize Arguments
+ # TODO тут всё переименовать и сделать боеле универсальным как под нашего робото так и под UR чтобы запускалось одинаково
+ rbs_robot_type = LaunchConfiguration("rbs_robot_type")
+ safety_limits = LaunchConfiguration("safety_limits")
+ safety_pos_margin = LaunchConfiguration("safety_pos_margin")
+ safety_k_position = LaunchConfiguration("safety_k_position")
+ # General arguments
+ runtime_config_package = LaunchConfiguration("runtime_config_package")
+ controllers_file = LaunchConfiguration("controllers_file")
+ description_package = LaunchConfiguration("description_package")
+ description_file = LaunchConfiguration("description_file")
+ prefix = LaunchConfiguration("prefix")
+ start_joint_controller = LaunchConfiguration("start_joint_controller")
+ initial_joint_controller = LaunchConfiguration("initial_joint_controller")
+ launch_rviz = LaunchConfiguration("launch_rviz")
+ moveit_config_package = LaunchConfiguration("moveit_config_package")
+ moveit_config_file = LaunchConfiguration("moveit_config_file")
+ use_sim_time = LaunchConfiguration("use_sim_time")
+
+ initial_joint_controllers = PathJoinSubstitution(
+ [FindPackageShare(runtime_config_package), "config", controllers_file]
+ )
+
+ rviz_config_file = PathJoinSubstitution(
+ [FindPackageShare(moveit_config_package), "rviz", "view_robot.rviz"]
+ )
+
+ world_config_file = PathJoinSubstitution(
+ [FindPackageShare("sdf_models"), "worlds", "empty_world.sdf"] # TODO тут пакет извне должен задаваться
+ )
+
+ print(world_config_file)
+ robot_description_content = Command(
+ [
+ PathJoinSubstitution([FindExecutable(name="xacro")]),
+ " ",
+ PathJoinSubstitution(
+ [FindPackageShare(description_package), "urdf", description_file]
+ ),
+ " ",
+ "safety_limits:=",
+ safety_limits,
+ " ",
+ "safety_pos_margin:=",
+ safety_pos_margin,
+ " ",
+ "safety_k_position:=",
+ safety_k_position,
+ " ",
+ "name:=",
+ "ur",
+ " ",
+ "ur_type:=", # TODO сделать более универсальным
+ rbs_robot_type,
+ " ",
+ "prefix:=",
+ prefix,
+ " ",
+ "sim_ignition:=true",
+ " ",
+ "simulation_controllers:=",
+ initial_joint_controllers,
+ ]
+ )
+ robot_description = {"robot_description": robot_description_content}
+
+ robot_state_publisher_node = Node(
+ package="robot_state_publisher",
+ executable="robot_state_publisher",
+ output="both",
+ parameters=[{"use_sim_time": True}, robot_description],
+ )
+
+ joint_state_broadcaster_spawner = Node(
+ package="controller_manager",
+ executable="spawner",
+ arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"],
+ )
+
+ # There may be other controllers of the joints, but this is the initially-started one
+ initial_joint_controller_spawner_started = Node(
+ package="controller_manager",
+ executable="spawner",
+ arguments=[initial_joint_controller, "--controller-manager", "/controller_manager"],
+ condition=IfCondition(start_joint_controller),
+ )
+ initial_joint_controller_spawner_stopped = Node(
+ package="controller_manager",
+ executable="spawner",
+ arguments=[initial_joint_controller, "--controller-manager", "/controller_manager", "--stopped"],
+ condition=UnlessCondition(start_joint_controller),
+ )
+
+ # Gazebo nodes
+ gazebo = IncludeLaunchDescription(
+ PythonLaunchDescriptionSource(
+ os.path.join(get_package_share_path('ros_ign_gazebo'), 'launch', 'ign_gazebo.launch.py')),
+ launch_arguments={'ign_args': ['-r'," ", world_config_file]}.items(),
+ )
+ # Spawn robot
+ gazebo_spawn_robot = Node(package='ros_ign_gazebo', executable='create',
+ arguments=[
+ '-name', rbs_robot_type,
+ '-x', '0.0',
+ '-z', '0.0',
+ '-y', '0.0',
+ '-topic', '/robot_description'],
+ output='screen')
+
+ # MoveIt Configuration
+ robot_description_semantic_content = Command(
+ [
+ PathJoinSubstitution([FindExecutable(name="xacro")]),
+ " ",
+ PathJoinSubstitution(
+ [FindPackageShare(moveit_config_package), "srdf", moveit_config_file]
+ ),
+ " ",
+ "name:=",
+ "ur",
+ " ",
+ "prefix:=",
+ prefix,
+ " ",
+ ]
+ )
+ robot_description_semantic = {"robot_description_semantic": robot_description_semantic_content}
+
+ robot_description_kinematics = PathJoinSubstitution(
+ [FindPackageShare(moveit_config_package), "config", "kinematics.yaml"]
+ )
+
+ # robot_description_planning = {
+ # "robot_description_planning": load_yaml_abs(str(joint_limit_params.perform(context)))
+ # }
+
+ # Planning Configuration
+ ompl_planning_pipeline_config = {
+ "move_group": {
+ "planning_plugin": "ompl_interface/OMPLPlanner",
+ "request_adapters": """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""",
+ "start_state_max_bounds_error": 0.1,
+ }
+ }
+ ompl_planning_yaml = load_yaml("ur_moveit_config", "config/ompl_planning.yaml") # TODO сделать извне
+ ompl_planning_pipeline_config["move_group"].update(ompl_planning_yaml)
+
+ # Trajectory Execution Configuration
+ 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_simple_controller_manager": controllers_yaml,
+ "moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager",
+ }
+
+ trajectory_execution = {
+ "moveit_manage_controllers": True,
+ "trajectory_execution.allowed_execution_duration_scaling": 1.2,
+ "trajectory_execution.allowed_goal_duration_margin": 0.5,
+ "trajectory_execution.allowed_start_tolerance": 0.01,
+ }
+
+ planning_scene_monitor_parameters = {
+ "publish_planning_scene": True,
+ "publish_geometry_updates": True,
+ "publish_state_updates": True,
+ "publish_transforms_updates": True,
+ }
+
+ # Start the actual move_group node/action server
+ move_group_node = Node(
+ package="moveit_ros_move_group",
+ executable="move_group",
+ output="screen",
+ name="move_group",
+ parameters=[
+ robot_description,
+ robot_description_semantic,
+ robot_description_kinematics,
+ # robot_description_planning,
+ ompl_planning_pipeline_config,
+ trajectory_execution,
+ moveit_controllers,
+ planning_scene_monitor_parameters,
+ {"use_sim_time": use_sim_time},
+ ],
+ )
+
+ rviz_node = Node(
+ package="rviz2",
+ executable="rviz2",
+ name="rviz2",
+ output="log",
+ arguments=["-d", rviz_config_file],
+ parameters=[
+ robot_description,
+ robot_description_semantic,
+ ompl_planning_pipeline_config,
+ robot_description_kinematics,
+ # robot_description_planning,
+ ],
+ condition=IfCondition(launch_rviz),
+ )
+
+ move_topose_action_server = Node(
+ package="rbs_skill_servers",
+ executable="move_topose_action_server",
+ name="move_to_pose_moveit",
+ parameters=[
+ robot_description,
+ robot_description_semantic,
+ robot_description_kinematics,
+ # robot_description_planning,
+ ompl_planning_pipeline_config,
+ trajectory_execution,
+ moveit_controllers,
+ planning_scene_monitor_parameters,
+ {"use_sim_time": use_sim_time},
+ ]
+ )
+
+ move_cartesian_path_action_server = Node(
+ package="rbs_skill_servers",
+ executable="move_cartesian_path_action_server",
+ name="move_cartesian_path_action_server",
+ parameters=[
+ robot_description,
+ robot_description_semantic,
+ robot_description_kinematics,
+ # robot_description_planning,
+ ompl_planning_pipeline_config,
+ trajectory_execution,
+ moveit_controllers,
+ planning_scene_monitor_parameters,
+ {"use_sim_time": use_sim_time},
+ ]
+ )
+
+ nodes_to_start = [
+ robot_state_publisher_node,
+ joint_state_broadcaster_spawner,
+ rviz_node,
+ initial_joint_controller_spawner_stopped,
+ initial_joint_controller_spawner_started,
+ gazebo,
+ gazebo_spawn_robot,
+ move_group_node,
+ move_topose_action_server,
+ move_cartesian_path_action_server
+ ]
+
+ return LaunchDescription(declared_arguments + nodes_to_start)
\ No newline at end of file
diff --git a/robossembler_scene_monitor/scene_monitor/package.xml b/rbs_simulation/package.xml
similarity index 69%
rename from robossembler_scene_monitor/scene_monitor/package.xml
rename to rbs_simulation/package.xml
index 32f19d8..bb4de1b 100644
--- a/robossembler_scene_monitor/scene_monitor/package.xml
+++ b/rbs_simulation/package.xml
@@ -1,10 +1,10 @@
- robossembler_scene_monitor
+ rbs_simulation
0.0.0
TODO: Package description
- Splinter1984
+ root
TODO: License declaration
ament_cmake
@@ -13,11 +13,7 @@
tf2
tf2_ros
std_msgs
-
- gazebo_msgs
geometry_msgs
-
- scene_monitor_interfaces
nlohmann_json
ament_lint_auto
@@ -26,4 +22,4 @@
ament_cmake
-
\ No newline at end of file
+
diff --git a/robossembler_interfaces/CMakeLists.txt b/rbs_skill_interfaces/CMakeLists.txt
similarity index 50%
rename from robossembler_interfaces/CMakeLists.txt
rename to rbs_skill_interfaces/CMakeLists.txt
index a13068d..d964d51 100644
--- a/robossembler_interfaces/CMakeLists.txt
+++ b/rbs_skill_interfaces/CMakeLists.txt
@@ -1,15 +1,5 @@
-cmake_minimum_required(VERSION 3.5)
-project(robossembler_interfaces)
-
-# Default to C99
-if(NOT CMAKE_C_STANDARD)
- set(CMAKE_C_STANDARD 99)
-endif()
-
-# Default to C++14
-if(NOT CMAKE_CXX_STANDARD)
- set(CMAKE_CXX_STANDARD 14)
-endif()
+cmake_minimum_required(VERSION 3.8)
+project(rbs_skill_interfaces)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
@@ -19,6 +9,9 @@ endif()
find_package(ament_cmake REQUIRED)
find_package(rosidl_default_generators REQUIRED)
find_package(geometry_msgs REQUIRED)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package( REQUIRED)
rosidl_generate_interfaces(${PROJECT_NAME}
"action/MoveitSendPose.action"
@@ -27,11 +20,20 @@ rosidl_generate_interfaces(${PROJECT_NAME}
"msg/PropertyValuePair.msg"
"msg/ActionFeedbackStatusConstants.msg"
"msg/ActionResultStatusConstants.msg"
- DEPENDENCIES geometry_msgs std_msgs
+ "srv/BtInit.srv"
+ DEPENDENCIES geometry_msgs
)
+
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
+ # the following line skips the linter which checks for copyrights
+ # comment the line when a copyright and license is added to all source files
+ set(ament_cmake_copyright_FOUND TRUE)
+ # the following line skips cpplint (only works in a git repo)
+ # comment the line when this package is in a git repo and when
+ # a copyright and license is added to all source files
+ set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
endif()
diff --git a/rbs_skill_interfaces/LICENSE b/rbs_skill_interfaces/LICENSE
new file mode 100644
index 0000000..d645695
--- /dev/null
+++ b/rbs_skill_interfaces/LICENSE
@@ -0,0 +1,202 @@
+
+ Apache License
+ Version 2.0, January 2004
+ http://www.apache.org/licenses/
+
+ TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
+
+ 1. Definitions.
+
+ "License" shall mean the terms and conditions for use, reproduction,
+ and distribution as defined by Sections 1 through 9 of this document.
+
+ "Licensor" shall mean the copyright owner or entity authorized by
+ the copyright owner that is granting the License.
+
+ "Legal Entity" shall mean the union of the acting entity and all
+ other entities that control, are controlled by, or are under common
+ control with that entity. For the purposes of this definition,
+ "control" means (i) the power, direct or indirect, to cause the
+ direction or management of such entity, whether by contract or
+ otherwise, or (ii) ownership of fifty percent (50%) or more of the
+ outstanding shares, or (iii) beneficial ownership of such entity.
+
+ "You" (or "Your") shall mean an individual or Legal Entity
+ exercising permissions granted by this License.
+
+ "Source" form shall mean the preferred form for making modifications,
+ including but not limited to software source code, documentation
+ source, and configuration files.
+
+ "Object" form shall mean any form resulting from mechanical
+ transformation or translation of a Source form, including but
+ not limited to compiled object code, generated documentation,
+ and conversions to other media types.
+
+ "Work" shall mean the work of authorship, whether in Source or
+ Object form, made available under the License, as indicated by a
+ copyright notice that is included in or attached to the work
+ (an example is provided in the Appendix below).
+
+ "Derivative Works" shall mean any work, whether in Source or Object
+ form, that is based on (or derived from) the Work and for which the
+ editorial revisions, annotations, elaborations, or other modifications
+ represent, as a whole, an original work of authorship. For the purposes
+ of this License, Derivative Works shall not include works that remain
+ separable from, or merely link (or bind by name) to the interfaces of,
+ the Work and Derivative Works thereof.
+
+ "Contribution" shall mean any work of authorship, including
+ the original version of the Work and any modifications or additions
+ to that Work or Derivative Works thereof, that is intentionally
+ submitted to Licensor for inclusion in the Work by the copyright owner
+ or by an individual or Legal Entity authorized to submit on behalf of
+ the copyright owner. For the purposes of this definition, "submitted"
+ means any form of electronic, verbal, or written communication sent
+ to the Licensor or its representatives, including but not limited to
+ communication on electronic mailing lists, source code control systems,
+ and issue tracking systems that are managed by, or on behalf of, the
+ Licensor for the purpose of discussing and improving the Work, but
+ excluding communication that is conspicuously marked or otherwise
+ designated in writing by the copyright owner as "Not a Contribution."
+
+ "Contributor" shall mean Licensor and any individual or Legal Entity
+ on behalf of whom a Contribution has been received by Licensor and
+ subsequently incorporated within the Work.
+
+ 2. Grant of Copyright License. Subject to the terms and conditions of
+ this License, each Contributor hereby grants to You a perpetual,
+ worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+ copyright license to reproduce, prepare Derivative Works of,
+ publicly display, publicly perform, sublicense, and distribute the
+ Work and such Derivative Works in Source or Object form.
+
+ 3. Grant of Patent License. Subject to the terms and conditions of
+ this License, each Contributor hereby grants to You a perpetual,
+ worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+ (except as stated in this section) patent license to make, have made,
+ use, offer to sell, sell, import, and otherwise transfer the Work,
+ where such license applies only to those patent claims licensable
+ by such Contributor that are necessarily infringed by their
+ Contribution(s) alone or by combination of their Contribution(s)
+ with the Work to which such Contribution(s) was submitted. If You
+ institute patent litigation against any entity (including a
+ cross-claim or counterclaim in a lawsuit) alleging that the Work
+ or a Contribution incorporated within the Work constitutes direct
+ or contributory patent infringement, then any patent licenses
+ granted to You under this License for that Work shall terminate
+ as of the date such litigation is filed.
+
+ 4. Redistribution. You may reproduce and distribute copies of the
+ Work or Derivative Works thereof in any medium, with or without
+ modifications, and in Source or Object form, provided that You
+ meet the following conditions:
+
+ (a) You must give any other recipients of the Work or
+ Derivative Works a copy of this License; and
+
+ (b) You must cause any modified files to carry prominent notices
+ stating that You changed the files; and
+
+ (c) You must retain, in the Source form of any Derivative Works
+ that You distribute, all copyright, patent, trademark, and
+ attribution notices from the Source form of the Work,
+ excluding those notices that do not pertain to any part of
+ the Derivative Works; and
+
+ (d) If the Work includes a "NOTICE" text file as part of its
+ distribution, then any Derivative Works that You distribute must
+ include a readable copy of the attribution notices contained
+ within such NOTICE file, excluding those notices that do not
+ pertain to any part of the Derivative Works, in at least one
+ of the following places: within a NOTICE text file distributed
+ as part of the Derivative Works; within the Source form or
+ documentation, if provided along with the Derivative Works; or,
+ within a display generated by the Derivative Works, if and
+ wherever such third-party notices normally appear. The contents
+ of the NOTICE file are for informational purposes only and
+ do not modify the License. You may add Your own attribution
+ notices within Derivative Works that You distribute, alongside
+ or as an addendum to the NOTICE text from the Work, provided
+ that such additional attribution notices cannot be construed
+ as modifying the License.
+
+ You may add Your own copyright statement to Your modifications and
+ may provide additional or different license terms and conditions
+ for use, reproduction, or distribution of Your modifications, or
+ for any such Derivative Works as a whole, provided Your use,
+ reproduction, and distribution of the Work otherwise complies with
+ the conditions stated in this License.
+
+ 5. Submission of Contributions. Unless You explicitly state otherwise,
+ any Contribution intentionally submitted for inclusion in the Work
+ by You to the Licensor shall be under the terms and conditions of
+ this License, without any additional terms or conditions.
+ Notwithstanding the above, nothing herein shall supersede or modify
+ the terms of any separate license agreement you may have executed
+ with Licensor regarding such Contributions.
+
+ 6. Trademarks. This License does not grant permission to use the trade
+ names, trademarks, service marks, or product names of the Licensor,
+ except as required for reasonable and customary use in describing the
+ origin of the Work and reproducing the content of the NOTICE file.
+
+ 7. Disclaimer of Warranty. Unless required by applicable law or
+ agreed to in writing, Licensor provides the Work (and each
+ Contributor provides its Contributions) on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
+ implied, including, without limitation, any warranties or conditions
+ of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
+ PARTICULAR PURPOSE. You are solely responsible for determining the
+ appropriateness of using or redistributing the Work and assume any
+ risks associated with Your exercise of permissions under this License.
+
+ 8. Limitation of Liability. In no event and under no legal theory,
+ whether in tort (including negligence), contract, or otherwise,
+ unless required by applicable law (such as deliberate and grossly
+ negligent acts) or agreed to in writing, shall any Contributor be
+ liable to You for damages, including any direct, indirect, special,
+ incidental, or consequential damages of any character arising as a
+ result of this License or out of the use or inability to use the
+ Work (including but not limited to damages for loss of goodwill,
+ work stoppage, computer failure or malfunction, or any and all
+ other commercial damages or losses), even if such Contributor
+ has been advised of the possibility of such damages.
+
+ 9. Accepting Warranty or Additional Liability. While redistributing
+ the Work or Derivative Works thereof, You may choose to offer,
+ and charge a fee for, acceptance of support, warranty, indemnity,
+ or other liability obligations and/or rights consistent with this
+ License. However, in accepting such obligations, You may act only
+ on Your own behalf and on Your sole responsibility, not on behalf
+ of any other Contributor, and only if You agree to indemnify,
+ defend, and hold each Contributor harmless for any liability
+ incurred by, or claims asserted against, such Contributor by reason
+ of your accepting any such warranty or additional liability.
+
+ END OF TERMS AND CONDITIONS
+
+ APPENDIX: How to apply the Apache License to your work.
+
+ To apply the Apache License to your work, attach the following
+ boilerplate notice, with the fields enclosed by brackets "[]"
+ replaced with your own identifying information. (Don't include
+ the brackets!) The text should be enclosed in the appropriate
+ comment syntax for the file format. We also recommend that a
+ file or class name and description of purpose be included on the
+ same "printed page" as the copyright notice for easier
+ identification within third-party archives.
+
+ Copyright [yyyy] [name of copyright owner]
+
+ Licensed under the Apache License, Version 2.0 (the "License");
+ you may not use this file except in compliance with the License.
+ You may obtain a copy of the License at
+
+ http://www.apache.org/licenses/LICENSE-2.0
+
+ Unless required by applicable law or agreed to in writing, software
+ distributed under the License is distributed on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ See the License for the specific language governing permissions and
+ limitations under the License.
diff --git a/robossembler_interfaces/action/GripperCommand.action b/rbs_skill_interfaces/action/GripperCommand.action
similarity index 100%
rename from robossembler_interfaces/action/GripperCommand.action
rename to rbs_skill_interfaces/action/GripperCommand.action
diff --git a/robossembler_interfaces/action/MoveitSendJointStates.action b/rbs_skill_interfaces/action/MoveitSendJointStates.action
similarity index 86%
rename from robossembler_interfaces/action/MoveitSendJointStates.action
rename to rbs_skill_interfaces/action/MoveitSendJointStates.action
index 6187200..db729dd 100644
--- a/robossembler_interfaces/action/MoveitSendJointStates.action
+++ b/rbs_skill_interfaces/action/MoveitSendJointStates.action
@@ -2,7 +2,7 @@
#goal definition
string robot_name
-robossembler_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
float32 joints_velocity_scaling_factor
float32 joints_acceleration_scaling_factor
diff --git a/robossembler_interfaces/action/MoveitSendPose.action b/rbs_skill_interfaces/action/MoveitSendPose.action
similarity index 100%
rename from robossembler_interfaces/action/MoveitSendPose.action
rename to rbs_skill_interfaces/action/MoveitSendPose.action
diff --git a/robossembler_interfaces/msg/ActionFeedbackStatusConstants.msg b/rbs_skill_interfaces/msg/ActionFeedbackStatusConstants.msg
similarity index 100%
rename from robossembler_interfaces/msg/ActionFeedbackStatusConstants.msg
rename to rbs_skill_interfaces/msg/ActionFeedbackStatusConstants.msg
diff --git a/robossembler_interfaces/msg/ActionResultStatusConstants.msg b/rbs_skill_interfaces/msg/ActionResultStatusConstants.msg
similarity index 100%
rename from robossembler_interfaces/msg/ActionResultStatusConstants.msg
rename to rbs_skill_interfaces/msg/ActionResultStatusConstants.msg
diff --git a/robossembler_interfaces/msg/PropertyValuePair.msg b/rbs_skill_interfaces/msg/PropertyValuePair.msg
similarity index 100%
rename from robossembler_interfaces/msg/PropertyValuePair.msg
rename to rbs_skill_interfaces/msg/PropertyValuePair.msg
diff --git a/robossembler_interfaces/package.xml b/rbs_skill_interfaces/package.xml
similarity index 81%
rename from robossembler_interfaces/package.xml
rename to rbs_skill_interfaces/package.xml
index 7c6e61c..2f62f74 100644
--- a/robossembler_interfaces/package.xml
+++ b/rbs_skill_interfaces/package.xml
@@ -1,21 +1,23 @@
- robossembler_interfaces
+ rbs_skill_interfaces
0.0.0
TODO: Package description
bill-finger
- TODO: License declaration
+ Apache-2.0
ament_cmake
rosidl_default_generators
-
rosidl_default_runtime
+ rosidl_interface_packages
ament_lint_auto
ament_lint_common
- rosidl_interface_packages
+
+ ament_lint_auto
+ ament_lint_common
ament_cmake
diff --git a/rbs_skill_interfaces/srv/BtInit.srv b/rbs_skill_interfaces/srv/BtInit.srv
new file mode 100644
index 0000000..163a237
--- /dev/null
+++ b/rbs_skill_interfaces/srv/BtInit.srv
@@ -0,0 +1,3 @@
+geometry_msgs/Pose[] target_poses
+---
+string result
\ No newline at end of file
diff --git a/robossembler_servers/CMakeLists.txt b/rbs_skill_servers/CMakeLists.txt
similarity index 70%
rename from robossembler_servers/CMakeLists.txt
rename to rbs_skill_servers/CMakeLists.txt
index d3fbc17..72e10b0 100644
--- a/robossembler_servers/CMakeLists.txt
+++ b/rbs_skill_servers/CMakeLists.txt
@@ -1,5 +1,5 @@
-cmake_minimum_required(VERSION 3.5)
-project(robossembler_servers)
+cmake_minimum_required(VERSION 3.8)
+project(rbs_skill_servers)
# Default to C99
if(NOT CMAKE_C_STANDARD)
@@ -26,9 +26,8 @@ find_package(moveit_ros_planning_interface REQUIRED)
find_package(moveit_msgs REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(geometry_msgs REQUIRED)
-find_package(robossembler_interfaces REQUIRED)
+find_package(rbs_skill_interfaces REQUIRED)
find_package(rmw REQUIRED)
-find_package(gazebo_ros_link_attacher REQUIRED)
set(deps
rclcpp
@@ -40,8 +39,7 @@ set(deps
geometry_msgs
tf2_ros
rclcpp_components
- robossembler_interfaces
- gazebo_ros_link_attacher
+ rbs_skill_interfaces
)
#
# include_directories(include)
@@ -55,13 +53,13 @@ if(BUILD_TESTING)
ament_lint_auto_find_test_dependencies()
endif()
-add_library(gripper_cmd_action_server SHARED src/gripper_cmd.cpp)
-ament_target_dependencies(gripper_cmd_action_server ${deps})
-target_include_directories(gripper_cmd_action_server PRIVATE
- $
- $)
-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)
+# add_library(gripper_cmd_action_server SHARED src/gripper_cmd.cpp)
+# ament_target_dependencies(gripper_cmd_action_server ${deps})
+# target_include_directories(gripper_cmd_action_server PRIVATE
+# $
+# $)
+# 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)
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})
@@ -77,7 +75,7 @@ install(
move_topose_action_server
move_to_joint_states_action_server
move_cartesian_path_action_server
- gripper_cmd_action_server
+ #gripper_cmd_action_server
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION lib/${PROJECT_NAME}
diff --git a/rbs_skill_servers/LICENSE b/rbs_skill_servers/LICENSE
new file mode 100644
index 0000000..d645695
--- /dev/null
+++ b/rbs_skill_servers/LICENSE
@@ -0,0 +1,202 @@
+
+ Apache License
+ Version 2.0, January 2004
+ http://www.apache.org/licenses/
+
+ TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
+
+ 1. Definitions.
+
+ "License" shall mean the terms and conditions for use, reproduction,
+ and distribution as defined by Sections 1 through 9 of this document.
+
+ "Licensor" shall mean the copyright owner or entity authorized by
+ the copyright owner that is granting the License.
+
+ "Legal Entity" shall mean the union of the acting entity and all
+ other entities that control, are controlled by, or are under common
+ control with that entity. For the purposes of this definition,
+ "control" means (i) the power, direct or indirect, to cause the
+ direction or management of such entity, whether by contract or
+ otherwise, or (ii) ownership of fifty percent (50%) or more of the
+ outstanding shares, or (iii) beneficial ownership of such entity.
+
+ "You" (or "Your") shall mean an individual or Legal Entity
+ exercising permissions granted by this License.
+
+ "Source" form shall mean the preferred form for making modifications,
+ including but not limited to software source code, documentation
+ source, and configuration files.
+
+ "Object" form shall mean any form resulting from mechanical
+ transformation or translation of a Source form, including but
+ not limited to compiled object code, generated documentation,
+ and conversions to other media types.
+
+ "Work" shall mean the work of authorship, whether in Source or
+ Object form, made available under the License, as indicated by a
+ copyright notice that is included in or attached to the work
+ (an example is provided in the Appendix below).
+
+ "Derivative Works" shall mean any work, whether in Source or Object
+ form, that is based on (or derived from) the Work and for which the
+ editorial revisions, annotations, elaborations, or other modifications
+ represent, as a whole, an original work of authorship. For the purposes
+ of this License, Derivative Works shall not include works that remain
+ separable from, or merely link (or bind by name) to the interfaces of,
+ the Work and Derivative Works thereof.
+
+ "Contribution" shall mean any work of authorship, including
+ the original version of the Work and any modifications or additions
+ to that Work or Derivative Works thereof, that is intentionally
+ submitted to Licensor for inclusion in the Work by the copyright owner
+ or by an individual or Legal Entity authorized to submit on behalf of
+ the copyright owner. For the purposes of this definition, "submitted"
+ means any form of electronic, verbal, or written communication sent
+ to the Licensor or its representatives, including but not limited to
+ communication on electronic mailing lists, source code control systems,
+ and issue tracking systems that are managed by, or on behalf of, the
+ Licensor for the purpose of discussing and improving the Work, but
+ excluding communication that is conspicuously marked or otherwise
+ designated in writing by the copyright owner as "Not a Contribution."
+
+ "Contributor" shall mean Licensor and any individual or Legal Entity
+ on behalf of whom a Contribution has been received by Licensor and
+ subsequently incorporated within the Work.
+
+ 2. Grant of Copyright License. Subject to the terms and conditions of
+ this License, each Contributor hereby grants to You a perpetual,
+ worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+ copyright license to reproduce, prepare Derivative Works of,
+ publicly display, publicly perform, sublicense, and distribute the
+ Work and such Derivative Works in Source or Object form.
+
+ 3. Grant of Patent License. Subject to the terms and conditions of
+ this License, each Contributor hereby grants to You a perpetual,
+ worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+ (except as stated in this section) patent license to make, have made,
+ use, offer to sell, sell, import, and otherwise transfer the Work,
+ where such license applies only to those patent claims licensable
+ by such Contributor that are necessarily infringed by their
+ Contribution(s) alone or by combination of their Contribution(s)
+ with the Work to which such Contribution(s) was submitted. If You
+ institute patent litigation against any entity (including a
+ cross-claim or counterclaim in a lawsuit) alleging that the Work
+ or a Contribution incorporated within the Work constitutes direct
+ or contributory patent infringement, then any patent licenses
+ granted to You under this License for that Work shall terminate
+ as of the date such litigation is filed.
+
+ 4. Redistribution. You may reproduce and distribute copies of the
+ Work or Derivative Works thereof in any medium, with or without
+ modifications, and in Source or Object form, provided that You
+ meet the following conditions:
+
+ (a) You must give any other recipients of the Work or
+ Derivative Works a copy of this License; and
+
+ (b) You must cause any modified files to carry prominent notices
+ stating that You changed the files; and
+
+ (c) You must retain, in the Source form of any Derivative Works
+ that You distribute, all copyright, patent, trademark, and
+ attribution notices from the Source form of the Work,
+ excluding those notices that do not pertain to any part of
+ the Derivative Works; and
+
+ (d) If the Work includes a "NOTICE" text file as part of its
+ distribution, then any Derivative Works that You distribute must
+ include a readable copy of the attribution notices contained
+ within such NOTICE file, excluding those notices that do not
+ pertain to any part of the Derivative Works, in at least one
+ of the following places: within a NOTICE text file distributed
+ as part of the Derivative Works; within the Source form or
+ documentation, if provided along with the Derivative Works; or,
+ within a display generated by the Derivative Works, if and
+ wherever such third-party notices normally appear. The contents
+ of the NOTICE file are for informational purposes only and
+ do not modify the License. You may add Your own attribution
+ notices within Derivative Works that You distribute, alongside
+ or as an addendum to the NOTICE text from the Work, provided
+ that such additional attribution notices cannot be construed
+ as modifying the License.
+
+ You may add Your own copyright statement to Your modifications and
+ may provide additional or different license terms and conditions
+ for use, reproduction, or distribution of Your modifications, or
+ for any such Derivative Works as a whole, provided Your use,
+ reproduction, and distribution of the Work otherwise complies with
+ the conditions stated in this License.
+
+ 5. Submission of Contributions. Unless You explicitly state otherwise,
+ any Contribution intentionally submitted for inclusion in the Work
+ by You to the Licensor shall be under the terms and conditions of
+ this License, without any additional terms or conditions.
+ Notwithstanding the above, nothing herein shall supersede or modify
+ the terms of any separate license agreement you may have executed
+ with Licensor regarding such Contributions.
+
+ 6. Trademarks. This License does not grant permission to use the trade
+ names, trademarks, service marks, or product names of the Licensor,
+ except as required for reasonable and customary use in describing the
+ origin of the Work and reproducing the content of the NOTICE file.
+
+ 7. Disclaimer of Warranty. Unless required by applicable law or
+ agreed to in writing, Licensor provides the Work (and each
+ Contributor provides its Contributions) on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
+ implied, including, without limitation, any warranties or conditions
+ of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
+ PARTICULAR PURPOSE. You are solely responsible for determining the
+ appropriateness of using or redistributing the Work and assume any
+ risks associated with Your exercise of permissions under this License.
+
+ 8. Limitation of Liability. In no event and under no legal theory,
+ whether in tort (including negligence), contract, or otherwise,
+ unless required by applicable law (such as deliberate and grossly
+ negligent acts) or agreed to in writing, shall any Contributor be
+ liable to You for damages, including any direct, indirect, special,
+ incidental, or consequential damages of any character arising as a
+ result of this License or out of the use or inability to use the
+ Work (including but not limited to damages for loss of goodwill,
+ work stoppage, computer failure or malfunction, or any and all
+ other commercial damages or losses), even if such Contributor
+ has been advised of the possibility of such damages.
+
+ 9. Accepting Warranty or Additional Liability. While redistributing
+ the Work or Derivative Works thereof, You may choose to offer,
+ and charge a fee for, acceptance of support, warranty, indemnity,
+ or other liability obligations and/or rights consistent with this
+ License. However, in accepting such obligations, You may act only
+ on Your own behalf and on Your sole responsibility, not on behalf
+ of any other Contributor, and only if You agree to indemnify,
+ defend, and hold each Contributor harmless for any liability
+ incurred by, or claims asserted against, such Contributor by reason
+ of your accepting any such warranty or additional liability.
+
+ END OF TERMS AND CONDITIONS
+
+ APPENDIX: How to apply the Apache License to your work.
+
+ To apply the Apache License to your work, attach the following
+ boilerplate notice, with the fields enclosed by brackets "[]"
+ replaced with your own identifying information. (Don't include
+ the brackets!) The text should be enclosed in the appropriate
+ comment syntax for the file format. We also recommend that a
+ file or class name and description of purpose be included on the
+ same "printed page" as the copyright notice for easier
+ identification within third-party archives.
+
+ Copyright [yyyy] [name of copyright owner]
+
+ Licensed under the Apache License, Version 2.0 (the "License");
+ you may not use this file except in compliance with the License.
+ You may obtain a copy of the License at
+
+ http://www.apache.org/licenses/LICENSE-2.0
+
+ Unless required by applicable law or agreed to in writing, software
+ distributed under the License is distributed on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ See the License for the specific language governing permissions and
+ limitations under the License.
diff --git a/robossembler_servers/package.xml b/rbs_skill_servers/package.xml
similarity index 86%
rename from robossembler_servers/package.xml
rename to rbs_skill_servers/package.xml
index cb20723..cd813c0 100644
--- a/robossembler_servers/package.xml
+++ b/rbs_skill_servers/package.xml
@@ -1,11 +1,11 @@
- robossembler_servers
+ rbs_skill_servers
0.0.0
TODO: Package description
bill-finger
- TODO: License declaration
+ Apache-2.0
ament_cmake
@@ -18,7 +18,7 @@
rclcpp_action
geometry_msgs
action_msgs
- robossembler_interfaces
+ rbs_skill_interfaces
ament_lint_auto
ament_lint_common
diff --git a/robossembler_servers/src/move_cartesian_path_action_server.cpp b/rbs_skill_servers/src/move_cartesian_path_action_server.cpp
similarity index 95%
rename from robossembler_servers/src/move_cartesian_path_action_server.cpp
rename to rbs_skill_servers/src/move_cartesian_path_action_server.cpp
index 18dcaf1..1c1cae9 100644
--- a/robossembler_servers/src/move_cartesian_path_action_server.cpp
+++ b/rbs_skill_servers/src/move_cartesian_path_action_server.cpp
@@ -8,8 +8,8 @@
// action libs
#include "rclcpp_action/rclcpp_action.hpp"
-#include "robossembler_interfaces/msg/action_feedback_status_constants.hpp"
-#include "robossembler_interfaces/action/moveit_send_pose.hpp"
+#include "rbs_skill_interfaces/msg/action_feedback_status_constants.hpp"
+#include "rbs_skill_interfaces/action/moveit_send_pose.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "geometry_msgs/msg/quaternion.hpp"
@@ -32,13 +32,13 @@
#include "moveit_msgs/msg/robot_trajectory.hpp"
#include "moveit_msgs/action/move_group.hpp"
-namespace robossembler_actions
+namespace rbs_skill_actions
{
class MoveCartesianActionServer : public rclcpp::Node
{
public:
- using MoveitSendPose = robossembler_interfaces::action::MoveitSendPose;
+ using MoveitSendPose = rbs_skill_interfaces::action::MoveitSendPose;
//explicit MoveCartesianActionServer(const rclcpp::Node::SharedPtr& node)
explicit MoveCartesianActionServer(const rclcpp::Node::SharedPtr &node) : Node("move_cartesian_action_server"), node_(node)
@@ -159,7 +159,7 @@ private:
}
}; // class MoveCartesianActionServer
-}// namespace robossembler_actions
+}// namespace rbs_skill_actions
int main(int argc, char ** argv)
{
@@ -168,7 +168,7 @@ int main(int argc, char ** argv)
node_options.automatically_declare_parameters_from_overrides(true);
auto node = rclcpp::Node::make_shared("move_cartesian", "", node_options);
- robossembler_actions::MoveCartesianActionServer server(node);
+ rbs_skill_actions::MoveCartesianActionServer server(node);
std::thread run_server([&server]() {
rclcpp::sleep_for(std::chrono::seconds(3));
server.init();
diff --git a/robossembler_servers/src/move_to_joint_states_action_server.cpp b/rbs_skill_servers/src/move_to_joint_states_action_server.cpp
similarity index 94%
rename from robossembler_servers/src/move_to_joint_states_action_server.cpp
rename to rbs_skill_servers/src/move_to_joint_states_action_server.cpp
index 365bae3..2234ed8 100644
--- a/robossembler_servers/src/move_to_joint_states_action_server.cpp
+++ b/rbs_skill_servers/src/move_to_joint_states_action_server.cpp
@@ -8,7 +8,7 @@
// action libs
#include "rclcpp_action/rclcpp_action.hpp"
-#include "robossembler_interfaces/action/moveit_send_joint_states.hpp"
+#include "rbs_skill_interfaces/action/moveit_send_joint_states.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "geometry_msgs/msg/quaternion.hpp"
@@ -30,13 +30,13 @@
#include "moveit_msgs/msg/robot_trajectory.hpp"
#include "moveit_msgs/action/move_group.hpp"
-namespace robossembler_actions
+namespace rbs_skill_actions
{
class MoveToJointStateActionServer : public rclcpp::Node
{
public:
- using MoveitSendJointStates = robossembler_interfaces::action::MoveitSendJointStates;
+ using MoveitSendJointStates = rbs_skill_interfaces::action::MoveitSendJointStates;
//explicit MoveToPoseActionServer(const rclcpp::Node::SharedPtr& node)
explicit MoveToJointStateActionServer(const rclcpp::Node::SharedPtr &node) : Node("move_to_joint_states_action_server"), node_(node)
@@ -123,7 +123,7 @@ private:
moveit::planning_interface::MoveGroupInterface::Plan plan;
- bool success = (move_group_interface.plan(plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
+ bool success = (move_group_interface.plan(plan) == moveit::core::MoveItErrorCode::SUCCESS);
if(success)
{
RCLCPP_INFO(this->get_logger(), "Planning success");
@@ -145,7 +145,7 @@ private:
}
}; // class MoveToJointStateActionServer
-}// namespace robossembler_actions
+}// namespace rbs_skill_actions
int main(int argc, char ** argv)
{
@@ -154,7 +154,7 @@ int main(int argc, char ** argv)
node_options.automatically_declare_parameters_from_overrides(true);
auto node = rclcpp::Node::make_shared("move_topose", "", node_options);
- robossembler_actions::MoveToJointStateActionServer server(node);
+ rbs_skill_actions::MoveToJointStateActionServer server(node);
std::thread run_server([&server]() {
rclcpp::sleep_for(std::chrono::seconds(3));
server.init();
diff --git a/robossembler_servers/src/move_topose_action_server.cpp b/rbs_skill_servers/src/move_topose_action_server.cpp
similarity index 93%
rename from robossembler_servers/src/move_topose_action_server.cpp
rename to rbs_skill_servers/src/move_topose_action_server.cpp
index 0e5eff2..cba243c 100644
--- a/robossembler_servers/src/move_topose_action_server.cpp
+++ b/rbs_skill_servers/src/move_topose_action_server.cpp
@@ -8,8 +8,8 @@
// action libs
#include "rclcpp_action/rclcpp_action.hpp"
-#include "robossembler_interfaces/msg/action_feedback_status_constants.hpp"
-#include "robossembler_interfaces/action/moveit_send_pose.hpp"
+#include "rbs_skill_interfaces/msg/action_feedback_status_constants.hpp"
+#include "rbs_skill_interfaces/action/moveit_send_pose.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "geometry_msgs/msg/quaternion.hpp"
@@ -32,13 +32,13 @@
#include "moveit_msgs/msg/robot_trajectory.hpp"
#include "moveit_msgs/action/move_group.hpp"
-namespace robossembler_actions
+namespace rbs_skill_actions
{
class MoveToPoseActionServer : public rclcpp::Node
{
public:
- using MoveitSendPose = robossembler_interfaces::action::MoveitSendPose;
+ using MoveitSendPose = rbs_skill_interfaces::action::MoveitSendPose;
//explicit MoveToPoseActionServer(const rclcpp::Node::SharedPtr& node)
explicit MoveToPoseActionServer(const rclcpp::Node::SharedPtr &node) : Node("move_topose_action_server"), node_(node)
@@ -119,7 +119,7 @@ private:
move_group_interface.setMaxAccelerationScalingFactor(goal->end_effector_acceleration);
moveit::planning_interface::MoveGroupInterface::Plan plan;
- bool success = (move_group_interface.plan(plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
+ bool success = (move_group_interface.plan(plan) == moveit::core::MoveItErrorCode::SUCCESS);
if(success)
{
RCLCPP_INFO(this->get_logger(), "Planning success");
@@ -141,7 +141,7 @@ private:
}
}; // class MoveToPoseActionServer
-}// namespace robossembler_actions
+}// namespace rbs_skill_actions
int main(int argc, char ** argv)
{
@@ -150,7 +150,7 @@ int main(int argc, char ** argv)
node_options.automatically_declare_parameters_from_overrides(true);
auto node = rclcpp::Node::make_shared("move_topose", "", node_options);
- robossembler_actions::MoveToPoseActionServer server(node);
+ rbs_skill_actions::MoveToPoseActionServer server(node);
std::thread run_server([&server]() {
rclcpp::sleep_for(std::chrono::seconds(3));
server.init();
diff --git a/robonomics/package.xml b/robonomics/package.xml
index cc40c3c..ad3c944 100644
--- a/robonomics/package.xml
+++ b/robonomics/package.xml
@@ -8,7 +8,7 @@
Apache license 2.0
rclpy
- plansys2_msgs
+
ament_copyright
ament_flake8
diff --git a/robossembler/CMakeLists.txt b/robossembler/CMakeLists.txt
deleted file mode 100644
index fa6e4ee..0000000
--- a/robossembler/CMakeLists.txt
+++ /dev/null
@@ -1,106 +0,0 @@
-cmake_minimum_required(VERSION 3.5)
-project(robossembler)
-
-find_package(ament_cmake REQUIRED)
-find_package(rclcpp REQUIRED)
-find_package(rclcpp_action REQUIRED)
-find_package(geometry_msgs REQUIRED)
-find_package(tf2_geometry_msgs REQUIRED)
-find_package(moveit_msgs REQUIRED)
-find_package(moveit_core REQUIRED)
-find_package(moveit_ros_planning REQUIRED)
-find_package(moveit_ros_planning_interface REQUIRED)
-find_package(plansys2_msgs REQUIRED)
-find_package(plansys2_domain_expert REQUIRED)
-find_package(plansys2_executor REQUIRED)
-find_package(plansys2_planner REQUIRED)
-find_package(plansys2_problem_expert REQUIRED)
-find_package(plansys2_pddl_parser REQUIRED)
-find_package(ament_index_cpp REQUIRED)
-find_package(plansys2_bt_actions REQUIRED)
-find_package(gazebo_msgs REQUIRED)
-find_package(robossembler_interfaces REQUIRED)
-find_package(behavior_tree REQUIRED)
-find_package(control_msgs REQUIRED)
-find_package(scene_monitor_interfaces REQUIRED)
-
-if (NOT CMAKE_CXX_STANDARD)
- set(CMAKE_CXX_STANDARD 17)
-endif()
-
-set(dependencies
- rclcpp
- rclcpp_action
- geometry_msgs tf2_geometry_msgs
- moveit_msgs
- moveit_core
- moveit_ros_planning
- moveit_ros_planning_interface
- plansys2_msgs
- plansys2_domain_expert
- plansys2_executor
- plansys2_planner
- plansys2_problem_expert
- plansys2_pddl_parser
- ament_index_cpp
- plansys2_bt_actions
- gazebo_msgs
- robossembler_interfaces
- behavior_tree
- std_msgs
- control_msgs
- scene_monitor_interfaces
-)
-
-include_directories(include)
-
-add_library(robossembler_move_topose_bt_action_client SHARED src/behavior_tree_nodes/atomic_skills/MoveToPose.cpp)
-list(APPEND plugin_libs robossembler_move_topose_bt_action_client)
-
-add_library(robossembler_get_entity_state_bt_action_client SHARED src/behavior_tree_nodes/atomic_skills/GetEntityState.cpp)
-list(APPEND plugin_libs robossembler_get_entity_state_bt_action_client)
-
-add_library(robossembler_move_gripper_bt_action_client SHARED src/behavior_tree_nodes/atomic_skills/MoveGripper.cpp)
-list(APPEND plugin_libs robossembler_move_gripper_bt_action_client)
-
-add_library(robossembler_print_bt_node SHARED src/behavior_tree_nodes/Print.cpp)
-list(APPEND plugin_libs robossembler_print_bt_node)
-
-add_library(robossembler_get_grasp_poses_bt_action_client SHARED src/behavior_tree_nodes/atomic_skills/GetGraspPoses.cpp)
-list(APPEND plugin_libs robossembler_get_grasp_poses_bt_action_client)
-
-add_library(robossembler_get_workspace_placement_pose_bt_action_client SHARED src/behavior_tree_nodes/atomic_skills/GetWorkspacePlacementPose.cpp)
-list(APPEND plugin_libs robossembler_get_workspace_placement_pose_bt_action_client)
-
-add_library(robossembler_construct_workspace_placement_pose_bt_action_client SHARED src/behavior_tree_nodes/atomic_skills/ConstructWorkspacePlacementPose.cpp)
-list(APPEND plugin_libs robossembler_construct_workspace_placement_pose_bt_action_client)
-
-foreach(bt_plugin ${plugin_libs})
- ament_target_dependencies(${bt_plugin} ${dependencies})
- target_compile_definitions(${bt_plugin} PRIVATE BT_PLUGIN_EXPORT)
-endforeach()
-
-add_executable(move_controller_node src/move_controller_node.cpp)
-ament_target_dependencies(move_controller_node ${dependencies})
-
-
-install(DIRECTORY launch pddl behavior_trees_xml config DESTINATION share/${PROJECT_NAME})
-
-install(TARGETS
- move_controller_node
- ${plugin_libs}
- ARCHIVE DESTINATION lib
- LIBRARY DESTINATION lib
- RUNTIME DESTINATION lib/${PROJECT_NAME}
-)
-
-if(BUILD_TESTING)
- find_package(ament_lint_auto REQUIRED)
- ament_lint_auto_find_test_dependencies()
-
- find_package(ament_cmake_gtest REQUIRED)
-endif()
-
-ament_export_dependencies(${dependencies})
-
-ament_package()
diff --git a/robossembler/README.md b/robossembler/README.md
deleted file mode 100644
index 35b7f18..0000000
--- a/robossembler/README.md
+++ /dev/null
@@ -1,40 +0,0 @@
-# Robossembler Plansys2
-
-This package is the main one and contains a task planner working in combination with MoveIt2. For more information about the requirements, read the main README.md
-
-
-```
-.
-├── behavior_trees_xml # Folder with BT nodes in XML file
-│ └── atomic_skills_xml
-│ ├── EmuPoseEstimation.xml
-│ └── move.xml
-| └── move_gripper.xml
-├── CMakeLists.txt
-├── config
-│ └── params.yaml # Config params for PlanSys2 robot planning
-├── include # Libs which used
-│ └── robot_bt
-│ └── behavior_tree_nodes
-│ └── atomic_skills
-│ ├── EmuPoseEstimation.hpp # Lib for get Pose of model from Gazebo
-│ └── Move.hpp # Lib for P2P arm move
-| └── MoveGripper.hpp # Lib for P2P gripper move
-├── launch
-│ ├── robossembler_bringup.launch.py # Launch file for start all scene
-│ └── task_planner.launch.py # Launch file for PlanSys2
-├── package.xml
-├── pddl
-│ ├── commands # Example commands for PlanSys2 terminal
-│ └── domain.pddl # The main domain file for launch PDDL
-├── README.md
-└── src
- ├── atomic_tasks
- ├── behavior_tree_nodes
- │ └── atomic_skills
- │ ├── EmuPoseEstimation.cpp
- │ └── Move.cpp
- | └── MoveGripper.cpp
- ├── move_action_node.cpp # Temporarily not used
- └── move_controller_node.cpp # Executable move arm controller (plan without terminal)
-```
\ No newline at end of file
diff --git a/robossembler/behavior_trees_xml/test_tree.xml b/robossembler/behavior_trees_xml/test_tree.xml
deleted file mode 100644
index 2f24c18..0000000
--- a/robossembler/behavior_trees_xml/test_tree.xml
+++ /dev/null
@@ -1,8 +0,0 @@
-
-
-
-
-
-
-
-
\ No newline at end of file
diff --git a/robossembler/config/params.yaml b/robossembler/config/params.yaml
deleted file mode 100644
index 47dbc70..0000000
--- a/robossembler/config/params.yaml
+++ /dev/null
@@ -1,22 +0,0 @@
-# assemble:
-# ros__parameters:
-# plugins:
-# - robossembler_get_entity_state_bt_action_client
-# - robossembler_move_topose_bt_action_client
-# - robossembler_move_gripper_bt_action_client
-# - robossembler_get_grasp_poses_bt_action_client
-
-print_part:
- ros__parameters:
- plugins:
- - robossembler_print_bt_node
-
-remove_part:
- ros__parameters:
- plugins:
- - robossembler_get_grasp_poses_bt_action_client
- - robossembler_move_topose_bt_action_client
- - robossembler_move_gripper_bt_action_client
- - robossembler_get_workspace_placement_pose_bt_action_client
- - robossembler_construct_workspace_placement_pose_bt_action_client
-
diff --git a/robossembler/include/robot_bt/behavior_tree_nodes/ConstructWorkspacePlacementPose.hpp b/robossembler/include/robot_bt/behavior_tree_nodes/ConstructWorkspacePlacementPose.hpp
deleted file mode 100644
index 603723b..0000000
--- a/robossembler/include/robot_bt/behavior_tree_nodes/ConstructWorkspacePlacementPose.hpp
+++ /dev/null
@@ -1,36 +0,0 @@
-#pragma once
-
-#include
-#include
-
-#include
-#include
-
-#include
-#include
-#include
-
-
-class ConstructWorkspacePlacementPose : public BtService
-{
-public:
- ConstructWorkspacePlacementPose(const std::string &xml_tag_name,
- const BT::NodeConfiguration &conf);
-
- scene_monitor_interfaces::srv::ConstructWorkspacePlacementPose::Request::SharedPtr populate_request() override;
-
- BT::NodeStatus handle_response(scene_monitor_interfaces::srv::ConstructWorkspacePlacementPose::Response::SharedPtr response) override;
-
- static BT::PortsList providedPorts()
- {
- return providedBasicPorts({
- BT::InputPort("part_id"),
- BT::InputPort("workspace"),
- BT::OutputPort("constructed_part"),
- });
- }
-
-private:
- std::string frame_;
- std::string workspace_;
-};
\ No newline at end of file
diff --git a/robossembler/include/robot_bt/behavior_tree_nodes/GetGraspPoses.hpp b/robossembler/include/robot_bt/behavior_tree_nodes/GetGraspPoses.hpp
deleted file mode 100644
index cbbdc2e..0000000
--- a/robossembler/include/robot_bt/behavior_tree_nodes/GetGraspPoses.hpp
+++ /dev/null
@@ -1,38 +0,0 @@
-#pragma once
-
-#include
-#include
-
-#include
-#include
-
-#include
-#include
-#include
-
-
-class GetGraspPoses : public BtService
-{
-public:
- GetGraspPoses(const std::string &xml_tag_name,
- const BT::NodeConfiguration &conf);
-
- scene_monitor_interfaces::srv::GetGraspPoses::Request::SharedPtr populate_request() override;
-
- BT::NodeStatus handle_response(scene_monitor_interfaces::srv::GetGraspPoses::Response::SharedPtr response) override;
-
- static BT::PortsList providedPorts()
- {
- return providedBasicPorts({
- BT::InputPort("part_id"),
- BT::OutputPort("grasp_pose"),
- BT::OutputPort("pre_grasp_pose"),
- BT::OutputPort("pre_gap_distance"),
- BT::OutputPort("gap_distance")
- });
- }
-
-private:
- std::string frame_;
- scene_monitor_interfaces::msg::GraspPose grasp_pose_;
-};
\ No newline at end of file
diff --git a/robossembler/include/robot_bt/behavior_tree_nodes/GetWorkspacePlacementPose.hpp b/robossembler/include/robot_bt/behavior_tree_nodes/GetWorkspacePlacementPose.hpp
deleted file mode 100644
index 79cc506..0000000
--- a/robossembler/include/robot_bt/behavior_tree_nodes/GetWorkspacePlacementPose.hpp
+++ /dev/null
@@ -1,39 +0,0 @@
-#pragma once
-
-#include
-#include
-
-#include
-#include
-
-#include
-#include
-#include
-
-
-class GetWorkspacePlacementPose : public BtService
-{
-public:
- GetWorkspacePlacementPose(const std::string &xml_tag_name,
- const BT::NodeConfiguration &conf);
-
- scene_monitor_interfaces::srv::GetWorkspacePlacementPose::Request::SharedPtr populate_request() override;
-
- BT::NodeStatus handle_response(scene_monitor_interfaces::srv::GetWorkspacePlacementPose::Response::SharedPtr response) override;
-
- static BT::PortsList providedPorts()
- {
- return providedBasicPorts({
- BT::InputPort("part_id"),
- BT::InputPort("workspace"),
- BT::OutputPort("placement_pose"),
- BT::OutputPort("pre_placement_pose"),
- });
- }
-
-private:
- std::string frame_;
- std::string workspace_;
- geometry_msgs::msg::Pose placement_pose_;
- geometry_msgs::msg::Pose pre_placement_pose_;
-};
\ No newline at end of file
diff --git a/robossembler/include/robot_bt/behavior_tree_nodes/Print.hpp b/robossembler/include/robot_bt/behavior_tree_nodes/Print.hpp
deleted file mode 100644
index 9f627d6..0000000
--- a/robossembler/include/robot_bt/behavior_tree_nodes/Print.hpp
+++ /dev/null
@@ -1,36 +0,0 @@
-#pragma once
-
-#include
-#include
-
-#include
-#include
-
-#include
-#include
-#include
-
-
-class Print : public BtService
-{
-public:
- Print(const std::string &xml_tag_name,
- const BT::NodeConfiguration &conf);
-
- scene_monitor_interfaces::srv::PrintPart::Request::SharedPtr populate_request() override;
-
- BT::NodeStatus handle_response(scene_monitor_interfaces::srv::PrintPart::Response::SharedPtr response) override;
-
- static BT::PortsList providedPorts()
- {
- return providedBasicPorts({
- BT::InputPort("frame"),
- BT::InputPort("printer")
- });
- }
-
-private:
-
- std::string printer_;
- std::string frame_;
-};
\ No newline at end of file
diff --git a/robossembler/launch/bt_check.launch.py b/robossembler/launch/bt_check.launch.py
deleted file mode 100644
index 1bf89a6..0000000
--- a/robossembler/launch/bt_check.launch.py
+++ /dev/null
@@ -1,20 +0,0 @@
-from launch import LaunchDescription
-from launch_ros.actions import Node
-from ament_index_python.packages import get_package_share_directory
-import os
-
-
-def generate_launch_description():
- return LaunchDescription([
- Node(
- package='robossembler',
- namespace='',
- executable='bt_check',
- # Do not declare a node name otherwise it messes with the action node names and will result in
- # duplicate nodes!
- parameters=[
- {'bt_file_path': os.path.join(get_package_share_directory('robossembler'), 'behavior_trees_xml/atomic_skills_xml/simple_sequence.xml')},
- {'plugins': ['robossembler_get_entity_state_bt_action_client', 'robossembler_move_topose_bt_action']}
- ]
- ),
- ])
\ No newline at end of file
diff --git a/robossembler/launch/robossembler_bringup.launch.py b/robossembler/launch/robossembler_bringup.launch.py
deleted file mode 100644
index 85d0649..0000000
--- a/robossembler/launch/robossembler_bringup.launch.py
+++ /dev/null
@@ -1,117 +0,0 @@
-from launch.actions.declare_launch_argument import DeclareLaunchArgument
-from launch.launch_description import LaunchDescription
-from launch.actions import IncludeLaunchDescription, OpaqueFunction
-from launch.conditions import IfCondition
-from launch.substitutions import Command, FindExecutable, PathJoinSubstitution
-from launch.substitutions.launch_configuration import LaunchConfiguration
-from launch_ros.actions import Node
-from launch_ros.substitutions import FindPackageShare
-from launch.launch_description_sources import PythonLaunchDescriptionSource
-from ament_index_python import get_package_share_directory
-import xacro
-import os
-
-def generate_launch_description():
- launch_args = []
- launch_args.append(
- DeclareLaunchArgument(
- name="grip",
- default_value="true",
- description="On or OFF gripper for launch"
- )
- )
- launch_args.append(
- DeclareLaunchArgument(
- name="sim",
- default_value="true",
- description="On or OFF simulation"
- )
- )
- launch_args.append(
- DeclareLaunchArgument(
- name="robot_name",
- default_value="rasmt",
- description="Robot name"
- )
- )
- launch_args.append(
- DeclareLaunchArgument(
- name="plansys2",
- default_value="false",
- description="enable plansys2"
- )
- )
-
- # Load robot description
- robot_description_content = Command(
- [
- FindExecutable(name="xacro"), " ",
- PathJoinSubstitution(
- [FindPackageShare("rasmt_support"), "urdf/rasmt.xacro"]
- ), " ",
- "robot_name:=", LaunchConfiguration("robot_name"), " ",
- "sim:=", LaunchConfiguration("sim"), " ",
- "grip:=", LaunchConfiguration("grip")
- ]
- )
-
- control = IncludeLaunchDescription(
- PythonLaunchDescriptionSource(
- PathJoinSubstitution([
- FindPackageShare("rasmt_support"),
- "launch",
- "rasmt_control.launch.py"
- ])
- ), launch_arguments=[
- ("robot_description", robot_description_content),
- ("sim", LaunchConfiguration("sim"))
- ]
- )
-
- simulation = IncludeLaunchDescription(
- PythonLaunchDescriptionSource(
- PathJoinSubstitution([
- FindPackageShare("rasmt_support"),
- "launch",
- "rasmt_gazebo.launch.py"
- ])
- ), launch_arguments=[
- ("robot_name", LaunchConfiguration("robot_name"))
- ],
- condition=IfCondition(LaunchConfiguration("sim"))
- )
-
- moveit = IncludeLaunchDescription(
- PythonLaunchDescriptionSource(
- PathJoinSubstitution([
- FindPackageShare("rasmt_moveit_config"),
- "launch",
- "rasmt_moveit.launch.py"
- ])
- ), launch_arguments=[
- ("robot_description", robot_description_content),
- ("sim",LaunchConfiguration("sim"))
- ]
- )
-
- task_planner_init = IncludeLaunchDescription(
- PythonLaunchDescriptionSource(
- PathJoinSubstitution([
- FindPackageShare("robossembler"),
- "launch",
- "task_planner.launch.py"
- ])
- ), launch_arguments=[
- ("robot_description", robot_description_content)
- ],
- condition=IfCondition(LaunchConfiguration("plansys2"))
- )
-
- launch_nodes = []
- launch_nodes.append(control)
- launch_nodes.append(simulation)
- launch_nodes.append(moveit)
- launch_nodes.append(task_planner_init)
-
-
- return LaunchDescription(launch_args + launch_nodes)
diff --git a/robossembler/launch/task_planner.launch.py b/robossembler/launch/task_planner.launch.py
deleted file mode 100644
index 0ec4edc..0000000
--- a/robossembler/launch/task_planner.launch.py
+++ /dev/null
@@ -1,163 +0,0 @@
-import os
-import yaml
-import xacro
-
-from ament_index_python.packages import get_package_share_directory
-from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, SetEnvironmentVariable
-from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.substitutions import LaunchConfiguration
-from launch_ros.actions import Node
-
-__debug = True
-
-def load_file(package_name, file_path):
- package_path = get_package_share_directory(package_name)
- absolute_file_path = os.path.join(package_path, file_path)
-
- try:
- with open(absolute_file_path, "r") as file:
- return file.read()
- except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available
- return None
-
-
-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:
- with open(absolute_file_path, "r") as file:
- return yaml.safe_load(file)
- except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available
- return None
-
-
-
-def generate_launch_description():
-
- robot_description_semantic_config = load_file(
- "rasmt_moveit_config", "config/rasmt.srdf"
- )
- robot_description_semantic = {
- "robot_description_semantic": robot_description_semantic_config
- }
-
- kinematics_yaml = load_yaml(
- "rasmt_moveit_config", "config/kinematics.yaml"
- )
-
- pkg_dir = get_package_share_directory('robossembler')
- namespace = LaunchConfiguration('namespace')
-
- declare_namespace_cmd = DeclareLaunchArgument(
- name="namespace",
- default_value='',
- description='Namespace')
-
- """declare_robot_description = DeclareLaunchArgument(
- name="robot_description",
- description="Robot description XML file"
- )"""
- # stdout_linebuf_envvar = SetEnvironmentVariable(
- # 'RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED', '1')
-
- if __debug:
- # get xacro file path
- xacro_file = os.path.join(get_package_share_directory("rasmt_support"),"urdf/","rasmt.xacro")
- # get error if xacro file if missing
- assert os.path.exists(xacro_file), "The xacro file of neptun_description doesnt exist"+str(xacro_file)
- # parse xacro file from file with arguments
- robot_description_file = xacro.process_file(xacro_file, mappings={'grip':"true",'sim':"false",'robot_name':"rasmt"})
- # convert file to xml format
- robot_description_content = robot_description_file.toxml()
- robot_description = {'robot_description': robot_description_content}
-
- plansys2_cmd = IncludeLaunchDescription(
- PythonLaunchDescriptionSource(os.path.join(
- get_package_share_directory('plansys2_bringup'),
- 'launch',
- 'plansys2_bringup_launch_monolithic.py')),
- launch_arguments={
- 'model_file': pkg_dir + '/pddl/domain.pddl',
- 'namespace': namespace
- }.items())
- """
- assemble = Node(
- package='plansys2_bt_actions',
- executable='bt_action_node',
- name='assemble',
- namespace=namespace,
- output='screen',
- parameters=[
- pkg_dir + '/config/params.yaml',
- robot_description,
- robot_description_semantic,
- kinematics_yaml,
- {
- 'action_name': 'assemble',
- # 'publisher_port': 1666,
- # 'server_port': 1667,
- 'bt_xml_file': pkg_dir + '/behavior_trees_xml/atomic_skills_xml/assemble.xml',
- }
- ])
- """
- print_part = Node(
- package='plansys2_bt_actions',
- executable='bt_action_node',
- name='print_part',
- namespace=namespace,
- output='screen',
- parameters=[
- pkg_dir + '/config/params.yaml',
- {
- 'action_name': 'print',
- 'bt_xml_file': pkg_dir + '/behavior_trees_xml/atomic_skills_xml/print.xml',
- }
- ])
-
- remove_part = Node(
- package='plansys2_bt_actions',
- executable='bt_action_node',
- name='remove_part',
- namespace=namespace,
- output='screen',
- parameters=[
- pkg_dir + '/config/params.yaml',
- robot_description,
- robot_description_semantic,
- kinematics_yaml,
- {
- 'action_name': 'remove',
- 'bt_xml_file': pkg_dir + '/behavior_trees_xml/atomic_skills_xml/remove.xml',
- }
- ])
-
- """gz_get_entity_state = Node(
- package='plansys2_bt_actions',
- executable='bt_action_node',
- name='get_part_state',
- namespace=namespace,
- output='screen',
- parameters=[
- pkg_dir + '/config/params.yaml',
- {
- 'action_name':'get_part_state',
- 'bt_xml_file':pkg_dir + '/behavior_trees_xml/atomic_skills_xml/get_part_state.xml'
- }
- ]
- )"""
-
- ld = LaunchDescription()
-
- ld.add_action(declare_namespace_cmd)
- #ld.add_action(declare_robot_description)
- #ld.add_action(gz_get_entity_state)
-
- # Declare the launch options
- ld.add_action(plansys2_cmd)
- # ld.add_action(assemble)
- ld.add_action(print_part)
- ld.add_action(remove_part)
-
- return ld
\ No newline at end of file
diff --git a/robossembler/package.xml b/robossembler/package.xml
deleted file mode 100644
index bce1d6b..0000000
--- a/robossembler/package.xml
+++ /dev/null
@@ -1,42 +0,0 @@
-
-
-
-
- robossembler
- 0.0.1
-
- ROS2 task planner for manipulator
- Splinter1984
-
- Apache License, Version 2.0
- ament_cmake
-
- rclcpp
- rclcpp_action
- geometry_msgs
- tf2_geometry_msgs
- gazebo_msgs
- moveit_msgs
- moveit_core
- moveit_ros_planning
- moveit_ros_planning_interface
- plansys2_msgs
- plansys2_domain_expert
- plansys2_executor
- plansys2_planner
- plansys2_problem_expert
- plansys2_pddl_parser
- plansys2_bt_actions
- ament_index_cpp
- robossembler_interfaces
- behavior_tree
- scene_monitor_interfaces
-
- ament_lint_common
- ament_lint_auto
- ament_cmake_gtest
-
-
- ament_cmake
-
-
\ No newline at end of file
diff --git a/robossembler/pddl/commands b/robossembler/pddl/commands
deleted file mode 100644
index ca6127b..0000000
--- a/robossembler/pddl/commands
+++ /dev/null
@@ -1,11 +0,0 @@
-// Move arm to pose
-set instance rasmt_arm_group robot
-set instance one zone
-set goal (and (robot_move rasmt_arm_group one))
-run
-
-// Move gripper to pose
-set instance rasmt_hand_arm_group robot
-set instance open gap
-set goal (and (gripper_move rasmt_hand_arm_group open))
-run
\ No newline at end of file
diff --git a/robossembler/pddl/domain.pddl b/robossembler/pddl/domain.pddl
deleted file mode 100644
index a770698..0000000
--- a/robossembler/pddl/domain.pddl
+++ /dev/null
@@ -1,67 +0,0 @@
-;; Modified domain taken from
-;; "Knowledge transfer in robot manipulation tasks" by Jacob O. Huckaby 2014
-(define (domain robossembler)
- (:requirements :strips :typing :adl :fluents :durative-actions)
- (:types
- printer workspace - zone
- part
- arm
- assembly
- )
-
- (:predicates
- (arm_available ?a - arm)
- (part_at ?p - part ?z - zone)
- (printer_ready ?p - printer)
- (part_of ?part - part ?whole - assembly)
- (assembly_order ?prev ?next - assembly)
- (assembled ?whole - assembly ?z - zone)
- )
-
- (:functions)
-
- (:durative-action print
- :parameters (?p - part ?pr - printer)
- :duration ( = ?duration 10)
- :condition (and
- (at start(printer_ready ?pr))
- )
- :effect (and
- (at start (not (printer_ready ?pr)))
- (at end(part_at ?p ?pr))
- )
- )
-
- (:durative-action remove
- :parameters (?p - part ?pr - printer ?z - zone ?a - arm)
- :duration (= ?duration 1)
- :condition (and
- (at start (part_at ?p ?pr))
- (at start (arm_available ?a))
- )
- :effect (and
- (at start (not (arm_available ?a)))
- (at end (part_at ?p ?z))
- (at end (arm_available ?a))
- (at end (printer_ready ?pr))
- )
- )
-
- (:durative-action assemble
- :parameters (?p - part ?prev ?next - assembly ?w - workspace ?arm - arm)
- :duration (= ?duration 5)
- :condition (and
- (at start (assembled ?prev ?w))
- (at start (part-at ?p ?w))
- (at start (part-of ?p ?next))
- (at start (arm-available ?arm))
- (at start (assembly-order ?prev ?next))
- )
- :effect (and
- (at start (not (arm-available ?arm)))
- (at end (not (part-at ?p ?w)))
- (at end (arm-available ?arm))
- (at end (assembled ?next ?w))
- )
- )
-);; end Domain ;;;;;;;;;;;;;;;;;;;;;;;;
\ No newline at end of file
diff --git a/robossembler/pddl/problem.pddl b/robossembler/pddl/problem.pddl
deleted file mode 100644
index fed4491..0000000
--- a/robossembler/pddl/problem.pddl
+++ /dev/null
@@ -1,18 +0,0 @@
-(define (problem p1)
-(:domain robossembler)
-(:objects
- rasmt_arm_group - arm
- printer1 printer2 - printer
- workspace1 workspace2 - workspace
- box1 - part
-)
-(:init
- (printer_ready printer1)
- (printer_ready printer2)
- (arm_available rasmt_arm_group)
-)
-(:goal (and
- (part_at box1 workspace1)
- )
-)
-)
diff --git a/robossembler/pddl/problem_full.pddl b/robossembler/pddl/problem_full.pddl
deleted file mode 100644
index c90ad0a..0000000
--- a/robossembler/pddl/problem_full.pddl
+++ /dev/null
@@ -1,29 +0,0 @@
-(define (problem p1)
-(:domain robossembler)
-(:objects
- ;; information from Scene
- rasmt - arm
- printer1 printer2 - printer
- workspace1 - workspace
- ;; information from CAD
- part1 part2 - part
- subasm0 subasm1 subasm2 - assembly
-)
-(:init
- ;; information from Scene
- (printer-ready printer1)
- (printer-ready printer2)
- (arm-available rasmt)
- ;; information from CAD
- (assembled subasm0 workspace1)
- (part-of part1 subasm1)
- (part-of part2 subasm2)
- (assembly-order subasm0 subasm1)
- (assembly-order subasm1 subasm2)
-)
-(:goal (and
- ;; information from CAD
- (assembled subasm2 workspace1)
- )
-)
-)
\ No newline at end of file
diff --git a/robossembler/src/behavior_tree_nodes/Print.cpp b/robossembler/src/behavior_tree_nodes/Print.cpp
deleted file mode 100644
index fac5dcf..0000000
--- a/robossembler/src/behavior_tree_nodes/Print.cpp
+++ /dev/null
@@ -1,54 +0,0 @@
-#include "robot_bt/behavior_tree_nodes/Print.hpp"
-
-#include
-#include
-#include
-
-#include
-#include
-#include
-#include
-#include
-
-
-using PrintPartSrv = scene_monitor_interfaces::srv::PrintPart;
-
-Print::Print(const std::string &xml_tag_name,
- const BT::NodeConfiguration &conf)
-:BtService(xml_tag_name, conf)
-{}
-
-PrintPartSrv::Request::SharedPtr Print::populate_request()
-{
- std::string frame, printer;
- getInput("frame", frame_);
- RCLCPP_INFO_STREAM(_node->get_logger(), "Provided part name ["<< frame_ <<"]");
-
- getInput("printer", printer_);
- RCLCPP_INFO_STREAM(_node->get_logger(), "Provided printer name ["<< printer_ <<"]");
-
- auto request = std::make_shared();
- request->frame_id = frame_;
- request->printer_id = printer_;
-
- return request;
-}
-
-BT::NodeStatus Print::handle_response(PrintPartSrv::Response::SharedPtr response)
-{
- RCLCPP_INFO(_node->get_logger(), "Handle Response call");
-
- if (!response->success)
- {
- RCLCPP_WARN(_node->get_logger(), "Get failure response from service with msg: `%s`", response->message.c_str());
- return BT::NodeStatus::FAILURE;
- }
-
- RCLCPP_INFO(_node->get_logger(), "Get success response status with msg: `%s`", response->message.c_str());
- return BT::NodeStatus::SUCCESS;
-}
-
-BT_REGISTER_NODES(factory)
-{
- factory.registerNodeType("Print");
-}
\ No newline at end of file
diff --git a/robossembler/src/behavior_tree_nodes/atomic_skills/ConstructWorkspacePlacementPose.cpp b/robossembler/src/behavior_tree_nodes/atomic_skills/ConstructWorkspacePlacementPose.cpp
deleted file mode 100644
index 1f081b4..0000000
--- a/robossembler/src/behavior_tree_nodes/atomic_skills/ConstructWorkspacePlacementPose.cpp
+++ /dev/null
@@ -1,58 +0,0 @@
-#include "robot_bt/behavior_tree_nodes/ConstructWorkspacePlacementPose.hpp"
-
-#include
-#include
-#include
-#include
-
-#include
-#include
-#include
-#include
-#include
-
-
-using ConstructWorkspacePlacementPoseSrv = scene_monitor_interfaces::srv::ConstructWorkspacePlacementPose;
-
-ConstructWorkspacePlacementPose::ConstructWorkspacePlacementPose(const std::string &xml_tag_name,
- const BT::NodeConfiguration &conf)
-:BtService(xml_tag_name, conf)
-{}
-
-ConstructWorkspacePlacementPoseSrv::Request::SharedPtr ConstructWorkspacePlacementPose::populate_request()
-{
- std::string frame, printer;
- getInput("part_id", frame_);
- RCLCPP_INFO_STREAM(_node->get_logger(), "Provided part name ["<< frame_ <<"]");
-
- getInput("workspace", workspace_);
- RCLCPP_INFO_STREAM(_node->get_logger(), "Provided printer name ["<< workspace_ <<"]");
-
- auto request = std::make_shared();
- request->frame_id = frame_;
- request->workspace_id = workspace_;
-
- return request;
-}
-
-BT::NodeStatus ConstructWorkspacePlacementPose::handle_response(ConstructWorkspacePlacementPoseSrv::Response::SharedPtr response)
-{
- RCLCPP_INFO(_node->get_logger(), "Handle Response call");
-
- if (!response->success)
- {
- RCLCPP_WARN(_node->get_logger(), "Get failure response from service with msg: `%s`", response->message.c_str());
- return BT::NodeStatus::FAILURE;
- }
-
- RCLCPP_INFO(_node->get_logger(), "Get success response status with msg: `%s`", response->message.c_str());
-
- setOutput("constructed_part", frame_ + "_" + workspace_);
-
- return BT::NodeStatus::SUCCESS;
-}
-
-BT_REGISTER_NODES(factory)
-{
- factory.registerNodeType("ConstructWorkspacePlacementPose");
-}
\ No newline at end of file
diff --git a/robossembler/src/behavior_tree_nodes/atomic_skills/GetEntityState.cpp b/robossembler/src/behavior_tree_nodes/atomic_skills/GetEntityState.cpp
deleted file mode 100644
index 372d8c8..0000000
--- a/robossembler/src/behavior_tree_nodes/atomic_skills/GetEntityState.cpp
+++ /dev/null
@@ -1,47 +0,0 @@
-#include "gazebo_msgs/srv/get_entity_state.hpp"
-#include "behaviortree_cpp_v3/bt_factory.h"
-#include "behavior_tree/BtService.hpp"
-
-using namespace BT;
-using GetEntityStateSrv = gazebo_msgs::srv::GetEntityState;
-
-class GetEntityState : public BtService
-{
- public:
- GetEntityState(const std::string & name, const BT::NodeConfiguration & config)
- : BtService(name, config){
-
- }
-
- GetEntityStateSrv::Request::SharedPtr populate_request() override
- {
- getInput("part_name", part_name_);
- //part_name_ = getInput("PartName").value();
-
- RCLCPP_INFO_STREAM(_node->get_logger(), "Provided part name ["<< part_name_<<"]");
-
- //GetEntityStateSrv::Request::SharedPtr request;
- auto request = std::make_shared();
- request->name = part_name_;
- RCLCPP_INFO(_node->get_logger(), "Request populated");
- return request;
- }
- BT::NodeStatus handle_response(GetEntityStateSrv::Response::SharedPtr response) override
- {
- RCLCPP_INFO_STREAM(_node->get_logger(), "part_name ["<< part_name_ <<"] Entity state x = "<< response->state.pose.position.x <<" y = "<< response->state.pose.position.y <<" z = "<< response->state.pose.position.z <<"");
- return BT::NodeStatus::SUCCESS;
- }
-
- static PortsList providedPorts()
- {
- return providedBasicPorts({
- InputPort("part_name")});
- }
- private:
- std::string part_name_;
-};
-
-BT_REGISTER_NODES(factory)
-{
- factory.registerNodeType("GetEntityState");
-}
\ No newline at end of file
diff --git a/robossembler/src/behavior_tree_nodes/atomic_skills/GetGraspPoses.cpp b/robossembler/src/behavior_tree_nodes/atomic_skills/GetGraspPoses.cpp
deleted file mode 100644
index d7a7079..0000000
--- a/robossembler/src/behavior_tree_nodes/atomic_skills/GetGraspPoses.cpp
+++ /dev/null
@@ -1,58 +0,0 @@
-#include "robot_bt/behavior_tree_nodes/GetGraspPoses.hpp"
-
-#include
-#include
-#include
-
-#include
-#include
-#include
-#include
-#include
-
-
-using GetGraspPosesSrv = scene_monitor_interfaces::srv::GetGraspPoses;
-
-GetGraspPoses::GetGraspPoses(const std::string &xml_tag_name,
- const BT::NodeConfiguration &conf)
-:BtService(xml_tag_name, conf)
-{}
-
-GetGraspPosesSrv::Request::SharedPtr GetGraspPoses::populate_request()
-{
- std::string frame, printer;
- getInput("part_id", frame_);
- RCLCPP_INFO_STREAM(_node->get_logger(), "Provided part name ["<< frame_ <<"]");
-
- auto request = std::make_shared();
- request->frame_id = frame_;
-
- return request;
-}
-
-BT::NodeStatus GetGraspPoses::handle_response(GetGraspPosesSrv::Response::SharedPtr response)
-{
- RCLCPP_INFO(_node->get_logger(), "Handle Response call");
-
- if (!response->success)
- {
- RCLCPP_WARN(_node->get_logger(), "Get failure response from service with msg: `%s`", response->message.c_str());
- return BT::NodeStatus::FAILURE;
- }
-
- RCLCPP_INFO(_node->get_logger(), "Get success response status with msg: `%s`", response->message.c_str());
-
- scene_monitor_interfaces::msg::GraspPose grasp_pose = response->grasp_poses[0];
-
- setOutput("grasp_pose", grasp_pose.grasp_pose);
- setOutput("pre_grasp_pose", grasp_pose.pre_grasp_pose);
- setOutput("pre_gap_distance", grasp_pose.gap_distance+0.006);
- setOutput("gap_distance", grasp_pose.gap_distance);
-
- return BT::NodeStatus::SUCCESS;
-}
-
-BT_REGISTER_NODES(factory)
-{
- factory.registerNodeType("GetGraspPoses");
-}
\ No newline at end of file
diff --git a/robossembler/src/behavior_tree_nodes/atomic_skills/GetWorkspacePlacementPose.cpp b/robossembler/src/behavior_tree_nodes/atomic_skills/GetWorkspacePlacementPose.cpp
deleted file mode 100644
index a03926e..0000000
--- a/robossembler/src/behavior_tree_nodes/atomic_skills/GetWorkspacePlacementPose.cpp
+++ /dev/null
@@ -1,61 +0,0 @@
-#include "robot_bt/behavior_tree_nodes/GetWorkspacePlacementPose.hpp"
-
-#include
-#include
-#include
-
-#include
-#include
-#include
-#include
-#include
-
-
-using GetWorkspacePlacementPoseSrv = scene_monitor_interfaces::srv::GetWorkspacePlacementPose;
-
-GetWorkspacePlacementPose::GetWorkspacePlacementPose(const std::string &xml_tag_name,
- const BT::NodeConfiguration &conf)
-:BtService(xml_tag_name, conf)
-{}
-
-GetWorkspacePlacementPoseSrv::Request::SharedPtr GetWorkspacePlacementPose::populate_request()
-{
- std::string frame, printer;
- getInput("part_id", frame_);
- RCLCPP_INFO_STREAM(_node->get_logger(), "Provided part name ["<< frame_ <<"]");
-
- getInput("workspace", workspace_);
- RCLCPP_INFO_STREAM(_node->get_logger(), "Provided printer name ["<< workspace_ <<"]");
-
- auto request = std::make_shared();
- request->frame_id = frame_;
- request->workspace_id = workspace_;
-
- return request;
-}
-
-BT::NodeStatus GetWorkspacePlacementPose::handle_response(GetWorkspacePlacementPoseSrv::Response::SharedPtr response)
-{
- RCLCPP_INFO(_node->get_logger(), "Handle Response call");
-
- if (!response->success)
- {
- RCLCPP_WARN(_node->get_logger(), "Get failure response from service with msg: `%s`", response->message.c_str());
- return BT::NodeStatus::FAILURE;
- }
-
- RCLCPP_INFO(_node->get_logger(), "Get success response status with msg: `%s`", response->message.c_str());
-
- scene_monitor_interfaces::msg::GraspPose pose = response->grasp_poses[0];
- pose.grasp_pose.position.z += 0.015;
- setOutput("placement_pose", pose.grasp_pose);
- pose.pre_grasp_pose.position.z += 0.015;
- setOutput("pre_placement_pose", pose.pre_grasp_pose);
-
- return BT::NodeStatus::SUCCESS;
-}
-
-BT_REGISTER_NODES(factory)
-{
- factory.registerNodeType("GetWorkspacePlacementPose");
-}
\ No newline at end of file
diff --git a/robossembler/src/behavior_tree_nodes/atomic_skills/MoveGripper.cpp b/robossembler/src/behavior_tree_nodes/atomic_skills/MoveGripper.cpp
deleted file mode 100644
index 55e4223..0000000
--- a/robossembler/src/behavior_tree_nodes/atomic_skills/MoveGripper.cpp
+++ /dev/null
@@ -1,50 +0,0 @@
-//#include "robossembler_interfaces/action/moveit_send_joint_states.hpp"
-//#include "control_msgs/action/follow_joint_trajectory.hpp"
-#include "robossembler_interfaces/action/gripper_command.hpp"
-#include "behaviortree_cpp_v3/bt_factory.h"
-#include "behavior_tree/BtAction.hpp"
-
-using namespace BT;
-using GripperCmd = robossembler_interfaces::action::GripperCommand;
-static const rclcpp::Logger LOGGER = rclcpp::get_logger("gripper_cmd");
-
-class SendGripperCmd : public BtAction
-{
- public:
- SendGripperCmd(const std::string & name, const BT::NodeConfiguration & config)
- : BtAction(name, config)
- {
- }
-
- GripperCmd::Goal populate_goal() override
- {
-
- auto goal = GripperCmd::Goal();
-
- getInput("gap_distance", distance_);
- getInput("frame_name", frame_);
- goal.value = distance_;
- goal.frame = frame_;
-
- return goal;
- }
-
- static PortsList providedPorts()
- {
- return providedBasicPorts({
- InputPort("gap_distance"),
- InputPort("frame_name")
- });
- }
-
- private:
- std::vector joint_names_ = {"rasmt_Slide_1", "rasmt_Slide_2"};
- double distance_;
- std::string frame_;
-
-}; // class MoveToJointState
-
-BT_REGISTER_NODES(factory)
-{
- factory.registerNodeType("GripperCmd");
-}
\ No newline at end of file
diff --git a/robossembler/src/move_controller_node.cpp b/robossembler/src/move_controller_node.cpp
deleted file mode 100644
index 693d0f3..0000000
--- a/robossembler/src/move_controller_node.cpp
+++ /dev/null
@@ -1,141 +0,0 @@
-// Copyright 2022 Roman Andrianov
-//
-// /* LICENSE (Checkout: https://github.com/ament/ament_lint/tree/master/ament_copyright/test/cases) */
-
-#include
-
-#include
-
-#include "plansys2_msgs/msg/action_execution_info.hpp"
-#include "plansys2_msgs/msg/plan.hpp"
-
-#include "plansys2_domain_expert/DomainExpertClient.hpp"
-#include "plansys2_executor/ExecutorClient.hpp"
-#include "plansys2_planner/PlannerClient.hpp"
-#include "plansys2_problem_expert/ProblemExpertClient.hpp"
-
-#include "rclcpp/rclcpp.hpp"
-#include "rclcpp_action/rclcpp_action.hpp"
-
-class MoveController : public rclcpp::Node
-{
-public:
- MoveController()
- : rclcpp::Node("move_controller")
- {
- }
-
- bool init()
- {
- domain_expert_ = std::make_shared();
- planner_client_ = std::make_shared();
- problem_expert_ = std::make_shared();
- executor_client_ = std::make_shared();
-
-
- init_knowledge();
-
- auto domain = domain_expert_->getDomain();
- auto problem = problem_expert_->getProblem();
- auto plan = planner_client_->getPlan(domain, problem);
-
- if (!plan.has_value()) {
- std::cout << "Could not find plan to reach goal " <<
- parser::pddl::toString(problem_expert_->getGoal()) << std::endl;
- return false;
- }
-
- if (!executor_client_->start_plan_execution(plan.value()))
- {
- RCLCPP_ERROR(get_logger(), "Error starting a new plan (first)");
- }
- return true;
- }
-
- void init_knowledge()
- {
- problem_expert_->addInstance(plansys2::Instance{"printer1", "printer"});
- problem_expert_->addInstance(plansys2::Instance{"box1", "part"});
- problem_expert_->addInstance(plansys2::Instance{"rasmt_arm_group", "arm"});
- problem_expert_->addInstance(plansys2::Instance{"workspace1", "workspace"});
-
-
- // problem_expert_->addInstance(plansys2::Instance("rasmt_hand_arm_group", "gripper"));
-
- problem_expert_->addPredicate(plansys2::Predicate{"(printer_ready printer1)"});
- problem_expert_->addPredicate(plansys2::Predicate{"(arm_available rasmt_arm_group)"});
-
- problem_expert_->setGoal(plansys2::Goal("(and(part_at box1 workspace1))"));
- // problem_expert_->setGoal(plansys2::Goal("(and(enviroment_setup rasmt_arm_group rasmt_hand_arm_group cube))"));
-
- }
-
- /*std::string status_to_string(int8_t status)
- {
- switch (status)
- {
- case plansys2_msgs::msg::ActionExecutionInfo::NOT_EXECUTED:
- return "NOT_EXECUTED";
- break;
- case plansys2_msgs::msg::ActionExecutionInfo::EXECUTING:
- return "EXECUTING";
- break;
- case plansys2_msgs::msg::ActionExecutionInfo::FAILED:
- return "FAILED";
- break;
- case plansys2_msgs::msg::ActionExecutionInfo::SUCCEEDED:
- return "SUCCEEDED";
- break;
- case plansys2_msgs::msg::ActionExecutionInfo::CANCELLED:
- return "CANCELLED";
- break;
- default:
- return "UNKNOWN";
- break;
- }
- }*/
-
- void step()
- {
- if (!executor_client_->execute_and_check_plan()) { // Plan finished
- auto result = executor_client_->getResult();
-
- if (result.value().success) {
- RCLCPP_INFO(get_logger(), "Plan succesfully finished");
- } else {
- RCLCPP_ERROR(get_logger(), "Plan finished with error");
- }
- }
- }
-
-private:
- std::shared_ptr problem_expert_;
- std::shared_ptr domain_expert_;
- std::shared_ptr executor_client_;
- std::shared_ptr planner_client_;
-};
-
-int main(int argc, char ** argv)
-{
- rclcpp::init(argc, argv);
- auto node = std::make_shared();
-
- //node->init();
- if (!node->init())
- {
- RCLCPP_ERROR(node->get_logger(), "Error in Init");
- return 0;
- }
-
- rclcpp::Rate rate(5);
- while (rclcpp::ok()) {
- node->step();
-
- rate.sleep();
- rclcpp::spin_some(node->get_node_base_interface());
- }
-
- rclcpp::shutdown();
-
- return 0;
-}
\ No newline at end of file
diff --git a/robossembler_scene_monitor/nlohmann_json/CMakeLists.txt b/robossembler_scene_monitor/nlohmann_json/CMakeLists.txt
deleted file mode 100644
index fea4071..0000000
--- a/robossembler_scene_monitor/nlohmann_json/CMakeLists.txt
+++ /dev/null
@@ -1,37 +0,0 @@
-cmake_minimum_required(VERSION 3.5)
-project(nlohmann_json)
-
-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)
-
-add_library(${PROJECT_NAME} INTERFACE)
-
-target_include_directories(${PROJECT_NAME} INTERFACE
- "$"
- "$"
-)
-
-install(TARGETS ${PROJECT_NAME}
- EXPORT "export_${PROJECT_NAME}"
- ARCHIVE DESTINATION lib
- LIBRARY DESTINATION lib
- RUNTIME DESTINATION lib
- INCLUDES DESTINATION include
-)
-
-install(DIRECTORY include/${PROJECT_NAME}/
- DESTINATION include/${PROJECT_NAME}
-)
-
-if(BUILD_TESTING)
- find_package(ament_lint_auto REQUIRED)
- ament_lint_auto_find_test_dependencies()
-endif()
-
-#ament_export_libraries(${PROJECT_NAME})
-ament_export_targets("export_${PROJECT_NAME}")
-ament_package()
\ No newline at end of file
diff --git a/robossembler_scene_monitor/nlohmann_json/include/nlohmann_json/json.hpp b/robossembler_scene_monitor/nlohmann_json/include/nlohmann_json/json.hpp
deleted file mode 100644
index cb27e05..0000000
--- a/robossembler_scene_monitor/nlohmann_json/include/nlohmann_json/json.hpp
+++ /dev/null
@@ -1,22091 +0,0 @@
-/*
- __ _____ _____ _____
- __| | __| | | | JSON for Modern C++
-| | |__ | | | | | | version 3.10.5
-|_____|_____|_____|_|___| https://github.com/nlohmann/json
-
-Licensed under the MIT License .
-SPDX-License-Identifier: MIT
-Copyright (c) 2013-2022 Niels Lohmann .
-
-Permission is hereby granted, free of charge, to any person obtaining a copy
-of this software and associated documentation files (the "Software"), to deal
-in the Software without restriction, including without limitation the rights
-to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-copies of the Software, and to permit persons to whom the Software is
-furnished to do so, subject to the following conditions:
-
-The above copyright notice and this permission notice shall be included in all
-copies or substantial portions of the Software.
-
-THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
-SOFTWARE.
-*/
-
-/****************************************************************************\
- * Note on documentation: The source files contain links to the online *
- * documentation of the public API at https://json.nlohmann.me. This URL *
- * contains the most recent documentation and should also be applicable to *
- * previous versions; documentation for deprecated functions is not *
- * removed, but marked deprecated. See "Generate documentation" section in *
- * file doc/README.md. *
-\****************************************************************************/
-
-#ifndef INCLUDE_NLOHMANN_JSON_HPP_
-#define INCLUDE_NLOHMANN_JSON_HPP_
-
-#define NLOHMANN_JSON_VERSION_MAJOR 3
-#define NLOHMANN_JSON_VERSION_MINOR 10
-#define NLOHMANN_JSON_VERSION_PATCH 5
-
-#include // all_of, find, for_each
-#include // nullptr_t, ptrdiff_t, size_t
-#include // hash, less
-#include // initializer_list
-#ifndef JSON_NO_IO
- #include // istream, ostream
-#endif // JSON_NO_IO
-#include // random_access_iterator_tag
-#include // unique_ptr
-#include // accumulate
-#include // string, stoi, to_string
-#include // declval, forward, move, pair, swap
-#include // vector
-
-// #include
-
-
-#include
-#include
-
-// #include
-
-
-#include // transform
-#include // array
-#include // forward_list
-#include // inserter, front_inserter, end
-#include