diff --git a/CMakeLists.txt b/CMakeLists.txt index 88bb1635..b1e103cd 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -2,8 +2,11 @@ cmake_minimum_required(VERSION 2.6.12) project(moveit_task_constructor) +set(MSG_DEPS trajectory_msgs moveit_msgs) + find_package(catkin REQUIRED COMPONENTS roscpp + genmsg ${MSG_DEPS} moveit_core moveit_ros_planning moveit_ros_planning_interface @@ -11,6 +14,20 @@ find_package(catkin REQUIRED COMPONENTS eigen_conversions ) +# ROS messages, services and actions +add_message_files(DIRECTORY msg + FILES + Stage.msg + Task.msg + Solution.msg +) +add_service_files(DIRECTORY srv + FILES + GetInterfaceState.srv + GetSolutionTrajectory.srv +) +generate_messages(DEPENDENCIES ${MSG_DEPS}) + catkin_package( INCLUDE_DIRS include ) diff --git a/msg/Solution.msg b/msg/Solution.msg new file mode 100644 index 00000000..e948b630 --- /dev/null +++ b/msg/Solution.msg @@ -0,0 +1,8 @@ +# unique id within task +uint32 id + +# IDs of subsolutions, empty if ID refers to an actual SubTrajectory +uint32[] sub_solutions + +# associated cost +float32 cost diff --git a/msg/Stage.msg b/msg/Stage.msg new file mode 100644 index 00000000..3aa73d2d --- /dev/null +++ b/msg/Stage.msg @@ -0,0 +1,22 @@ +# unique id within task +uint32 id + +# name of this stage +string name + +# flags: interface, ... +uint32 flags + +# parent id, parent_id == id means root +uint32 parent_id + +# ids of received start / end states +uint32[] received_starts +uint32[] received_ends +# ids of generated start / end states +uint32[] generated_starts +uint32[] generated_ends + +# successful and failed solutions of this stage +Solution[] solutions +Solution[] failures diff --git a/msg/Task.msg b/msg/Task.msg new file mode 100644 index 00000000..7c8f5d70 --- /dev/null +++ b/msg/Task.msg @@ -0,0 +1,5 @@ +# unique of this task +string id + +# list of all stages, include the task stage itself +Stage[] stages diff --git a/srv/GetInterfaceState.srv b/srv/GetInterfaceState.srv new file mode 100644 index 00000000..33f7f7af --- /dev/null +++ b/srv/GetInterfaceState.srv @@ -0,0 +1,7 @@ +# ID of InterfaceState +uint32 state_id + +--- + +# planning scene diff to tasks' main planning scene +moveit_msgs/PlanningScene scene diff --git a/srv/GetSolutionTrajectory.srv b/srv/GetSolutionTrajectory.srv new file mode 100644 index 00000000..a5099f3e --- /dev/null +++ b/srv/GetSolutionTrajectory.srv @@ -0,0 +1,7 @@ +# ID of solution (as published in Task msg) +uint32 solution_id + +--- + +# solution trajectory composed from multiple sub trajectories +moveit_msgs/DisplayTrajectory trajectory