From ece03990f2285be372298e4bfec74a1f4ac3450e Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" <43976882+isamu-takagi@users.noreply.github.com> Date: Mon, 4 Sep 2023 13:37:21 +0900 Subject: [PATCH] feat(autoware_adapi_v1_msgs): add cooperation (#41) * feat: add cooperation messages Signed-off-by: Takagi, Isamu * feat: add cooperation service Signed-off-by: Takagi, Isamu * feat: merge planning factors Signed-off-by: Takagi, Isamu * feat: modify constant names Signed-off-by: Takagi, Isamu * feat: add cooperation default Signed-off-by: Takagi, Isamu * update fields Signed-off-by: Takagi, Isamu * upfate default decision service Signed-off-by: Takagi, Isamu * rename service Signed-off-by: Takagi, Isamu * update message name Signed-off-by: Takagi, Isamu * fix constants Signed-off-by: Takagi, Isamu * add constants for behavior and sequence Signed-off-by: Takagi, Isamu * update field Signed-off-by: Takagi, Isamu * fix order Signed-off-by: Takagi, Isamu * add comment Signed-off-by: Takagi, Isamu --------- Signed-off-by: Takagi, Isamu --- autoware_adapi_v1_msgs/CMakeLists.txt | 11 +++++- .../planning/msg/CooperationCommand.msg | 2 + .../planning/msg/CooperationDecision.msg | 7 ++++ .../planning/msg/CooperationPolicy.msg | 6 +++ .../planning/msg/CooperationStatus.msg | 4 ++ .../planning/msg/PlanningBehavior.msg | 16 ++++++++ .../planning/msg/PlanningSequence.msg | 5 +++ .../planning/msg/SteeringFactor.msg | 37 ++++++++++++------- .../planning/msg/VelocityFactor.msg | 27 +++++++++----- .../planning/srv/GetCooperationPolicies.srv | 3 ++ .../planning/srv/SetCooperationCommands.srv | 3 ++ .../planning/srv/SetCooperationPolicies.srv | 3 ++ 12 files changed, 99 insertions(+), 25 deletions(-) create mode 100644 autoware_adapi_v1_msgs/planning/msg/CooperationCommand.msg create mode 100644 autoware_adapi_v1_msgs/planning/msg/CooperationDecision.msg create mode 100644 autoware_adapi_v1_msgs/planning/msg/CooperationPolicy.msg create mode 100644 autoware_adapi_v1_msgs/planning/msg/CooperationStatus.msg create mode 100644 autoware_adapi_v1_msgs/planning/msg/PlanningBehavior.msg create mode 100644 autoware_adapi_v1_msgs/planning/msg/PlanningSequence.msg create mode 100644 autoware_adapi_v1_msgs/planning/srv/GetCooperationPolicies.srv create mode 100644 autoware_adapi_v1_msgs/planning/srv/SetCooperationCommands.srv create mode 100644 autoware_adapi_v1_msgs/planning/srv/SetCooperationPolicies.srv diff --git a/autoware_adapi_v1_msgs/CMakeLists.txt b/autoware_adapi_v1_msgs/CMakeLists.txt index 28d3138..d0b56ae 100644 --- a/autoware_adapi_v1_msgs/CMakeLists.txt +++ b/autoware_adapi_v1_msgs/CMakeLists.txt @@ -30,6 +30,15 @@ rosidl_generate_interfaces(${PROJECT_NAME} planning/msg/SteeringFactorArray.msg planning/msg/VelocityFactor.msg planning/msg/VelocityFactorArray.msg + planning/msg/PlanningBehavior.msg + planning/msg/PlanningSequence.msg + planning/msg/CooperationCommand.msg + planning/msg/CooperationDecision.msg + planning/msg/CooperationPolicy.msg + planning/msg/CooperationStatus.msg + planning/srv/SetCooperationCommands.srv + planning/srv/SetCooperationPolicies.srv + planning/srv/GetCooperationPolicies.srv system/msg/MrmState.msg vehicle/msg/DoorCommand.msg vehicle/msg/DoorLayout.msg @@ -46,10 +55,10 @@ rosidl_generate_interfaces(${PROJECT_NAME} vehicle/srv/GetVehicleDimensions.srv DEPENDENCIES builtin_interfaces - std_msgs geographic_msgs geometry_msgs shape_msgs + std_msgs unique_identifier_msgs ) diff --git a/autoware_adapi_v1_msgs/planning/msg/CooperationCommand.msg b/autoware_adapi_v1_msgs/planning/msg/CooperationCommand.msg new file mode 100644 index 0000000..392d91c --- /dev/null +++ b/autoware_adapi_v1_msgs/planning/msg/CooperationCommand.msg @@ -0,0 +1,2 @@ +unique_identifier_msgs/UUID uuid +autoware_adapi_v1_msgs/CooperationDecision cooperator diff --git a/autoware_adapi_v1_msgs/planning/msg/CooperationDecision.msg b/autoware_adapi_v1_msgs/planning/msg/CooperationDecision.msg new file mode 100644 index 0000000..95773fe --- /dev/null +++ b/autoware_adapi_v1_msgs/planning/msg/CooperationDecision.msg @@ -0,0 +1,7 @@ +uint8 UNKNOWN = 0 +uint8 DEACTIVATE = 1 +uint8 ACTIVATE = 2 +uint8 AUTONOMOUS = 3 +uint8 UNDECIDED = 4 + +uint8 decision diff --git a/autoware_adapi_v1_msgs/planning/msg/CooperationPolicy.msg b/autoware_adapi_v1_msgs/planning/msg/CooperationPolicy.msg new file mode 100644 index 0000000..d1aaaa7 --- /dev/null +++ b/autoware_adapi_v1_msgs/planning/msg/CooperationPolicy.msg @@ -0,0 +1,6 @@ +uint8 OPTIONAL = 1 +uint8 REQUIRED = 2 + +string behavior +string sequence +uint8 policy diff --git a/autoware_adapi_v1_msgs/planning/msg/CooperationStatus.msg b/autoware_adapi_v1_msgs/planning/msg/CooperationStatus.msg new file mode 100644 index 0000000..21f295d --- /dev/null +++ b/autoware_adapi_v1_msgs/planning/msg/CooperationStatus.msg @@ -0,0 +1,4 @@ +unique_identifier_msgs/UUID uuid +autoware_adapi_v1_msgs/CooperationDecision autonomous +autoware_adapi_v1_msgs/CooperationDecision cooperator +bool cancellable diff --git a/autoware_adapi_v1_msgs/planning/msg/PlanningBehavior.msg b/autoware_adapi_v1_msgs/planning/msg/PlanningBehavior.msg new file mode 100644 index 0000000..7924631 --- /dev/null +++ b/autoware_adapi_v1_msgs/planning/msg/PlanningBehavior.msg @@ -0,0 +1,16 @@ +# These constants are used in the behavior field of the SteeringFactor/VelocityFactor. +string AVOIDANCE = "avoidance" +string CROSSWALK = "crosswalk" +string EMERGENCY_OPERATION = "emergency-operation" +string INTERSECTION = "intersection" +string LANE_CHANGE = "lane-change" +string MERGE = "merge" +string NO_DRIVABLE_LANE = "no-drivable-lane" +string NO_STOPPING_AREA = "no-stopping-area" +string REAR_CHECK = "rear-check" +string ROUTE_OBSTACLE = "route-obstacle" +string SIDEWALK = "sidewalk" +string STOP_SIGN = "stop-sign" +string SURROUNDING_OBSTACLE = "surrounding-obstacle" +string TRAFFIC_SIGNAL = "traffic-signal" +string USER_DEFINED_DETECTION_AREA = "user-defined-attention-area" diff --git a/autoware_adapi_v1_msgs/planning/msg/PlanningSequence.msg b/autoware_adapi_v1_msgs/planning/msg/PlanningSequence.msg new file mode 100644 index 0000000..a4a613e --- /dev/null +++ b/autoware_adapi_v1_msgs/planning/msg/PlanningSequence.msg @@ -0,0 +1,5 @@ +# These constants are used in the sequence field of the SteeringFactor/VelocityFactor. + +# for avoidance behavior +string CHANGE = "change" +string RETURN = "return" diff --git a/autoware_adapi_v1_msgs/planning/msg/SteeringFactor.msg b/autoware_adapi_v1_msgs/planning/msg/SteeringFactor.msg index 846ae86..774cd8a 100644 --- a/autoware_adapi_v1_msgs/planning/msg/SteeringFactor.msg +++ b/autoware_adapi_v1_msgs/planning/msg/SteeringFactor.msg @@ -1,18 +1,6 @@ # constants for common use uint16 UNKNOWN = 0 -# constants for type -uint16 INTERSECTION = 1 -uint16 LANE_CHANGE = 2 -uint16 AVOIDANCE_PATH_CHANGE = 3 -uint16 AVOIDANCE_PATH_RETURN = 4 -uint16 STATION = 5 -uint16 PULL_OUT = 6 # Deprecated. Use START_PLANNER. -uint16 START_PLANNER = 6 -uint16 PULL_OVER = 7 # Deprecated. Use GOAL_PLANNER. -uint16 GOAL_PLANNER = 7 -uint16 EMERGENCY_OPERATION = 8 - # constants for direction uint16 LEFT = 1 uint16 RIGHT = 2 @@ -20,13 +8,34 @@ uint16 STRAIGHT = 3 # constants for status uint16 APPROACHING = 1 -uint16 TRYING = 2 uint16 TURNING = 3 # variables geometry_msgs/Pose[2] pose float32[2] distance -uint16 type uint16 direction uint16 status +string behavior +string sequence string detail +autoware_adapi_v1_msgs/CooperationStatus[<=1] cooperation + + + +# deprecated constants for type +uint16 INTERSECTION = 1 +uint16 LANE_CHANGE = 2 +uint16 AVOIDANCE_PATH_CHANGE = 3 +uint16 AVOIDANCE_PATH_RETURN = 4 +uint16 STATION = 5 +uint16 PULL_OUT = 6 # Deprecated. Use START_PLANNER. +uint16 START_PLANNER = 6 +uint16 PULL_OVER = 7 # Deprecated. Use GOAL_PLANNER. +uint16 GOAL_PLANNER = 7 +uint16 EMERGENCY_OPERATION = 8 + +# deprecated constants for status +uint16 TRYING = 2 + +# deprecated variables +uint16 type diff --git a/autoware_adapi_v1_msgs/planning/msg/VelocityFactor.msg b/autoware_adapi_v1_msgs/planning/msg/VelocityFactor.msg index 192a57e..62bb2a7 100644 --- a/autoware_adapi_v1_msgs/planning/msg/VelocityFactor.msg +++ b/autoware_adapi_v1_msgs/planning/msg/VelocityFactor.msg @@ -1,7 +1,22 @@ # constants for common use uint16 UNKNOWN = 0 -# constants for type +# constants for status +uint16 APPROACHING = 1 +uint16 STOPPED = 2 + +# variables +geometry_msgs/Pose pose +float32 distance +uint16 status +string behavior +string sequence +string detail +autoware_adapi_v1_msgs/CooperationStatus[<=1] cooperation + + + +# deprecated constants for type uint16 SURROUNDING_OBSTACLE = 1 uint16 ROUTE_OBSTACLE = 2 uint16 INTERSECTION = 3 @@ -20,13 +35,5 @@ uint16 AVOIDANCE = 15 uint16 EMERGENCY_STOP_OPERATION = 16 uint16 NO_DRIVABLE_LANE = 17 -# constants for status -uint16 APPROACHING = 1 -uint16 STOPPED = 2 - -# variables -geometry_msgs/Pose pose -float32 distance +# deprecated variables uint16 type -uint16 status -string detail diff --git a/autoware_adapi_v1_msgs/planning/srv/GetCooperationPolicies.srv b/autoware_adapi_v1_msgs/planning/srv/GetCooperationPolicies.srv new file mode 100644 index 0000000..9437c10 --- /dev/null +++ b/autoware_adapi_v1_msgs/planning/srv/GetCooperationPolicies.srv @@ -0,0 +1,3 @@ +--- +autoware_adapi_v1_msgs/ResponseStatus status +autoware_adapi_v1_msgs/CooperationPolicy[] policies diff --git a/autoware_adapi_v1_msgs/planning/srv/SetCooperationCommands.srv b/autoware_adapi_v1_msgs/planning/srv/SetCooperationCommands.srv new file mode 100644 index 0000000..18fb13d --- /dev/null +++ b/autoware_adapi_v1_msgs/planning/srv/SetCooperationCommands.srv @@ -0,0 +1,3 @@ +autoware_adapi_v1_msgs/CooperationCommand[] commands +--- +autoware_adapi_v1_msgs/ResponseStatus status diff --git a/autoware_adapi_v1_msgs/planning/srv/SetCooperationPolicies.srv b/autoware_adapi_v1_msgs/planning/srv/SetCooperationPolicies.srv new file mode 100644 index 0000000..7a08835 --- /dev/null +++ b/autoware_adapi_v1_msgs/planning/srv/SetCooperationPolicies.srv @@ -0,0 +1,3 @@ +autoware_adapi_v1_msgs/CooperationPolicy[] policies +--- +autoware_adapi_v1_msgs/ResponseStatus status