From 1bd3e616205badee1f2adfbc39b93ce31798b626 Mon Sep 17 00:00:00 2001 From: tkimura4 Date: Tue, 14 Jul 2020 13:56:11 +0900 Subject: [PATCH 01/71] Feature/awapi awiv adapter first pr (#685) * add base file of awapi_awiv_adapter * publish autoware status * update readme * fix readme * rename file * add relay topic * change msg name * publish autoware status * update readme * add new message * add lane change topic, obstacle avoidance topic * update readme * fix readme * add namespace * rename lane change available * fix readme * change pub hz * change topic name * change control mode and add gate_mode * fix readme * update readme Signed-off-by: tanaka3 --- awapi/awapi_awiv_adapter/CMakeLists.txt | 86 +++++++ awapi/awapi_awiv_adapter/Readme.md | 136 ++++++++++++ .../awapi_autoware_state_publisher.h | 63 ++++++ .../awapi_awiv_adapter/awapi_autoware_util.h | 79 +++++++ .../awapi_awiv_adapter_core.h | 96 ++++++++ .../awapi_lane_change_state_publisher.h | 45 ++++ ...awapi_obstacle_avoidance_state_publisher.h | 44 ++++ .../awapi_vehicle_state_publisher.h | 69 ++++++ .../launch/awapi_awiv_adapter.launch | 105 +++++++++ .../msg/AwapiAutowareStatus.msg | 8 + .../msg/AwapiVehicleStatus.msg | 16 ++ awapi/awapi_awiv_adapter/msg/Euler.msg | 3 + .../msg/LaneChangeStatus.msg | 4 + awapi/awapi_awiv_adapter/msg/Latlon.msg | 3 + .../msg/ObstacleAvoidanceStatus.msg | 3 + awapi/awapi_awiv_adapter/package.xml | 25 +++ .../src/awapi_autoware_state_publisher.cpp | 134 +++++++++++ .../src/awapi_autoware_util.cpp | 26 +++ .../src/awapi_awiv_adapter_core.cpp | 209 ++++++++++++++++++ .../src/awapi_awiv_adapter_node.cpp | 30 +++ .../src/awapi_lane_change_state_publisher.cpp | 85 +++++++ ...api_obstacle_avoidance_state_publisher.cpp | 72 ++++++ .../src/awapi_vehicle_state_publisher.cpp | 183 +++++++++++++++ 23 files changed, 1524 insertions(+) create mode 100644 awapi/awapi_awiv_adapter/CMakeLists.txt create mode 100644 awapi/awapi_awiv_adapter/Readme.md create mode 100644 awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.h create mode 100644 awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.h create mode 100644 awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.h create mode 100644 awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_lane_change_state_publisher.h create mode 100644 awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_obstacle_avoidance_state_publisher.h create mode 100644 awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_vehicle_state_publisher.h create mode 100644 awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch create mode 100644 awapi/awapi_awiv_adapter/msg/AwapiAutowareStatus.msg create mode 100644 awapi/awapi_awiv_adapter/msg/AwapiVehicleStatus.msg create mode 100644 awapi/awapi_awiv_adapter/msg/Euler.msg create mode 100644 awapi/awapi_awiv_adapter/msg/LaneChangeStatus.msg create mode 100644 awapi/awapi_awiv_adapter/msg/Latlon.msg create mode 100644 awapi/awapi_awiv_adapter/msg/ObstacleAvoidanceStatus.msg create mode 100644 awapi/awapi_awiv_adapter/package.xml create mode 100644 awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp create mode 100644 awapi/awapi_awiv_adapter/src/awapi_autoware_util.cpp create mode 100644 awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp create mode 100644 awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_node.cpp create mode 100644 awapi/awapi_awiv_adapter/src/awapi_lane_change_state_publisher.cpp create mode 100644 awapi/awapi_awiv_adapter/src/awapi_obstacle_avoidance_state_publisher.cpp create mode 100644 awapi/awapi_awiv_adapter/src/awapi_vehicle_state_publisher.cpp diff --git a/awapi/awapi_awiv_adapter/CMakeLists.txt b/awapi/awapi_awiv_adapter/CMakeLists.txt new file mode 100644 index 0000000000000..d464d5e6ed4ac --- /dev/null +++ b/awapi/awapi_awiv_adapter/CMakeLists.txt @@ -0,0 +1,86 @@ +cmake_minimum_required(VERSION 3.0.2) +project(awapi_awiv_adapter) + +add_compile_options(-std=c++14) + +find_package(catkin REQUIRED COMPONENTS + message_generation + message_runtime + roscpp + autoware_system_msgs + autoware_planning_msgs + autoware_control_msgs + autoware_vehicle_msgs + diagnostic_msgs + geometry_msgs + std_msgs + sensor_msgs + tf2 + tf2_geometry_msgs +) + +add_message_files( + DIRECTORY msg + FILES + Latlon.msg + Euler.msg + AwapiVehicleStatus.msg + AwapiAutowareStatus.msg + LaneChangeStatus.msg + ObstacleAvoidanceStatus.msg +) + +generate_messages( + DEPENDENCIES + std_msgs + geometry_msgs + diagnostic_msgs + autoware_planning_msgs +) + +catkin_package( + INCLUDE_DIRS include +) + +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + +add_executable(awapi_awiv_adapter + src/awapi_awiv_adapter_node.cpp + src/awapi_awiv_adapter_core.cpp + src/awapi_vehicle_state_publisher.cpp + src/awapi_autoware_state_publisher.cpp + src/awapi_lane_change_state_publisher.cpp + src/awapi_obstacle_avoidance_state_publisher.cpp + src/awapi_autoware_util.cpp +) + +target_link_libraries(awapi_awiv_adapter + ${catkin_LIBRARIES} +) + +add_dependencies(awapi_awiv_adapter + ${catkin_EXPORTED_TARGETS} +) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) + +install( + DIRECTORY + launch + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +install(TARGETS awapi_awiv_adapter + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +if(CATKIN_ENABLE_TESTING) + find_package(rostest REQUIRED) +endif() diff --git a/awapi/awapi_awiv_adapter/Readme.md b/awapi/awapi_awiv_adapter/Readme.md new file mode 100644 index 0000000000000..cbfcc92265677 --- /dev/null +++ b/awapi/awapi_awiv_adapter/Readme.md @@ -0,0 +1,136 @@ +# AWAPI_AWIV_ADAPTER + +## get topic + +### /awapi/vehicle/get/status + +- get vehicle status +- MessageType: awapi_awiv_adapter/AwapiVehicleStatus.msg + +| type | name | unit | note | +| :------------------------ | :----------------------- | :-------------------------------------------- | :------------------------------------------- | +| std_msgs/Header | header | | | +| geometry_msgs/Pose | pose | [m](position) | | +| awapi_awiv_adapter/Euler | eulerangle | [rad] | roll/pitch/yaw | +| awapi_awiv_adapter/Latlon | latlon | | lat/lon/alt | +| float64 | velocity | [m/s] | | +| float64 | acceleration | [m/ss] | calculate from velocity in awapi_adapter | +| float64 | steering | [rad] | | +| float64 | steering_velocity | [rad/s] | calculate from steering in awapi_adapter | +| float64 | angular_velocity | [rad/s] | | +| int32 | gear | accroding to autoware_vehicle_msgs/Shift | | +| float64 | distance_to_empty | | **not implemented** | +| int32 | turn_signal | accroding to autoware_vehicle_msgs/TurnSignal | | +| float64 | target_velocity | [m/s] | | +| float64 | target_acceleration | [m/ss] | | +| float64 | target_steering | [rad] | | +| float64 | target_steering_velocity | [rad/s] | | + +### /awapi/autoware/get/status + +- get autoware status +- MessageType: awapi_awiv_adapter/AwapiVehicleStatus.msg + +| type | name | unit | note | +| :--------------------------------- | :------------------- | :--------------------------------------------- | :----------------------------------------------------- | +| std_msgs/Header | header | | | +| string | autoware_state | | | +| int32 | control_mode | accroding to autoware_vehicle_msgs/ControlMode | manual/auto (changed by /awapi/autoware/put/engage) | +| int32 | gate_mode | autoware_vehicle_msgs/GateMode | auto/remote (it is valid only when control_mode=auto)) | +| bool | emergency_stopped | True in emergency mode | | +| StopReasonArray??? | stop_reason | | **not implemented** | +| diagnostic_msgs/DiagnosticStatus[] | diagnostics | | | +| bool | autonomous_overriden | | **not implemented** | + +### /awapi/autoware/get/route + +- get route +- MessageType: autoware_planning_msgs/Route + +### /awapi/prediction/get/objects + +- get predicted object +- MessageType: autoware_perception_msgs/DynamicObjectArray + +### /awapi/lane_change/get/status + +- get lane change information +- MessageType: awapi_awiv_adapter/LaneChangeStatus.msg + +| type | name | unit | note | +| :-------------------------- | :-------------------------- | :--------------------------------------- | :--------------------------------------------------------------------------------- | +| std_msgs/Header | header | | | +| bool | force_lane_change_available | True when lane change is avilable | available: Physically lane changeable state (do not consider other vehicle) | +| bool | lane_change_ready | True when lane change is ready | ready: State that ego-vehicle can change lane without collision with other vehicle | +| autoware_planning_msgs/Path | candidate_path | according to autoware_planning_msgs/Path | | + +### /awapi/object_avoidance/get/status + +- get obstacle avoidance information +- MessageType: awapi_awiv_adapter/ObstacleAvoidanceStatus.msg + +| type | name | unit | note | +| :-------------------------------- | :----------------------- | :--------------------------------------------- | :---------------------------------------------------- | +| std_msgs/Header | header | | | +| bool | obstacle_avoidance_ready | True when obstacle avoidance is ready | | +| autoware_planning_msgs/Trajectory | candidate_path | according to autoware_planning_msgs/Trajectory | Msg type is different from lane change candidate path | + +### /awapi/traffic_light/get/status + +- get recognition result of traffic light +- MessageType: autoware_perception_msgs/TrafficLightStateArray + +## put topic + +### /awapi/vehicle/put/velocity + +- set upper velocity +- MessageType: std_msgs/Float32 + +### /awapi/autoware/put/gate_mode + +- send gate mode (auto/remote) +- MessageType: autoware_control_msgs/GateMode + +### /awapi/autoware/put/emergency + +- send emergency signal +- MessageType: std_msgs/Bool +- **Emergency stop does work only in Remote Mode. (not work in Auto Mode)** + +### /awapi/autoware/put/engage + +- send engage signal (both of autoware/engage and vehicle/engage) +- MessageType: std_msgs/Bool + +### /awapi/autoware/put/goal_pose + +- send goal pose +- MessageType: geometry_msgs/PoseStamped + +### /awapi/lane_change/put/approval + +- send lane change approval flag +- send True: start lane change when **lane_change_ready** is true +- MessageType: std_msgs/Bool + +### /awapi/lane_change/put/force + +- send force lane change flag +- send True: start lane change when **force_lane_change_available** is true +- MessageType: std_msgs/Bool + +### /awapi/object_avoidance/put/approval + +- send object avoidance approval flag +- MessageType: std_msgs/Bool + +### /awapi/object_avoidance/put/force + +- send force object avoidance flag +- **not implemented (Autoware does not have corresponded topic)** + +### /awapi/traffic_light/put/traffic_light + +- Overwrite the recognition result of traffic light +- **not implemented (Autoware does not have corresponded topic)** diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.h b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.h new file mode 100644 index 0000000000000..6ead50646ca06 --- /dev/null +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.h @@ -0,0 +1,63 @@ +/* + * Copyright 2020 Tier IV, Inc. All rights reserved. + * + * 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. + */ + +#include +#include + +namespace autoware_api +{ +class AutowareIvAutowareStatePublisher +{ +public: + AutowareIvAutowareStatePublisher(); + void statePublisher(const AutowareInfo & aw_info); + +private: + // node handle + ros::NodeHandle nh_; + ros::NodeHandle pnh_; + + // publisher + ros::Publisher pub_state_; + + void getAutowareStateInfo( + const autoware_system_msgs::AutowareState::ConstPtr & autoware_state_ptr, + awapi_awiv_adapter::AwapiAutowareStatus * status); + void getControlModeInfo( + const autoware_vehicle_msgs::ControlMode::ConstPtr & control_mode_ptr, + awapi_awiv_adapter::AwapiAutowareStatus * status); + void getGateModeInfo( + const autoware_control_msgs::GateMode::ConstPtr & gate_mode_ptr, + awapi_awiv_adapter::AwapiAutowareStatus * status); + void getIsEmergencyInfo( + const std_msgs::Bool::ConstPtr & is_emergency_ptr, + awapi_awiv_adapter::AwapiAutowareStatus * status); + // TODO: not implemented + /* + void getStopReasonInfo( + const - & stop_reason_ptr, + awapi_awiv_adapter::AwapiAutowareStatus * status); + */ + void getDiagInfo( + const diagnostic_msgs::DiagnosticArray::ConstPtr & diag_ptr, + awapi_awiv_adapter::AwapiAutowareStatus * status); + + void getAutoDriveEnableInfo( + const std_msgs::Bool::ConstPtr & auto_drive_enable_ptr, + awapi_awiv_adapter::AwapiAutowareStatus * status); +}; + +} // namespace autoware_api \ No newline at end of file diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.h b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.h new file mode 100644 index 0000000000000..071609d67524b --- /dev/null +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.h @@ -0,0 +1,79 @@ +/* + * Copyright 2020 Tier IV, Inc. All rights reserved. + * + * 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. + */ + +#pragma once + +#ifndef AWAPI_AUTOWARE_UTIL_H +#define AWAPI_AUTOWARE_UTIL_H + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace autoware_api +{ +struct AutowareInfo +{ + std::shared_ptr current_pose_ptr; + autoware_vehicle_msgs::Steering::ConstPtr steer_ptr; + autoware_vehicle_msgs::VehicleCommand::ConstPtr vehicle_cmd_ptr; + autoware_vehicle_msgs::TurnSignal::ConstPtr turn_signal_ptr; + geometry_msgs::TwistStamped::ConstPtr twist_ptr; + autoware_vehicle_msgs::ShiftStamped::ConstPtr gear_ptr; + sensor_msgs::NavSatFix::ConstPtr nav_sat_ptr; + autoware_system_msgs::AutowareState::ConstPtr autoware_state_ptr; + autoware_vehicle_msgs::ControlMode::ConstPtr control_mode_ptr; + autoware_control_msgs::GateMode::ConstPtr gate_mode_ptr; + std_msgs::Bool::ConstPtr is_emergency_ptr; + // TODO ?? stop_reason_ptr; + diagnostic_msgs::DiagnosticArray::ConstPtr diagnostic_ptr; + std_msgs::Bool::ConstPtr autodrive_enable_ptr; + std_msgs::Bool::ConstPtr lane_change_available_ptr; + std_msgs::Bool::ConstPtr lane_change_ready_ptr; + autoware_planning_msgs::Path::ConstPtr lane_change_candidate_ptr; + std_msgs::Bool::ConstPtr obstacle_avoid_ready_ptr; + autoware_planning_msgs::Trajectory::ConstPtr obstacle_avoid_candidate_ptr; +}; + +template +T getParam(const ros::NodeHandle & nh, const std::string & key, const T & default_value) +{ + T value; + nh.param(key, value, default_value); + return value; +} + +double lowpass_filter(const double current_value, const double prev_value, const double gain); + +} // namespace autoware_api + +#endif // AWAPI_AUTOWARE_UTIL_H diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.h b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.h new file mode 100644 index 0000000000000..1ae499975c4fd --- /dev/null +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.h @@ -0,0 +1,96 @@ +/* + * Copyright 2020 Tier IV, Inc. All rights reserved. + * + * 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. + */ + +#include +#include +#include +#include +#include + +namespace autoware_api +{ +class AutowareIvAdapter +{ +public: + AutowareIvAdapter(); + +private: + // node handle + ros::NodeHandle nh_; + ros::NodeHandle pnh_; + + // subscriber + ros::Subscriber sub_steer_; + ros::Subscriber sub_vehicle_cmd_; + ros::Subscriber sub_turn_signal_cmd_; + ros::Subscriber sub_twist_; + ros::Subscriber sub_gear_; + ros::Subscriber sub_nav_sat_; + ros::Subscriber sub_autoware_state_; + ros::Subscriber sub_control_mode_; + ros::Subscriber sub_gate_mode_; + ros::Subscriber sub_emergency_; + ros::Subscriber sub_stop_reason_; + ros::Subscriber sub_diagnostics_; + ros::Subscriber sub_autodrive_enable_; + ros::Subscriber sub_lane_change_available_; + ros::Subscriber sub_lane_change_ready_; + ros::Subscriber sub_lane_change_candidate_; + ros::Subscriber sub_obstacle_avoid_ready_; + ros::Subscriber sub_obstacle_avoid_candidate_; + // timer + ros::Timer timer_; + + // tf + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + + // callback function + void callbackSteer(const autoware_vehicle_msgs::Steering::ConstPtr & msg_ptr); + void callbackVehicleCmd(const autoware_vehicle_msgs::VehicleCommand::ConstPtr & msg_ptr); + void callbackTurnSignal(const autoware_vehicle_msgs::TurnSignal::ConstPtr & msg_ptr); + void callbackTwist(const geometry_msgs::TwistStamped::ConstPtr & msg_ptr); + void callbackGear(const autoware_vehicle_msgs::ShiftStamped::ConstPtr & msg_ptr); + void callbackNavSat(const sensor_msgs::NavSatFix::ConstPtr & msg_ptr); + void callbackAutowareState(const autoware_system_msgs::AutowareState::ConstPtr & msg_ptr); + void callbackControlMode(const autoware_vehicle_msgs::ControlMode::ConstPtr & msg_ptr); + void callbackGateMode(const autoware_control_msgs::GateMode::ConstPtr & msg_ptr); + void callbackIsEmergency(const std_msgs::Bool::ConstPtr & msg_ptr); + // void callbackStopReason(); // TODO: not implemented + void callbackDiagnostics(const diagnostic_msgs::DiagnosticArray::ConstPtr & msg_ptr); + void callbackAutodriveEnable(const std_msgs::Bool::ConstPtr & msg_ptr); + void callbackLaneChangeAvailable(const std_msgs::Bool::ConstPtr & msg_ptr); + void callbackLaneChangeReady(const std_msgs::Bool::ConstPtr & msg_ptr); + void callbackLaneChangeCandidatePath(const autoware_planning_msgs::Path::ConstPtr & msg_ptr); + void callbackLaneObstacleAvoidReady(const std_msgs::Bool::ConstPtr & msg_ptr); + void callbackLaneObstacleAvoidCandidatePath( + const autoware_planning_msgs::Trajectory::ConstPtr & msg_ptr); + + // timer function + void timerCallback(const ros::TimerEvent & e); + + void getCurrentPose(); + + // parameter + AutowareInfo aw_info_; + std::unique_ptr vehicle_state_publisher_; + std::unique_ptr autoware_state_publisher_; + std::unique_ptr lane_change_state_publisher_; + std::unique_ptr obstacle_avoidance_state_publisher_; + double status_pub_hz_; +}; + +} // namespace autoware_api \ No newline at end of file diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_lane_change_state_publisher.h b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_lane_change_state_publisher.h new file mode 100644 index 0000000000000..13f0ee5d6224f --- /dev/null +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_lane_change_state_publisher.h @@ -0,0 +1,45 @@ +/* + * Copyright 2020 Tier IV, Inc. All rights reserved. + * + * 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. + */ + +#include +#include + +namespace autoware_api +{ +class AutowareIvLaneChangeStatePublisher +{ +public: + AutowareIvLaneChangeStatePublisher(); + void statePublisher(const AutowareInfo & aw_info); + +private: + // node handle + ros::NodeHandle nh_; + ros::NodeHandle pnh_; + + // publisher + ros::Publisher pub_state_; + + void getLaneChangeAvailableInfo( + const std_msgs::Bool::ConstPtr & available_ptr, awapi_awiv_adapter::LaneChangeStatus * status); + void getLaneChangeReadyInfo( + const std_msgs::Bool::ConstPtr & ready_ptr, awapi_awiv_adapter::LaneChangeStatus * status); + void getCandidatePathInfo( + const autoware_planning_msgs::Path::ConstPtr & path_ptr, + awapi_awiv_adapter::LaneChangeStatus * status); +}; + +} // namespace autoware_api \ No newline at end of file diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_obstacle_avoidance_state_publisher.h b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_obstacle_avoidance_state_publisher.h new file mode 100644 index 0000000000000..e1ccfad73bee7 --- /dev/null +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_obstacle_avoidance_state_publisher.h @@ -0,0 +1,44 @@ +/* + * Copyright 2020 Tier IV, Inc. All rights reserved. + * + * 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. + */ + +#include +#include + +namespace autoware_api +{ +class AutowareIvObstacleAvoidanceStatePublisher +{ +public: + AutowareIvObstacleAvoidanceStatePublisher(); + void statePublisher(const AutowareInfo & aw_info); + +private: + // node handle + ros::NodeHandle nh_; + ros::NodeHandle pnh_; + + // publisher + ros::Publisher pub_state_; + + void getObstacleAvoidReadyInfo( + const std_msgs::Bool::ConstPtr & ready_ptr, + awapi_awiv_adapter::ObstacleAvoidanceStatus * status); + void getCandidatePathInfo( + const autoware_planning_msgs::Trajectory::ConstPtr & path_ptr, + awapi_awiv_adapter::ObstacleAvoidanceStatus * status); +}; + +} // namespace autoware_api \ No newline at end of file diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_vehicle_state_publisher.h b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_vehicle_state_publisher.h new file mode 100644 index 0000000000000..66e0af13a99a3 --- /dev/null +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_vehicle_state_publisher.h @@ -0,0 +1,69 @@ +/* + * Copyright 2020 Tier IV, Inc. All rights reserved. + * + * 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. + */ + +#include +#include + +namespace autoware_api +{ +class AutowareIvVehicleStatePublisher +{ +public: + AutowareIvVehicleStatePublisher(); + void statePublisher(const AutowareInfo & aw_info); + +private: + // node handle + ros::NodeHandle nh_; + ros::NodeHandle pnh_; + + // publisher + ros::Publisher pub_state_; + + void getPoseInfo( + const std::shared_ptr & pose_ptr, + awapi_awiv_adapter::AwapiVehicleStatus * status); + void getSteerInfo( + const autoware_vehicle_msgs::Steering::ConstPtr & steer_ptr, + awapi_awiv_adapter::AwapiVehicleStatus * status); + void getVehicleCmdInfo( + const autoware_vehicle_msgs::VehicleCommand::ConstPtr & vehicle_cmd_ptr, + awapi_awiv_adapter::AwapiVehicleStatus * status); + void getTurnSignalInfo( + const autoware_vehicle_msgs::TurnSignal::ConstPtr & turn_signal_ptr, + awapi_awiv_adapter::AwapiVehicleStatus * status); + void getTwistInfo( + const geometry_msgs::TwistStamped::ConstPtr & twist_ptr, + awapi_awiv_adapter::AwapiVehicleStatus * status); + void getGearInfo( + const autoware_vehicle_msgs::ShiftStamped::ConstPtr & gear_ptr, + awapi_awiv_adapter::AwapiVehicleStatus * status); + void getGpsInfo( + const sensor_msgs::NavSatFix::ConstPtr & nav_sat_ptr, + awapi_awiv_adapter::AwapiVehicleStatus * status); + + //parameters + geometry_msgs::TwistStamped::ConstPtr previous_twist_ptr_; + autoware_vehicle_msgs::Steering::ConstPtr previous_steer_ptr_; + double prev_accel_; + double prev_steer_vel_; + + // defined value + const double accel_lowpass_gain_ = 0.2; + const double steer_vel_lowpass_gain_ = 0.2; +}; + +} // namespace autoware_api \ No newline at end of file diff --git a/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch b/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch new file mode 100644 index 0000000000000..4c2cbb71b90bd --- /dev/null +++ b/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch @@ -0,0 +1,105 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/awapi/awapi_awiv_adapter/msg/AwapiAutowareStatus.msg b/awapi/awapi_awiv_adapter/msg/AwapiAutowareStatus.msg new file mode 100644 index 0000000000000..4d6f73c105ff2 --- /dev/null +++ b/awapi/awapi_awiv_adapter/msg/AwapiAutowareStatus.msg @@ -0,0 +1,8 @@ +Header header +string autoware_state +int32 control_mode +int32 gate_mode +bool emergency_stopped +string stop_reason +diagnostic_msgs/DiagnosticStatus[] diagnostics +bool autonomous_overriden diff --git a/awapi/awapi_awiv_adapter/msg/AwapiVehicleStatus.msg b/awapi/awapi_awiv_adapter/msg/AwapiVehicleStatus.msg new file mode 100644 index 0000000000000..e16b949611809 --- /dev/null +++ b/awapi/awapi_awiv_adapter/msg/AwapiVehicleStatus.msg @@ -0,0 +1,16 @@ +std_msgs/Header header +geometry_msgs/Pose pose +awapi_awiv_adapter/Euler eulerangle +awapi_awiv_adapter/Latlon latlon +float64 velocity +float64 acceleration +float64 steering +float64 steering_velocity +float64 angular_velocity +int32 gear +float64 distance_to_empty +int32 turn_signal +float64 target_velocity +float64 target_acceleration +float64 target_steering +float64 target_steering_velocity diff --git a/awapi/awapi_awiv_adapter/msg/Euler.msg b/awapi/awapi_awiv_adapter/msg/Euler.msg new file mode 100644 index 0000000000000..c963632f3dafe --- /dev/null +++ b/awapi/awapi_awiv_adapter/msg/Euler.msg @@ -0,0 +1,3 @@ +float64 roll +float64 pitch +float64 yaw diff --git a/awapi/awapi_awiv_adapter/msg/LaneChangeStatus.msg b/awapi/awapi_awiv_adapter/msg/LaneChangeStatus.msg new file mode 100644 index 0000000000000..b49435e8fac59 --- /dev/null +++ b/awapi/awapi_awiv_adapter/msg/LaneChangeStatus.msg @@ -0,0 +1,4 @@ +Header header +bool force_lane_change_available +bool lane_change_ready +autoware_planning_msgs/Path candidate_path diff --git a/awapi/awapi_awiv_adapter/msg/Latlon.msg b/awapi/awapi_awiv_adapter/msg/Latlon.msg new file mode 100644 index 0000000000000..798b6734375d0 --- /dev/null +++ b/awapi/awapi_awiv_adapter/msg/Latlon.msg @@ -0,0 +1,3 @@ +float64 lat +float64 lon +float64 alt diff --git a/awapi/awapi_awiv_adapter/msg/ObstacleAvoidanceStatus.msg b/awapi/awapi_awiv_adapter/msg/ObstacleAvoidanceStatus.msg new file mode 100644 index 0000000000000..408d3c954eb2a --- /dev/null +++ b/awapi/awapi_awiv_adapter/msg/ObstacleAvoidanceStatus.msg @@ -0,0 +1,3 @@ +Header header +bool obstacle_avoidance_ready +autoware_planning_msgs/Trajectory candidate_path diff --git a/awapi/awapi_awiv_adapter/package.xml b/awapi/awapi_awiv_adapter/package.xml new file mode 100644 index 0000000000000..b70c42a75f04c --- /dev/null +++ b/awapi/awapi_awiv_adapter/package.xml @@ -0,0 +1,25 @@ + + + awapi_awiv_adapter + 0.1.0 + The awapi_awiv_adapter package + Tomoya Kimura + Tomoya Kimura + BSD + + catkin + + message_generation + message_runtime + roscpp + autoware_system_msgs + autoware_planning_msgs + autoware_control_msgs + autoware_vehicle_msgs + diagnostic_msgs/ + geometry_msgs + std_msgs + sensor_msgs + tf2 + tf2_geometry_msgs + diff --git a/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp b/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp new file mode 100644 index 0000000000000..41f0c927382e9 --- /dev/null +++ b/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp @@ -0,0 +1,134 @@ +/* + * Copyright 2020 Tier IV, Inc. All rights reserved. + * + * 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. + */ + +#include + +namespace autoware_api +{ +AutowareIvAutowareStatePublisher::AutowareIvAutowareStatePublisher() : nh_(), pnh_("~") +{ + // publisher + pub_state_ = pnh_.advertise("output/autoware_status", 1); +} + +void AutowareIvAutowareStatePublisher::statePublisher(const AutowareInfo & aw_info) +{ + awapi_awiv_adapter::AwapiAutowareStatus status; + + //input header + status.header.frame_id = "base_link"; + status.header.stamp = ros::Time::now(); + + // get all info + getAutowareStateInfo(aw_info.autoware_state_ptr, &status); + getControlModeInfo(aw_info.control_mode_ptr, &status); + getGateModeInfo(aw_info.gate_mode_ptr, &status); + getIsEmergencyInfo(aw_info.is_emergency_ptr, &status); + // getStopReasonInfo(aw_info.stop_reason_ptr, &status); // TODO: not implemented + getDiagInfo(aw_info.diagnostic_ptr, &status); + getAutoDriveEnableInfo(aw_info.autodrive_enable_ptr, &status); + + // publish info + pub_state_.publish(status); +} + +void AutowareIvAutowareStatePublisher::getAutowareStateInfo( + const autoware_system_msgs::AutowareState::ConstPtr & autoware_state_ptr, + awapi_awiv_adapter::AwapiAutowareStatus * status) +{ + if (!autoware_state_ptr) { + ROS_WARN_STREAM_THROTTLE(5.0, "[AutowareIvAutowareStatePublisher] autoware_state is nullptr"); + return; + } + + // get autoware_state + status->autoware_state = autoware_state_ptr->state; +} + +void AutowareIvAutowareStatePublisher::getControlModeInfo( + const autoware_vehicle_msgs::ControlMode::ConstPtr & control_mode_ptr, + awapi_awiv_adapter::AwapiAutowareStatus * status) +{ + if (!control_mode_ptr) { + ROS_WARN_STREAM_THROTTLE(5.0, "[AutowareIvAutowareStatePublisher] control mode is nullptr"); + return; + } + + // get control mode + status->control_mode = control_mode_ptr->data; +} + +void AutowareIvAutowareStatePublisher::getGateModeInfo( + const autoware_control_msgs::GateMode::ConstPtr & gate_mode_ptr, + awapi_awiv_adapter::AwapiAutowareStatus * status) +{ + if (!gate_mode_ptr) { + ROS_WARN_STREAM_THROTTLE(5.0, "[AutowareIvAutowareStatePublisher] gate mode is nullptr"); + return; + } + + // get control mode + status->gate_mode = gate_mode_ptr->data; +} + +void AutowareIvAutowareStatePublisher::getIsEmergencyInfo( + const std_msgs::Bool::ConstPtr & is_emergency_ptr, + awapi_awiv_adapter::AwapiAutowareStatus * status) +{ + if (!is_emergency_ptr) { + ROS_WARN_STREAM_THROTTLE(5.0, "[AutowareIvAutowareStatePublisher] is_emergency is nullptr"); + return; + } + + // get emergency + status->emergency_stopped = is_emergency_ptr->data; +} + +// TODO: not implemented +/* + void AutowareIvAutowareStatePublisher::getStopReasonInfo( + const - & stop_reason_ptr, + awapi_awiv_adapter::AwapiAutowareStatus * status){} + */ + +void AutowareIvAutowareStatePublisher::getDiagInfo( + const diagnostic_msgs::DiagnosticArray::ConstPtr & diag_ptr, + awapi_awiv_adapter::AwapiAutowareStatus * status) +{ + if (!diag_ptr) { + ROS_WARN_STREAM_THROTTLE(5.0, "[AutowareIvAutowareStatePublisher] diagnostics is nullptr"); + return; + } + + // get diag + status->diagnostics = diag_ptr->status; +} + +void AutowareIvAutowareStatePublisher::getAutoDriveEnableInfo( + const std_msgs::Bool::ConstPtr & auto_drive_enable_ptr, + awapi_awiv_adapter::AwapiAutowareStatus * status) +{ + if (!auto_drive_enable_ptr) { + ROS_WARN_STREAM_THROTTLE( + 5.0, "[AutowareIvAutowareStatePublisher] auto_drive_enable is nullptr"); + return; + } + + // get auto_drive_enable (autonomous_overriden = not auto_drive_enable) + status->autonomous_overriden = auto_drive_enable_ptr->data ? false : true; +} + +} // namespace autoware_api \ No newline at end of file diff --git a/awapi/awapi_awiv_adapter/src/awapi_autoware_util.cpp b/awapi/awapi_awiv_adapter/src/awapi_autoware_util.cpp new file mode 100644 index 0000000000000..b6341ed8a5528 --- /dev/null +++ b/awapi/awapi_awiv_adapter/src/awapi_autoware_util.cpp @@ -0,0 +1,26 @@ +/* + * Copyright 2020 Tier IV, Inc. All rights reserved. + * + * 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. + */ + +#include + +namespace autoware_api +{ +double lowpass_filter(const double current_value, const double prev_value, const double gain) +{ + return gain * prev_value + (1.0 - gain) * current_value; +} + +} // namespace autoware_api \ No newline at end of file diff --git a/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp b/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp new file mode 100644 index 0000000000000..f342eb6afae9e --- /dev/null +++ b/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp @@ -0,0 +1,209 @@ +/* + * Copyright 2020 Tier IV, Inc. All rights reserved. + * + * 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. + */ + +#include + +namespace autoware_api +{ +AutowareIvAdapter::AutowareIvAdapter() : nh_(), pnh_("~"), tf_listener_(tf_buffer_) +{ + // get param + pnh_.param("status_pub_hz", status_pub_hz_, 5.0); + // setup instance + vehicle_state_publisher_ = std::make_unique(); + autoware_state_publisher_ = std::make_unique(); + lane_change_state_publisher_ = std::make_unique(); + obstacle_avoidance_state_publisher_ = + std::make_unique(); + + // subscriber + sub_steer_ = pnh_.subscribe("input/steer", 1, &AutowareIvAdapter::callbackSteer, this); + sub_vehicle_cmd_ = + pnh_.subscribe("input/vehicle_cmd", 1, &AutowareIvAdapter::callbackVehicleCmd, this); + sub_turn_signal_cmd_ = + pnh_.subscribe("input/turn_signal", 1, &AutowareIvAdapter::callbackTurnSignal, this); + sub_twist_ = pnh_.subscribe("input/twist", 1, &AutowareIvAdapter::callbackTwist, this); + sub_gear_ = pnh_.subscribe("input/gear", 1, &AutowareIvAdapter::callbackGear, this); + sub_nav_sat_ = pnh_.subscribe("input/nav_sat", 1, &AutowareIvAdapter::callbackNavSat, this); + sub_autoware_state_ = + pnh_.subscribe("input/autoware_state", 1, &AutowareIvAdapter::callbackAutowareState, this); + sub_control_mode_ = + pnh_.subscribe("input/control_mode", 1, &AutowareIvAdapter::callbackControlMode, this); + sub_gate_mode_ = pnh_.subscribe("input/gate_mode", 1, &AutowareIvAdapter::callbackGateMode, this); + sub_emergency_ = + pnh_.subscribe("input/is_emergency", 1, &AutowareIvAdapter::callbackIsEmergency, this); + //TODO: not implemented + /* + sub_stop_reason_ = + pnh_.subscribe("input/stop_reason", 1, &AutowareIvAdapter::callbackStopReason, this); + */ + sub_diagnostics_ = + pnh_.subscribe("input/diagnostics", 1, &AutowareIvAdapter::callbackDiagnostics, this); + sub_autodrive_enable_ = + pnh_.subscribe("input/autodrive_enable", 1, &AutowareIvAdapter::callbackAutodriveEnable, this); + sub_lane_change_available_ = pnh_.subscribe( + "input/lane_change_avaiable", 1, &AutowareIvAdapter::callbackLaneChangeAvailable, this); + sub_lane_change_ready_ = + pnh_.subscribe("input/lane_change_ready", 1, &AutowareIvAdapter::callbackLaneChangeReady, this); + sub_lane_change_candidate_ = pnh_.subscribe( + "input/lane_change_candidate_path", 1, &AutowareIvAdapter::callbackLaneChangeCandidatePath, + this); + sub_obstacle_avoid_ready_ = pnh_.subscribe( + "input/obstacle_avoid_ready", 1, &AutowareIvAdapter::callbackLaneObstacleAvoidReady, this); + sub_obstacle_avoid_candidate_ = pnh_.subscribe( + "input/obstacle_avoid_candidate_path", 1, + &AutowareIvAdapter::callbackLaneObstacleAvoidCandidatePath, this); + + // timer + timer_ = + nh_.createTimer(ros::Duration(1.0 / status_pub_hz_), &AutowareIvAdapter::timerCallback, this); +} + +void AutowareIvAdapter::timerCallback(const ros::TimerEvent & e) +{ + // get current pose + getCurrentPose(); + + // publish vehicle state + vehicle_state_publisher_->statePublisher(aw_info_); + + // publish autoware state + autoware_state_publisher_->statePublisher(aw_info_); + + // publish lane change state + lane_change_state_publisher_->statePublisher(aw_info_); + + // publish obstacle_avoidance state + obstacle_avoidance_state_publisher_->statePublisher(aw_info_); +} + +void AutowareIvAdapter::callbackSteer(const autoware_vehicle_msgs::Steering::ConstPtr & msg_ptr) +{ + aw_info_.steer_ptr = msg_ptr; +} + +void AutowareIvAdapter::callbackVehicleCmd( + const autoware_vehicle_msgs::VehicleCommand::ConstPtr & msg_ptr) +{ + aw_info_.vehicle_cmd_ptr = msg_ptr; +} + +void AutowareIvAdapter::callbackTurnSignal( + const autoware_vehicle_msgs::TurnSignal::ConstPtr & msg_ptr) +{ + aw_info_.turn_signal_ptr = msg_ptr; +} + +void AutowareIvAdapter::callbackTwist(const geometry_msgs::TwistStamped::ConstPtr & msg_ptr) +{ + aw_info_.twist_ptr = msg_ptr; +} + +void AutowareIvAdapter::callbackGear(const autoware_vehicle_msgs::ShiftStamped::ConstPtr & msg_ptr) +{ + aw_info_.gear_ptr = msg_ptr; +} + +void AutowareIvAdapter::callbackNavSat(const sensor_msgs::NavSatFix::ConstPtr & msg_ptr) +{ + aw_info_.nav_sat_ptr = msg_ptr; +} + +void AutowareIvAdapter::getCurrentPose() +{ + try { + auto transform = tf_buffer_.lookupTransform("map", "base_link", ros::Time(0)); + geometry_msgs::PoseStamped ps; + ps.header = transform.header; + ps.pose.position.x = transform.transform.translation.x; + ps.pose.position.y = transform.transform.translation.y; + ps.pose.position.z = transform.transform.translation.z; + ps.pose.orientation = transform.transform.rotation; + aw_info_.current_pose_ptr = std::make_shared(ps); + } catch (tf2::TransformException & ex) { + ROS_WARN_STREAM_THROTTLE(2.0, "[awapi_autoware_adapter] cannot get self pose"); + } +} + +void AutowareIvAdapter::callbackAutowareState( + const autoware_system_msgs::AutowareState::ConstPtr & msg_ptr) +{ + aw_info_.autoware_state_ptr = msg_ptr; +} +void AutowareIvAdapter::callbackControlMode( + const autoware_vehicle_msgs::ControlMode::ConstPtr & msg_ptr) +{ + aw_info_.control_mode_ptr = msg_ptr; +} + +void AutowareIvAdapter::callbackGateMode(const autoware_control_msgs::GateMode::ConstPtr & msg_ptr) +{ + aw_info_.gate_mode_ptr = msg_ptr; +} + +void AutowareIvAdapter::callbackIsEmergency(const std_msgs::Bool::ConstPtr & msg_ptr) +{ + aw_info_.is_emergency_ptr = msg_ptr; +} + +// TODO: not implemented +/* +void AutowareIvAdapter::callbackStopReason(& msg_ptr) +{ + aw_info_.stop_reason_ptr = msg_ptr; +} +*/ + +void AutowareIvAdapter::callbackDiagnostics( + const diagnostic_msgs::DiagnosticArray::ConstPtr & msg_ptr) +{ + aw_info_.diagnostic_ptr = msg_ptr; +} + +void AutowareIvAdapter::callbackAutodriveEnable(const std_msgs::Bool::ConstPtr & msg_ptr) +{ + // TODO: now, it is not used + aw_info_.autodrive_enable_ptr = msg_ptr; +} + +void AutowareIvAdapter::callbackLaneChangeAvailable(const std_msgs::Bool::ConstPtr & msg_ptr) +{ + aw_info_.lane_change_available_ptr = msg_ptr; +} + +void AutowareIvAdapter::callbackLaneChangeReady(const std_msgs::Bool::ConstPtr & msg_ptr) +{ + aw_info_.lane_change_ready_ptr = msg_ptr; +} + +void AutowareIvAdapter::callbackLaneChangeCandidatePath( + const autoware_planning_msgs::Path::ConstPtr & msg_ptr) +{ + aw_info_.lane_change_candidate_ptr = msg_ptr; +} + +void AutowareIvAdapter::callbackLaneObstacleAvoidReady(const std_msgs::Bool::ConstPtr & msg_ptr) +{ + aw_info_.obstacle_avoid_ready_ptr = msg_ptr; +} + +void AutowareIvAdapter::callbackLaneObstacleAvoidCandidatePath( + const autoware_planning_msgs::Trajectory::ConstPtr & msg_ptr) +{ + aw_info_.obstacle_avoid_candidate_ptr = msg_ptr; +} + +} // namespace autoware_api \ No newline at end of file diff --git a/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_node.cpp b/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_node.cpp new file mode 100644 index 0000000000000..a66c0a5773d3b --- /dev/null +++ b/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_node.cpp @@ -0,0 +1,30 @@ +/* + * Copyright 2020 Tier IV, Inc. All rights reserved. + * + * 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. + */ + +#include + +#include + +int main(int argc, char ** argv) +{ + ros::init(argc, argv, "awapi_awiv_adapter_node"); + + autoware_api::AutowareIvAdapter node; + + ros::spin(); + + return 0; +} \ No newline at end of file diff --git a/awapi/awapi_awiv_adapter/src/awapi_lane_change_state_publisher.cpp b/awapi/awapi_awiv_adapter/src/awapi_lane_change_state_publisher.cpp new file mode 100644 index 0000000000000..0a4de389cd85f --- /dev/null +++ b/awapi/awapi_awiv_adapter/src/awapi_lane_change_state_publisher.cpp @@ -0,0 +1,85 @@ +/* + * Copyright 2020 Tier IV, Inc. All rights reserved. + * + * 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. + */ + +#include + +namespace autoware_api +{ +AutowareIvLaneChangeStatePublisher::AutowareIvLaneChangeStatePublisher() : nh_(), pnh_("~") +{ + // publisher + pub_state_ = pnh_.advertise("output/lane_change_status", 1); +} + +void AutowareIvLaneChangeStatePublisher::statePublisher(const AutowareInfo & aw_info) +{ + awapi_awiv_adapter::LaneChangeStatus status; + + //input header + status.header.frame_id = "base_link"; + status.header.stamp = ros::Time::now(); + + // get all info + getLaneChangeAvailableInfo(aw_info.lane_change_available_ptr, &status); + getLaneChangeReadyInfo(aw_info.lane_change_ready_ptr, &status); + getCandidatePathInfo(aw_info.lane_change_candidate_ptr, &status); + + // publish info + pub_state_.publish(status); +} + +void AutowareIvLaneChangeStatePublisher::getLaneChangeAvailableInfo( + const std_msgs::Bool::ConstPtr & available_ptr, awapi_awiv_adapter::LaneChangeStatus * status) +{ + if (!available_ptr) { + ROS_WARN_STREAM_THROTTLE( + 5.0, "[AutowareIvLaneChangeStatePublisher] lane change available is nullptr"); + return; + } + + // get lane change available info + status->force_lane_change_available = available_ptr->data; +} + +void AutowareIvLaneChangeStatePublisher::getLaneChangeReadyInfo( + const std_msgs::Bool::ConstPtr & ready_ptr, awapi_awiv_adapter::LaneChangeStatus * status) +{ + if (!ready_ptr) { + ROS_WARN_STREAM_THROTTLE( + 5.0, "[AutowareIvLaneChangeStatePublisher] lane change ready is nullptr"); + return; + } + + // get lane change available info + status->lane_change_ready = ready_ptr->data; +} + +void AutowareIvLaneChangeStatePublisher::getCandidatePathInfo( + const autoware_planning_msgs::Path::ConstPtr & path_ptr, + awapi_awiv_adapter::LaneChangeStatus * status) +{ + if (!path_ptr) { + ROS_WARN_STREAM_THROTTLE( + 5.0, + "[AutowareIvLaneChangeStatePublisher] lane_change_candidate_path is " + "nullptr"); + return; + } + + status->candidate_path = *path_ptr; +} + +} // namespace autoware_api \ No newline at end of file diff --git a/awapi/awapi_awiv_adapter/src/awapi_obstacle_avoidance_state_publisher.cpp b/awapi/awapi_awiv_adapter/src/awapi_obstacle_avoidance_state_publisher.cpp new file mode 100644 index 0000000000000..e70b03f96cc64 --- /dev/null +++ b/awapi/awapi_awiv_adapter/src/awapi_obstacle_avoidance_state_publisher.cpp @@ -0,0 +1,72 @@ +/* + * Copyright 2020 Tier IV, Inc. All rights reserved. + * + * 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. + */ + +#include + +namespace autoware_api +{ +AutowareIvObstacleAvoidanceStatePublisher::AutowareIvObstacleAvoidanceStatePublisher() +: nh_(), pnh_("~") +{ + // publisher + pub_state_ = + pnh_.advertise("output/obstacle_avoid_status", 1); +} + +void AutowareIvObstacleAvoidanceStatePublisher::statePublisher(const AutowareInfo & aw_info) +{ + awapi_awiv_adapter::ObstacleAvoidanceStatus status; + + //input header + status.header.frame_id = "base_link"; + status.header.stamp = ros::Time::now(); + + // get all info + getObstacleAvoidReadyInfo(aw_info.obstacle_avoid_ready_ptr, &status); + getCandidatePathInfo(aw_info.obstacle_avoid_candidate_ptr, &status); + + // publish info + pub_state_.publish(status); +} + +void AutowareIvObstacleAvoidanceStatePublisher::getObstacleAvoidReadyInfo( + const std_msgs::Bool::ConstPtr & ready_ptr, awapi_awiv_adapter::ObstacleAvoidanceStatus * status) +{ + if (!ready_ptr) { + ROS_WARN_STREAM_THROTTLE( + 5.0, "[AutowareIvObstacleAvoidanceStatePublisher] obstacle_avoidance_ready is nullptr"); + return; + } + + status->obstacle_avoidance_ready = ready_ptr->data; +} + +void AutowareIvObstacleAvoidanceStatePublisher::getCandidatePathInfo( + const autoware_planning_msgs::Trajectory::ConstPtr & path_ptr, + awapi_awiv_adapter::ObstacleAvoidanceStatus * status) +{ + if (!path_ptr) { + ROS_WARN_STREAM_THROTTLE( + 5.0, + "[AutowareIvObstacleAvoidanceStatePublisher] obstacle_avoidance_candidate_path is " + "nullptr"); + return; + } + + status->candidate_path = *path_ptr; +} + +} // namespace autoware_api \ No newline at end of file diff --git a/awapi/awapi_awiv_adapter/src/awapi_vehicle_state_publisher.cpp b/awapi/awapi_awiv_adapter/src/awapi_vehicle_state_publisher.cpp new file mode 100644 index 0000000000000..b4a0d1e86c755 --- /dev/null +++ b/awapi/awapi_awiv_adapter/src/awapi_vehicle_state_publisher.cpp @@ -0,0 +1,183 @@ +/* + * Copyright 2020 Tier IV, Inc. All rights reserved. + * + * 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. + */ + +#include + +namespace autoware_api +{ +AutowareIvVehicleStatePublisher::AutowareIvVehicleStatePublisher() +: nh_(), pnh_("~"), prev_accel_(0.0) +{ + // publisher + pub_state_ = pnh_.advertise("output/vehicle_status", 1); +} + +void AutowareIvVehicleStatePublisher::statePublisher(const AutowareInfo & aw_info) +{ + awapi_awiv_adapter::AwapiVehicleStatus status; + + //input header + status.header.frame_id = "base_link"; + status.header.stamp = ros::Time::now(); + + // get all info + getPoseInfo(aw_info.current_pose_ptr, &status); + getSteerInfo(aw_info.steer_ptr, &status); + getVehicleCmdInfo(aw_info.vehicle_cmd_ptr, &status); + getTurnSignalInfo(aw_info.turn_signal_ptr, &status); + getTwistInfo(aw_info.twist_ptr, &status); + getGearInfo(aw_info.gear_ptr, &status); + getGpsInfo(aw_info.nav_sat_ptr, &status); + + // publish info + pub_state_.publish(status); +} + +void AutowareIvVehicleStatePublisher::getPoseInfo( + const std::shared_ptr & pose_ptr, + awapi_awiv_adapter::AwapiVehicleStatus * status) +{ + if (!pose_ptr) { + ROS_WARN_STREAM_THROTTLE(5.0, "[AutowareIvVehicleStatePublisher] current pose is nullptr"); + return; + } + + // get pose + status->pose = pose_ptr->pose; + + // convert quaternion to euler + double yaw, pitch, roll; + tf2::getEulerYPR(pose_ptr->pose.orientation, yaw, pitch, roll); + status->eulerangle.yaw = yaw; + status->eulerangle.pitch = pitch; + status->eulerangle.roll = roll; +} + +void AutowareIvVehicleStatePublisher::getSteerInfo( + const autoware_vehicle_msgs::Steering::ConstPtr & steer_ptr, + awapi_awiv_adapter::AwapiVehicleStatus * status) +{ + if (!steer_ptr) { + ROS_WARN_STREAM_THROTTLE(5.0, "[AutowareIvVehicleStatePublisher] steer is nullptr"); + return; + } + + // get steer + status->steering = steer_ptr->data; + + // get steer vel + if (previous_steer_ptr_) { + //calculate steer vel from steer + const double ds = steer_ptr->data - previous_steer_ptr_->data; + const double dt = + std::max((steer_ptr->header.stamp - previous_steer_ptr_->header.stamp).toSec(), 1e-03); + const double steer_vel = ds / dt; + + //apply lowpass filter + const double lowpass_steer = + lowpass_filter(steer_vel, prev_steer_vel_, steer_vel_lowpass_gain_); + prev_steer_vel_ = lowpass_steer; + status->steering_velocity = lowpass_steer; + } + previous_steer_ptr_ = steer_ptr; +} +void AutowareIvVehicleStatePublisher::getVehicleCmdInfo( + const autoware_vehicle_msgs::VehicleCommand::ConstPtr & vehicle_cmd_ptr, + awapi_awiv_adapter::AwapiVehicleStatus * status) +{ + if (!vehicle_cmd_ptr) { + ROS_WARN_STREAM_THROTTLE(5.0, "[AutowareIvVehicleStatePublisher] vehicle cmd is nullptr"); + return; + } + + // get command + status->target_acceleration = vehicle_cmd_ptr->control.acceleration; + status->target_velocity = vehicle_cmd_ptr->control.velocity; + status->target_steering = vehicle_cmd_ptr->control.steering_angle; + status->target_steering_velocity = vehicle_cmd_ptr->control.steering_angle_velocity; +} + +void AutowareIvVehicleStatePublisher::getTurnSignalInfo( + const autoware_vehicle_msgs::TurnSignal::ConstPtr & turn_signal_ptr, + awapi_awiv_adapter::AwapiVehicleStatus * status) +{ + if (!turn_signal_ptr) { + ROS_WARN_STREAM_THROTTLE(5.0, "[AutowareIvVehicleStatePublisher] turn signal is nullptr"); + return; + } + + //get turn singnal + status->turn_signal = turn_signal_ptr->data; +} + +void AutowareIvVehicleStatePublisher::getTwistInfo( + const geometry_msgs::TwistStamped::ConstPtr & twist_ptr, + awapi_awiv_adapter::AwapiVehicleStatus * status) +{ + if (!twist_ptr) { + ROS_WARN_STREAM_THROTTLE(5.0, "[AutowareIvVehicleStatePublisher] twist is nullptr"); + return; + } + + // get twist + status->velocity = twist_ptr->twist.linear.x; + status->angular_velocity = twist_ptr->twist.angular.z; + + // get accel + if (previous_twist_ptr_) { + //calculate accleration from velocity + const double dv = twist_ptr->twist.linear.x - previous_twist_ptr_->twist.linear.x; + const double dt = + std::max((twist_ptr->header.stamp - previous_twist_ptr_->header.stamp).toSec(), 1e-03); + const double accel = dv / dt; + + // apply lowpass filter + const double lowpass_accel = lowpass_filter(accel, prev_accel_, accel_lowpass_gain_); + prev_accel_ = lowpass_accel; + status->acceleration = lowpass_accel; + } + previous_twist_ptr_ = twist_ptr; +} + +void AutowareIvVehicleStatePublisher::getGearInfo( + const autoware_vehicle_msgs::ShiftStamped::ConstPtr & gear_ptr, + awapi_awiv_adapter::AwapiVehicleStatus * status) +{ + if (!gear_ptr) { + ROS_WARN_STREAM_THROTTLE(5.0, "[AutowareIvVehicleStatePublisher] gear is nullptr"); + return; + } + + // get gear (shift) + status->gear = gear_ptr->shift.data; +} + +void AutowareIvVehicleStatePublisher::getGpsInfo( + const sensor_msgs::NavSatFix::ConstPtr & nav_sat_ptr, + awapi_awiv_adapter::AwapiVehicleStatus * status) +{ + if (!nav_sat_ptr) { + ROS_WARN_STREAM_THROTTLE(5.0, "[AutowareIvVehicleStatePublisher] nav_sat(gps) is nullptr"); + return; + } + + // get position (latlon) + status->latlon.lat = nav_sat_ptr->latitude; + status->latlon.lon = nav_sat_ptr->longitude; + status->latlon.alt = nav_sat_ptr->altitude; +} + +} // namespace autoware_api \ No newline at end of file From 52944ca2bcf0ce5f9d36e88ca3141bda2424ce3c Mon Sep 17 00:00:00 2001 From: tkimura4 Date: Tue, 14 Jul 2020 16:48:55 +0900 Subject: [PATCH 02/71] fix typo (#687) Signed-off-by: tanaka3 --- awapi/awapi_awiv_adapter/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/awapi/awapi_awiv_adapter/package.xml b/awapi/awapi_awiv_adapter/package.xml index b70c42a75f04c..7952a32800428 100644 --- a/awapi/awapi_awiv_adapter/package.xml +++ b/awapi/awapi_awiv_adapter/package.xml @@ -16,7 +16,7 @@ autoware_planning_msgs autoware_control_msgs autoware_vehicle_msgs - diagnostic_msgs/ + diagnostic_msgs geometry_msgs std_msgs sensor_msgs From 93372995444aa7fec8e6419f3c816906000da637 Mon Sep 17 00:00:00 2001 From: tkimura4 Date: Wed, 15 Jul 2020 19:11:12 +0900 Subject: [PATCH 03/71] move msg file to autoware_api_msgs (#693) Signed-off-by: tanaka3 --- awapi/awapi_awiv_adapter/CMakeLists.txt | 20 +------------------ .../awapi_autoware_state_publisher.h | 18 ++++++++--------- .../awapi_lane_change_state_publisher.h | 10 +++++----- ...awapi_obstacle_avoidance_state_publisher.h | 8 ++++---- .../awapi_vehicle_state_publisher.h | 18 ++++++++--------- .../msg/AwapiAutowareStatus.msg | 8 -------- .../msg/AwapiVehicleStatus.msg | 16 --------------- awapi/awapi_awiv_adapter/msg/Euler.msg | 3 --- .../msg/LaneChangeStatus.msg | 4 ---- awapi/awapi_awiv_adapter/msg/Latlon.msg | 3 --- .../msg/ObstacleAvoidanceStatus.msg | 3 --- awapi/awapi_awiv_adapter/package.xml | 3 +-- .../src/awapi_autoware_state_publisher.cpp | 20 +++++++++---------- .../src/awapi_lane_change_state_publisher.cpp | 12 +++++------ ...api_obstacle_avoidance_state_publisher.cpp | 10 +++++----- .../src/awapi_vehicle_state_publisher.cpp | 20 +++++++++---------- 16 files changed, 60 insertions(+), 116 deletions(-) delete mode 100644 awapi/awapi_awiv_adapter/msg/AwapiAutowareStatus.msg delete mode 100644 awapi/awapi_awiv_adapter/msg/AwapiVehicleStatus.msg delete mode 100644 awapi/awapi_awiv_adapter/msg/Euler.msg delete mode 100644 awapi/awapi_awiv_adapter/msg/LaneChangeStatus.msg delete mode 100644 awapi/awapi_awiv_adapter/msg/Latlon.msg delete mode 100644 awapi/awapi_awiv_adapter/msg/ObstacleAvoidanceStatus.msg diff --git a/awapi/awapi_awiv_adapter/CMakeLists.txt b/awapi/awapi_awiv_adapter/CMakeLists.txt index d464d5e6ed4ac..432effa58b895 100644 --- a/awapi/awapi_awiv_adapter/CMakeLists.txt +++ b/awapi/awapi_awiv_adapter/CMakeLists.txt @@ -7,6 +7,7 @@ find_package(catkin REQUIRED COMPONENTS message_generation message_runtime roscpp + autoware_api_msgs autoware_system_msgs autoware_planning_msgs autoware_control_msgs @@ -19,25 +20,6 @@ find_package(catkin REQUIRED COMPONENTS tf2_geometry_msgs ) -add_message_files( - DIRECTORY msg - FILES - Latlon.msg - Euler.msg - AwapiVehicleStatus.msg - AwapiAutowareStatus.msg - LaneChangeStatus.msg - ObstacleAvoidanceStatus.msg -) - -generate_messages( - DEPENDENCIES - std_msgs - geometry_msgs - diagnostic_msgs - autoware_planning_msgs -) - catkin_package( INCLUDE_DIRS include ) diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.h b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.h index 6ead50646ca06..88d679d780e81 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.h +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.h @@ -14,7 +14,7 @@ * limitations under the License. */ -#include +#include #include namespace autoware_api @@ -35,29 +35,29 @@ class AutowareIvAutowareStatePublisher void getAutowareStateInfo( const autoware_system_msgs::AutowareState::ConstPtr & autoware_state_ptr, - awapi_awiv_adapter::AwapiAutowareStatus * status); + autoware_api_msgs::AwapiAutowareStatus * status); void getControlModeInfo( const autoware_vehicle_msgs::ControlMode::ConstPtr & control_mode_ptr, - awapi_awiv_adapter::AwapiAutowareStatus * status); + autoware_api_msgs::AwapiAutowareStatus * status); void getGateModeInfo( const autoware_control_msgs::GateMode::ConstPtr & gate_mode_ptr, - awapi_awiv_adapter::AwapiAutowareStatus * status); + autoware_api_msgs::AwapiAutowareStatus * status); void getIsEmergencyInfo( const std_msgs::Bool::ConstPtr & is_emergency_ptr, - awapi_awiv_adapter::AwapiAutowareStatus * status); + autoware_api_msgs::AwapiAutowareStatus * status); // TODO: not implemented /* void getStopReasonInfo( const - & stop_reason_ptr, - awapi_awiv_adapter::AwapiAutowareStatus * status); + autoware_api_msgs::AwapiAutowareStatus * status); */ void getDiagInfo( const diagnostic_msgs::DiagnosticArray::ConstPtr & diag_ptr, - awapi_awiv_adapter::AwapiAutowareStatus * status); + autoware_api_msgs::AwapiAutowareStatus * status); void getAutoDriveEnableInfo( const std_msgs::Bool::ConstPtr & auto_drive_enable_ptr, - awapi_awiv_adapter::AwapiAutowareStatus * status); + autoware_api_msgs::AwapiAutowareStatus * status); }; -} // namespace autoware_api \ No newline at end of file +} // namespace autoware_api diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_lane_change_state_publisher.h b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_lane_change_state_publisher.h index 13f0ee5d6224f..56c30f4aa30cd 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_lane_change_state_publisher.h +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_lane_change_state_publisher.h @@ -14,7 +14,7 @@ * limitations under the License. */ -#include +#include #include namespace autoware_api @@ -34,12 +34,12 @@ class AutowareIvLaneChangeStatePublisher ros::Publisher pub_state_; void getLaneChangeAvailableInfo( - const std_msgs::Bool::ConstPtr & available_ptr, awapi_awiv_adapter::LaneChangeStatus * status); + const std_msgs::Bool::ConstPtr & available_ptr, autoware_api_msgs::LaneChangeStatus * status); void getLaneChangeReadyInfo( - const std_msgs::Bool::ConstPtr & ready_ptr, awapi_awiv_adapter::LaneChangeStatus * status); + const std_msgs::Bool::ConstPtr & ready_ptr, autoware_api_msgs::LaneChangeStatus * status); void getCandidatePathInfo( const autoware_planning_msgs::Path::ConstPtr & path_ptr, - awapi_awiv_adapter::LaneChangeStatus * status); + autoware_api_msgs::LaneChangeStatus * status); }; -} // namespace autoware_api \ No newline at end of file +} // namespace autoware_api diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_obstacle_avoidance_state_publisher.h b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_obstacle_avoidance_state_publisher.h index e1ccfad73bee7..f82ddb16dff07 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_obstacle_avoidance_state_publisher.h +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_obstacle_avoidance_state_publisher.h @@ -14,7 +14,7 @@ * limitations under the License. */ -#include +#include #include namespace autoware_api @@ -35,10 +35,10 @@ class AutowareIvObstacleAvoidanceStatePublisher void getObstacleAvoidReadyInfo( const std_msgs::Bool::ConstPtr & ready_ptr, - awapi_awiv_adapter::ObstacleAvoidanceStatus * status); + autoware_api_msgs::ObstacleAvoidanceStatus * status); void getCandidatePathInfo( const autoware_planning_msgs::Trajectory::ConstPtr & path_ptr, - awapi_awiv_adapter::ObstacleAvoidanceStatus * status); + autoware_api_msgs::ObstacleAvoidanceStatus * status); }; -} // namespace autoware_api \ No newline at end of file +} // namespace autoware_api diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_vehicle_state_publisher.h b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_vehicle_state_publisher.h index 66e0af13a99a3..883c4d4b555f4 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_vehicle_state_publisher.h +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_vehicle_state_publisher.h @@ -14,7 +14,7 @@ * limitations under the License. */ -#include +#include #include namespace autoware_api @@ -35,25 +35,25 @@ class AutowareIvVehicleStatePublisher void getPoseInfo( const std::shared_ptr & pose_ptr, - awapi_awiv_adapter::AwapiVehicleStatus * status); + autoware_api_msgs::AwapiVehicleStatus * status); void getSteerInfo( const autoware_vehicle_msgs::Steering::ConstPtr & steer_ptr, - awapi_awiv_adapter::AwapiVehicleStatus * status); + autoware_api_msgs::AwapiVehicleStatus * status); void getVehicleCmdInfo( const autoware_vehicle_msgs::VehicleCommand::ConstPtr & vehicle_cmd_ptr, - awapi_awiv_adapter::AwapiVehicleStatus * status); + autoware_api_msgs::AwapiVehicleStatus * status); void getTurnSignalInfo( const autoware_vehicle_msgs::TurnSignal::ConstPtr & turn_signal_ptr, - awapi_awiv_adapter::AwapiVehicleStatus * status); + autoware_api_msgs::AwapiVehicleStatus * status); void getTwistInfo( const geometry_msgs::TwistStamped::ConstPtr & twist_ptr, - awapi_awiv_adapter::AwapiVehicleStatus * status); + autoware_api_msgs::AwapiVehicleStatus * status); void getGearInfo( const autoware_vehicle_msgs::ShiftStamped::ConstPtr & gear_ptr, - awapi_awiv_adapter::AwapiVehicleStatus * status); + autoware_api_msgs::AwapiVehicleStatus * status); void getGpsInfo( const sensor_msgs::NavSatFix::ConstPtr & nav_sat_ptr, - awapi_awiv_adapter::AwapiVehicleStatus * status); + autoware_api_msgs::AwapiVehicleStatus * status); //parameters geometry_msgs::TwistStamped::ConstPtr previous_twist_ptr_; @@ -66,4 +66,4 @@ class AutowareIvVehicleStatePublisher const double steer_vel_lowpass_gain_ = 0.2; }; -} // namespace autoware_api \ No newline at end of file +} // namespace autoware_api diff --git a/awapi/awapi_awiv_adapter/msg/AwapiAutowareStatus.msg b/awapi/awapi_awiv_adapter/msg/AwapiAutowareStatus.msg deleted file mode 100644 index 4d6f73c105ff2..0000000000000 --- a/awapi/awapi_awiv_adapter/msg/AwapiAutowareStatus.msg +++ /dev/null @@ -1,8 +0,0 @@ -Header header -string autoware_state -int32 control_mode -int32 gate_mode -bool emergency_stopped -string stop_reason -diagnostic_msgs/DiagnosticStatus[] diagnostics -bool autonomous_overriden diff --git a/awapi/awapi_awiv_adapter/msg/AwapiVehicleStatus.msg b/awapi/awapi_awiv_adapter/msg/AwapiVehicleStatus.msg deleted file mode 100644 index e16b949611809..0000000000000 --- a/awapi/awapi_awiv_adapter/msg/AwapiVehicleStatus.msg +++ /dev/null @@ -1,16 +0,0 @@ -std_msgs/Header header -geometry_msgs/Pose pose -awapi_awiv_adapter/Euler eulerangle -awapi_awiv_adapter/Latlon latlon -float64 velocity -float64 acceleration -float64 steering -float64 steering_velocity -float64 angular_velocity -int32 gear -float64 distance_to_empty -int32 turn_signal -float64 target_velocity -float64 target_acceleration -float64 target_steering -float64 target_steering_velocity diff --git a/awapi/awapi_awiv_adapter/msg/Euler.msg b/awapi/awapi_awiv_adapter/msg/Euler.msg deleted file mode 100644 index c963632f3dafe..0000000000000 --- a/awapi/awapi_awiv_adapter/msg/Euler.msg +++ /dev/null @@ -1,3 +0,0 @@ -float64 roll -float64 pitch -float64 yaw diff --git a/awapi/awapi_awiv_adapter/msg/LaneChangeStatus.msg b/awapi/awapi_awiv_adapter/msg/LaneChangeStatus.msg deleted file mode 100644 index b49435e8fac59..0000000000000 --- a/awapi/awapi_awiv_adapter/msg/LaneChangeStatus.msg +++ /dev/null @@ -1,4 +0,0 @@ -Header header -bool force_lane_change_available -bool lane_change_ready -autoware_planning_msgs/Path candidate_path diff --git a/awapi/awapi_awiv_adapter/msg/Latlon.msg b/awapi/awapi_awiv_adapter/msg/Latlon.msg deleted file mode 100644 index 798b6734375d0..0000000000000 --- a/awapi/awapi_awiv_adapter/msg/Latlon.msg +++ /dev/null @@ -1,3 +0,0 @@ -float64 lat -float64 lon -float64 alt diff --git a/awapi/awapi_awiv_adapter/msg/ObstacleAvoidanceStatus.msg b/awapi/awapi_awiv_adapter/msg/ObstacleAvoidanceStatus.msg deleted file mode 100644 index 408d3c954eb2a..0000000000000 --- a/awapi/awapi_awiv_adapter/msg/ObstacleAvoidanceStatus.msg +++ /dev/null @@ -1,3 +0,0 @@ -Header header -bool obstacle_avoidance_ready -autoware_planning_msgs/Trajectory candidate_path diff --git a/awapi/awapi_awiv_adapter/package.xml b/awapi/awapi_awiv_adapter/package.xml index 7952a32800428..2528aa91ccf6f 100644 --- a/awapi/awapi_awiv_adapter/package.xml +++ b/awapi/awapi_awiv_adapter/package.xml @@ -9,9 +9,8 @@ catkin - message_generation - message_runtime roscpp + autoware_api_msgs autoware_system_msgs autoware_planning_msgs autoware_control_msgs diff --git a/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp b/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp index 41f0c927382e9..84f7e4921a834 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp @@ -21,12 +21,12 @@ namespace autoware_api AutowareIvAutowareStatePublisher::AutowareIvAutowareStatePublisher() : nh_(), pnh_("~") { // publisher - pub_state_ = pnh_.advertise("output/autoware_status", 1); + pub_state_ = pnh_.advertise("output/autoware_status", 1); } void AutowareIvAutowareStatePublisher::statePublisher(const AutowareInfo & aw_info) { - awapi_awiv_adapter::AwapiAutowareStatus status; + autoware_api_msgs::AwapiAutowareStatus status; //input header status.header.frame_id = "base_link"; @@ -47,7 +47,7 @@ void AutowareIvAutowareStatePublisher::statePublisher(const AutowareInfo & aw_in void AutowareIvAutowareStatePublisher::getAutowareStateInfo( const autoware_system_msgs::AutowareState::ConstPtr & autoware_state_ptr, - awapi_awiv_adapter::AwapiAutowareStatus * status) + autoware_api_msgs::AwapiAutowareStatus * status) { if (!autoware_state_ptr) { ROS_WARN_STREAM_THROTTLE(5.0, "[AutowareIvAutowareStatePublisher] autoware_state is nullptr"); @@ -60,7 +60,7 @@ void AutowareIvAutowareStatePublisher::getAutowareStateInfo( void AutowareIvAutowareStatePublisher::getControlModeInfo( const autoware_vehicle_msgs::ControlMode::ConstPtr & control_mode_ptr, - awapi_awiv_adapter::AwapiAutowareStatus * status) + autoware_api_msgs::AwapiAutowareStatus * status) { if (!control_mode_ptr) { ROS_WARN_STREAM_THROTTLE(5.0, "[AutowareIvAutowareStatePublisher] control mode is nullptr"); @@ -73,7 +73,7 @@ void AutowareIvAutowareStatePublisher::getControlModeInfo( void AutowareIvAutowareStatePublisher::getGateModeInfo( const autoware_control_msgs::GateMode::ConstPtr & gate_mode_ptr, - awapi_awiv_adapter::AwapiAutowareStatus * status) + autoware_api_msgs::AwapiAutowareStatus * status) { if (!gate_mode_ptr) { ROS_WARN_STREAM_THROTTLE(5.0, "[AutowareIvAutowareStatePublisher] gate mode is nullptr"); @@ -86,7 +86,7 @@ void AutowareIvAutowareStatePublisher::getGateModeInfo( void AutowareIvAutowareStatePublisher::getIsEmergencyInfo( const std_msgs::Bool::ConstPtr & is_emergency_ptr, - awapi_awiv_adapter::AwapiAutowareStatus * status) + autoware_api_msgs::AwapiAutowareStatus * status) { if (!is_emergency_ptr) { ROS_WARN_STREAM_THROTTLE(5.0, "[AutowareIvAutowareStatePublisher] is_emergency is nullptr"); @@ -101,12 +101,12 @@ void AutowareIvAutowareStatePublisher::getIsEmergencyInfo( /* void AutowareIvAutowareStatePublisher::getStopReasonInfo( const - & stop_reason_ptr, - awapi_awiv_adapter::AwapiAutowareStatus * status){} + autoware_api_msgs::AwapiAutowareStatus * status){} */ void AutowareIvAutowareStatePublisher::getDiagInfo( const diagnostic_msgs::DiagnosticArray::ConstPtr & diag_ptr, - awapi_awiv_adapter::AwapiAutowareStatus * status) + autoware_api_msgs::AwapiAutowareStatus * status) { if (!diag_ptr) { ROS_WARN_STREAM_THROTTLE(5.0, "[AutowareIvAutowareStatePublisher] diagnostics is nullptr"); @@ -119,7 +119,7 @@ void AutowareIvAutowareStatePublisher::getDiagInfo( void AutowareIvAutowareStatePublisher::getAutoDriveEnableInfo( const std_msgs::Bool::ConstPtr & auto_drive_enable_ptr, - awapi_awiv_adapter::AwapiAutowareStatus * status) + autoware_api_msgs::AwapiAutowareStatus * status) { if (!auto_drive_enable_ptr) { ROS_WARN_STREAM_THROTTLE( @@ -131,4 +131,4 @@ void AutowareIvAutowareStatePublisher::getAutoDriveEnableInfo( status->autonomous_overriden = auto_drive_enable_ptr->data ? false : true; } -} // namespace autoware_api \ No newline at end of file +} // namespace autoware_api diff --git a/awapi/awapi_awiv_adapter/src/awapi_lane_change_state_publisher.cpp b/awapi/awapi_awiv_adapter/src/awapi_lane_change_state_publisher.cpp index 0a4de389cd85f..e69e8cb6050e6 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_lane_change_state_publisher.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_lane_change_state_publisher.cpp @@ -21,12 +21,12 @@ namespace autoware_api AutowareIvLaneChangeStatePublisher::AutowareIvLaneChangeStatePublisher() : nh_(), pnh_("~") { // publisher - pub_state_ = pnh_.advertise("output/lane_change_status", 1); + pub_state_ = pnh_.advertise("output/lane_change_status", 1); } void AutowareIvLaneChangeStatePublisher::statePublisher(const AutowareInfo & aw_info) { - awapi_awiv_adapter::LaneChangeStatus status; + autoware_api_msgs::LaneChangeStatus status; //input header status.header.frame_id = "base_link"; @@ -42,7 +42,7 @@ void AutowareIvLaneChangeStatePublisher::statePublisher(const AutowareInfo & aw_ } void AutowareIvLaneChangeStatePublisher::getLaneChangeAvailableInfo( - const std_msgs::Bool::ConstPtr & available_ptr, awapi_awiv_adapter::LaneChangeStatus * status) + const std_msgs::Bool::ConstPtr & available_ptr, autoware_api_msgs::LaneChangeStatus * status) { if (!available_ptr) { ROS_WARN_STREAM_THROTTLE( @@ -55,7 +55,7 @@ void AutowareIvLaneChangeStatePublisher::getLaneChangeAvailableInfo( } void AutowareIvLaneChangeStatePublisher::getLaneChangeReadyInfo( - const std_msgs::Bool::ConstPtr & ready_ptr, awapi_awiv_adapter::LaneChangeStatus * status) + const std_msgs::Bool::ConstPtr & ready_ptr, autoware_api_msgs::LaneChangeStatus * status) { if (!ready_ptr) { ROS_WARN_STREAM_THROTTLE( @@ -69,7 +69,7 @@ void AutowareIvLaneChangeStatePublisher::getLaneChangeReadyInfo( void AutowareIvLaneChangeStatePublisher::getCandidatePathInfo( const autoware_planning_msgs::Path::ConstPtr & path_ptr, - awapi_awiv_adapter::LaneChangeStatus * status) + autoware_api_msgs::LaneChangeStatus * status) { if (!path_ptr) { ROS_WARN_STREAM_THROTTLE( @@ -82,4 +82,4 @@ void AutowareIvLaneChangeStatePublisher::getCandidatePathInfo( status->candidate_path = *path_ptr; } -} // namespace autoware_api \ No newline at end of file +} // namespace autoware_api diff --git a/awapi/awapi_awiv_adapter/src/awapi_obstacle_avoidance_state_publisher.cpp b/awapi/awapi_awiv_adapter/src/awapi_obstacle_avoidance_state_publisher.cpp index e70b03f96cc64..a849a8962e92e 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_obstacle_avoidance_state_publisher.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_obstacle_avoidance_state_publisher.cpp @@ -23,12 +23,12 @@ AutowareIvObstacleAvoidanceStatePublisher::AutowareIvObstacleAvoidanceStatePubli { // publisher pub_state_ = - pnh_.advertise("output/obstacle_avoid_status", 1); + pnh_.advertise("output/obstacle_avoid_status", 1); } void AutowareIvObstacleAvoidanceStatePublisher::statePublisher(const AutowareInfo & aw_info) { - awapi_awiv_adapter::ObstacleAvoidanceStatus status; + autoware_api_msgs::ObstacleAvoidanceStatus status; //input header status.header.frame_id = "base_link"; @@ -43,7 +43,7 @@ void AutowareIvObstacleAvoidanceStatePublisher::statePublisher(const AutowareInf } void AutowareIvObstacleAvoidanceStatePublisher::getObstacleAvoidReadyInfo( - const std_msgs::Bool::ConstPtr & ready_ptr, awapi_awiv_adapter::ObstacleAvoidanceStatus * status) + const std_msgs::Bool::ConstPtr & ready_ptr, autoware_api_msgs::ObstacleAvoidanceStatus * status) { if (!ready_ptr) { ROS_WARN_STREAM_THROTTLE( @@ -56,7 +56,7 @@ void AutowareIvObstacleAvoidanceStatePublisher::getObstacleAvoidReadyInfo( void AutowareIvObstacleAvoidanceStatePublisher::getCandidatePathInfo( const autoware_planning_msgs::Trajectory::ConstPtr & path_ptr, - awapi_awiv_adapter::ObstacleAvoidanceStatus * status) + autoware_api_msgs::ObstacleAvoidanceStatus * status) { if (!path_ptr) { ROS_WARN_STREAM_THROTTLE( @@ -69,4 +69,4 @@ void AutowareIvObstacleAvoidanceStatePublisher::getCandidatePathInfo( status->candidate_path = *path_ptr; } -} // namespace autoware_api \ No newline at end of file +} // namespace autoware_api diff --git a/awapi/awapi_awiv_adapter/src/awapi_vehicle_state_publisher.cpp b/awapi/awapi_awiv_adapter/src/awapi_vehicle_state_publisher.cpp index b4a0d1e86c755..c1adb24e7a1bf 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_vehicle_state_publisher.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_vehicle_state_publisher.cpp @@ -22,12 +22,12 @@ AutowareIvVehicleStatePublisher::AutowareIvVehicleStatePublisher() : nh_(), pnh_("~"), prev_accel_(0.0) { // publisher - pub_state_ = pnh_.advertise("output/vehicle_status", 1); + pub_state_ = pnh_.advertise("output/vehicle_status", 1); } void AutowareIvVehicleStatePublisher::statePublisher(const AutowareInfo & aw_info) { - awapi_awiv_adapter::AwapiVehicleStatus status; + autoware_api_msgs::AwapiVehicleStatus status; //input header status.header.frame_id = "base_link"; @@ -48,7 +48,7 @@ void AutowareIvVehicleStatePublisher::statePublisher(const AutowareInfo & aw_inf void AutowareIvVehicleStatePublisher::getPoseInfo( const std::shared_ptr & pose_ptr, - awapi_awiv_adapter::AwapiVehicleStatus * status) + autoware_api_msgs::AwapiVehicleStatus * status) { if (!pose_ptr) { ROS_WARN_STREAM_THROTTLE(5.0, "[AutowareIvVehicleStatePublisher] current pose is nullptr"); @@ -68,7 +68,7 @@ void AutowareIvVehicleStatePublisher::getPoseInfo( void AutowareIvVehicleStatePublisher::getSteerInfo( const autoware_vehicle_msgs::Steering::ConstPtr & steer_ptr, - awapi_awiv_adapter::AwapiVehicleStatus * status) + autoware_api_msgs::AwapiVehicleStatus * status) { if (!steer_ptr) { ROS_WARN_STREAM_THROTTLE(5.0, "[AutowareIvVehicleStatePublisher] steer is nullptr"); @@ -96,7 +96,7 @@ void AutowareIvVehicleStatePublisher::getSteerInfo( } void AutowareIvVehicleStatePublisher::getVehicleCmdInfo( const autoware_vehicle_msgs::VehicleCommand::ConstPtr & vehicle_cmd_ptr, - awapi_awiv_adapter::AwapiVehicleStatus * status) + autoware_api_msgs::AwapiVehicleStatus * status) { if (!vehicle_cmd_ptr) { ROS_WARN_STREAM_THROTTLE(5.0, "[AutowareIvVehicleStatePublisher] vehicle cmd is nullptr"); @@ -112,7 +112,7 @@ void AutowareIvVehicleStatePublisher::getVehicleCmdInfo( void AutowareIvVehicleStatePublisher::getTurnSignalInfo( const autoware_vehicle_msgs::TurnSignal::ConstPtr & turn_signal_ptr, - awapi_awiv_adapter::AwapiVehicleStatus * status) + autoware_api_msgs::AwapiVehicleStatus * status) { if (!turn_signal_ptr) { ROS_WARN_STREAM_THROTTLE(5.0, "[AutowareIvVehicleStatePublisher] turn signal is nullptr"); @@ -125,7 +125,7 @@ void AutowareIvVehicleStatePublisher::getTurnSignalInfo( void AutowareIvVehicleStatePublisher::getTwistInfo( const geometry_msgs::TwistStamped::ConstPtr & twist_ptr, - awapi_awiv_adapter::AwapiVehicleStatus * status) + autoware_api_msgs::AwapiVehicleStatus * status) { if (!twist_ptr) { ROS_WARN_STREAM_THROTTLE(5.0, "[AutowareIvVehicleStatePublisher] twist is nullptr"); @@ -154,7 +154,7 @@ void AutowareIvVehicleStatePublisher::getTwistInfo( void AutowareIvVehicleStatePublisher::getGearInfo( const autoware_vehicle_msgs::ShiftStamped::ConstPtr & gear_ptr, - awapi_awiv_adapter::AwapiVehicleStatus * status) + autoware_api_msgs::AwapiVehicleStatus * status) { if (!gear_ptr) { ROS_WARN_STREAM_THROTTLE(5.0, "[AutowareIvVehicleStatePublisher] gear is nullptr"); @@ -167,7 +167,7 @@ void AutowareIvVehicleStatePublisher::getGearInfo( void AutowareIvVehicleStatePublisher::getGpsInfo( const sensor_msgs::NavSatFix::ConstPtr & nav_sat_ptr, - awapi_awiv_adapter::AwapiVehicleStatus * status) + autoware_api_msgs::AwapiVehicleStatus * status) { if (!nav_sat_ptr) { ROS_WARN_STREAM_THROTTLE(5.0, "[AutowareIvVehicleStatePublisher] nav_sat(gps) is nullptr"); @@ -180,4 +180,4 @@ void AutowareIvVehicleStatePublisher::getGpsInfo( status->latlon.alt = nav_sat_ptr->altitude; } -} // namespace autoware_api \ No newline at end of file +} // namespace autoware_api From 615cb3df260f926ddf3e799e5def481c905bc5bc Mon Sep 17 00:00:00 2001 From: tkimura4 Date: Sat, 25 Jul 2020 14:20:22 +0900 Subject: [PATCH 04/71] Fix/awapi awiv adapter (#698) * subscribe global rpt * update readme * update readme * add warning * fix readme * update readme Signed-off-by: tanaka3 --- awapi/awapi_awiv_adapter/CMakeLists.txt | 1 + awapi/awapi_awiv_adapter/Readme.md | 25 ++++++++++--------- .../awapi_autoware_state_publisher.h | 4 +-- .../awapi_awiv_adapter/awapi_autoware_util.h | 22 +++++++++++++++- .../awapi_awiv_adapter_core.h | 7 +++--- .../launch/awapi_awiv_adapter.launch | 7 ++++-- awapi/awapi_awiv_adapter/package.xml | 1 + .../src/awapi_autoware_state_publisher.cpp | 15 ++++++----- .../src/awapi_awiv_adapter_core.cpp | 21 ++++++++++++---- 9 files changed, 70 insertions(+), 33 deletions(-) diff --git a/awapi/awapi_awiv_adapter/CMakeLists.txt b/awapi/awapi_awiv_adapter/CMakeLists.txt index 432effa58b895..39a6ba4ecc536 100644 --- a/awapi/awapi_awiv_adapter/CMakeLists.txt +++ b/awapi/awapi_awiv_adapter/CMakeLists.txt @@ -14,6 +14,7 @@ find_package(catkin REQUIRED COMPONENTS autoware_vehicle_msgs diagnostic_msgs geometry_msgs + pacmod_msgs std_msgs sensor_msgs tf2 diff --git a/awapi/awapi_awiv_adapter/Readme.md b/awapi/awapi_awiv_adapter/Readme.md index cbfcc92265677..3168dd72bec20 100644 --- a/awapi/awapi_awiv_adapter/Readme.md +++ b/awapi/awapi_awiv_adapter/Readme.md @@ -10,7 +10,7 @@ | type | name | unit | note | | :------------------------ | :----------------------- | :-------------------------------------------- | :------------------------------------------- | | std_msgs/Header | header | | | -| geometry_msgs/Pose | pose | [m](position) | | +| geometry_msgs/Pose | pose | position:[m] | | | awapi_awiv_adapter/Euler | eulerangle | [rad] | roll/pitch/yaw | | awapi_awiv_adapter/Latlon | latlon | | lat/lon/alt | | float64 | velocity | [m/s] | | @@ -31,16 +31,17 @@ - get autoware status - MessageType: awapi_awiv_adapter/AwapiVehicleStatus.msg -| type | name | unit | note | -| :--------------------------------- | :------------------- | :--------------------------------------------- | :----------------------------------------------------- | -| std_msgs/Header | header | | | -| string | autoware_state | | | -| int32 | control_mode | accroding to autoware_vehicle_msgs/ControlMode | manual/auto (changed by /awapi/autoware/put/engage) | -| int32 | gate_mode | autoware_vehicle_msgs/GateMode | auto/remote (it is valid only when control_mode=auto)) | -| bool | emergency_stopped | True in emergency mode | | -| StopReasonArray??? | stop_reason | | **not implemented** | -| diagnostic_msgs/DiagnosticStatus[] | diagnostics | | | -| bool | autonomous_overriden | | **not implemented** | +| type | name | unit | note | +| :--------------------------------- | :------------------- | :--------------------------------------------- | :------------------------------------------------------------ | +| std_msgs/Header | header | | | +| string | autoware_state | | | +| int32 | control_mode | accroding to autoware_vehicle_msgs/ControlMode | manual/auto (changed by /awapi/autoware/put/engage) | +| int32 | gate_mode | autoware_vehicle_msgs/GateMode | auto/remote (it is valid only when control_mode=auto)) | +| bool | emergency_stopped | True in emergency mode | | +| StopReasonArray??? | stop_reason | | **not implemented** | +| diagnostic_msgs/DiagnosticStatus[] | diagnostics | | | +| bool | autonomous_overriden | | get info from pacmod msg directly (global_rpt/override_active | +| ) | ### /awapi/autoware/get/route @@ -96,7 +97,7 @@ - send emergency signal - MessageType: std_msgs/Bool -- **Emergency stop does work only in Remote Mode. (not work in Auto Mode)** +- **To enable this functionality, autoware have to be in the Remote Mode or set _/control/vehicle_cmd_gate/use_emergency_handling_ to true.** ### /awapi/autoware/put/engage diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.h b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.h index 88d679d780e81..6b2fbc5359538 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.h +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.h @@ -55,8 +55,8 @@ class AutowareIvAutowareStatePublisher const diagnostic_msgs::DiagnosticArray::ConstPtr & diag_ptr, autoware_api_msgs::AwapiAutowareStatus * status); - void getAutoDriveEnableInfo( - const std_msgs::Bool::ConstPtr & auto_drive_enable_ptr, + void getGlobalRptInfo( + const pacmod_msgs::GlobalRpt::ConstPtr & global_rpt_ptr, autoware_api_msgs::AwapiAutowareStatus * status); }; diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.h b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.h index 071609d67524b..1dff31e75fa9e 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.h +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.h @@ -21,6 +21,7 @@ #include #include +#include #include #include #include @@ -56,7 +57,7 @@ struct AutowareInfo std_msgs::Bool::ConstPtr is_emergency_ptr; // TODO ?? stop_reason_ptr; diagnostic_msgs::DiagnosticArray::ConstPtr diagnostic_ptr; - std_msgs::Bool::ConstPtr autodrive_enable_ptr; + pacmod_msgs::GlobalRpt::ConstPtr global_rpt_ptr; std_msgs::Bool::ConstPtr lane_change_available_ptr; std_msgs::Bool::ConstPtr lane_change_ready_ptr; autoware_planning_msgs::Path::ConstPtr lane_change_candidate_ptr; @@ -72,6 +73,25 @@ T getParam(const ros::NodeHandle & nh, const std::string & key, const T & defaul return value; } +template +T waitForParam(const ros::NodeHandle & nh, const std::string & key) +{ + T value; + ros::Rate rate(1.0); + + while (ros::ok()) { + const auto result = nh.getParam(key, value); + if (result) { + return value; + } + + ROS_WARN("waiting for parameter `%s` ...", key.c_str()); + rate.sleep(); + } + + return {}; +} + double lowpass_filter(const double current_value, const double prev_value, const double gain); } // namespace autoware_api diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.h b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.h index 1ae499975c4fd..d402a58bbdcbe 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.h +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.h @@ -45,7 +45,7 @@ class AutowareIvAdapter ros::Subscriber sub_emergency_; ros::Subscriber sub_stop_reason_; ros::Subscriber sub_diagnostics_; - ros::Subscriber sub_autodrive_enable_; + ros::Subscriber sub_global_rpt_; ros::Subscriber sub_lane_change_available_; ros::Subscriber sub_lane_change_ready_; ros::Subscriber sub_lane_change_candidate_; @@ -71,7 +71,7 @@ class AutowareIvAdapter void callbackIsEmergency(const std_msgs::Bool::ConstPtr & msg_ptr); // void callbackStopReason(); // TODO: not implemented void callbackDiagnostics(const diagnostic_msgs::DiagnosticArray::ConstPtr & msg_ptr); - void callbackAutodriveEnable(const std_msgs::Bool::ConstPtr & msg_ptr); + void callbackGlobalRpt(const pacmod_msgs::GlobalRpt::ConstPtr & msg_ptr); void callbackLaneChangeAvailable(const std_msgs::Bool::ConstPtr & msg_ptr); void callbackLaneChangeReady(const std_msgs::Bool::ConstPtr & msg_ptr); void callbackLaneChangeCandidatePath(const autoware_planning_msgs::Path::ConstPtr & msg_ptr); @@ -82,6 +82,7 @@ class AutowareIvAdapter // timer function void timerCallback(const ros::TimerEvent & e); + void emergencyParamCheck(const bool emergency_handling_param); void getCurrentPose(); // parameter @@ -93,4 +94,4 @@ class AutowareIvAdapter double status_pub_hz_; }; -} // namespace autoware_api \ No newline at end of file +} // namespace autoware_api diff --git a/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch b/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch index 4c2cbb71b90bd..c9ac2b436e6ce 100644 --- a/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch +++ b/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch @@ -15,7 +15,7 @@ - + @@ -41,6 +41,8 @@ + + @@ -70,7 +72,7 @@ - + @@ -80,6 +82,7 @@ + diff --git a/awapi/awapi_awiv_adapter/package.xml b/awapi/awapi_awiv_adapter/package.xml index 2528aa91ccf6f..6909569c2b9b5 100644 --- a/awapi/awapi_awiv_adapter/package.xml +++ b/awapi/awapi_awiv_adapter/package.xml @@ -17,6 +17,7 @@ autoware_vehicle_msgs diagnostic_msgs geometry_msgs + pacmod_msgs std_msgs sensor_msgs tf2 diff --git a/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp b/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp index 84f7e4921a834..40a2168d82502 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp @@ -39,7 +39,7 @@ void AutowareIvAutowareStatePublisher::statePublisher(const AutowareInfo & aw_in getIsEmergencyInfo(aw_info.is_emergency_ptr, &status); // getStopReasonInfo(aw_info.stop_reason_ptr, &status); // TODO: not implemented getDiagInfo(aw_info.diagnostic_ptr, &status); - getAutoDriveEnableInfo(aw_info.autodrive_enable_ptr, &status); + getGlobalRptInfo(aw_info.global_rpt_ptr, &status); // publish info pub_state_.publish(status); @@ -117,18 +117,17 @@ void AutowareIvAutowareStatePublisher::getDiagInfo( status->diagnostics = diag_ptr->status; } -void AutowareIvAutowareStatePublisher::getAutoDriveEnableInfo( - const std_msgs::Bool::ConstPtr & auto_drive_enable_ptr, +void AutowareIvAutowareStatePublisher::getGlobalRptInfo( + const pacmod_msgs::GlobalRpt::ConstPtr & global_rpt_ptr, autoware_api_msgs::AwapiAutowareStatus * status) { - if (!auto_drive_enable_ptr) { - ROS_WARN_STREAM_THROTTLE( - 5.0, "[AutowareIvAutowareStatePublisher] auto_drive_enable is nullptr"); + if (!global_rpt_ptr) { + ROS_WARN_STREAM_THROTTLE(5.0, "[AutowareIvAutowareStatePublisher] global_rpt is nullptr"); return; } - // get auto_drive_enable (autonomous_overriden = not auto_drive_enable) - status->autonomous_overriden = auto_drive_enable_ptr->data ? false : true; + // get global_rpt + status->autonomous_overriden = global_rpt_ptr->override_active; } } // namespace autoware_api diff --git a/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp b/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp index f342eb6afae9e..23a0b51449616 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp @@ -22,6 +22,9 @@ AutowareIvAdapter::AutowareIvAdapter() : nh_(), pnh_("~"), tf_listener_(tf_buffe { // get param pnh_.param("status_pub_hz", status_pub_hz_, 5.0); + const bool em_handle_param = waitForParam(pnh_, "param/emergency_handling"); + emergencyParamCheck(em_handle_param); + // setup instance vehicle_state_publisher_ = std::make_unique(); autoware_state_publisher_ = std::make_unique(); @@ -52,8 +55,8 @@ AutowareIvAdapter::AutowareIvAdapter() : nh_(), pnh_("~"), tf_listener_(tf_buffe */ sub_diagnostics_ = pnh_.subscribe("input/diagnostics", 1, &AutowareIvAdapter::callbackDiagnostics, this); - sub_autodrive_enable_ = - pnh_.subscribe("input/autodrive_enable", 1, &AutowareIvAdapter::callbackAutodriveEnable, this); + sub_global_rpt_ = + pnh_.subscribe("input/global_rpt", 1, &AutowareIvAdapter::callbackGlobalRpt, this); sub_lane_change_available_ = pnh_.subscribe( "input/lane_change_avaiable", 1, &AutowareIvAdapter::callbackLaneChangeAvailable, this); sub_lane_change_ready_ = @@ -72,6 +75,14 @@ AutowareIvAdapter::AutowareIvAdapter() : nh_(), pnh_("~"), tf_listener_(tf_buffe nh_.createTimer(ros::Duration(1.0 / status_pub_hz_), &AutowareIvAdapter::timerCallback, this); } +void AutowareIvAdapter::emergencyParamCheck(const bool emergency_handling_param) +{ + if (!emergency_handling_param) { + ROS_ERROR_STREAM("parameter[use_emergency_handling] is false."); + ROS_ERROR_STREAM("autoware/put/emergency is not valid"); + } +} + void AutowareIvAdapter::timerCallback(const ros::TimerEvent & e) { // get current pose @@ -173,10 +184,10 @@ void AutowareIvAdapter::callbackDiagnostics( aw_info_.diagnostic_ptr = msg_ptr; } -void AutowareIvAdapter::callbackAutodriveEnable(const std_msgs::Bool::ConstPtr & msg_ptr) +void AutowareIvAdapter::callbackGlobalRpt(const pacmod_msgs::GlobalRpt::ConstPtr & msg_ptr) { // TODO: now, it is not used - aw_info_.autodrive_enable_ptr = msg_ptr; + aw_info_.global_rpt_ptr = msg_ptr; } void AutowareIvAdapter::callbackLaneChangeAvailable(const std_msgs::Bool::ConstPtr & msg_ptr) @@ -206,4 +217,4 @@ void AutowareIvAdapter::callbackLaneObstacleAvoidCandidatePath( aw_info_.obstacle_avoid_candidate_ptr = msg_ptr; } -} // namespace autoware_api \ No newline at end of file +} // namespace autoware_api From 91ba803ff82dd0a5fef70cc23533bb8f88a80a26 Mon Sep 17 00:00:00 2001 From: hiroyuki obinata <58019445+obi-t4@users.noreply.github.com> Date: Mon, 3 Aug 2020 15:48:14 +0900 Subject: [PATCH 05/71] add check mark for awapi readme (#723) * add check mark for awapi readme * add blank * add blank * add blank Signed-off-by: tanaka3 --- awapi/awapi_awiv_adapter/Readme.md | 143 +++++++++++++++++++---------- 1 file changed, 97 insertions(+), 46 deletions(-) diff --git a/awapi/awapi_awiv_adapter/Readme.md b/awapi/awapi_awiv_adapter/Readme.md index 3168dd72bec20..2e5d809881fb8 100644 --- a/awapi/awapi_awiv_adapter/Readme.md +++ b/awapi/awapi_awiv_adapter/Readme.md @@ -1,86 +1,99 @@ # AWAPI_AWIV_ADAPTER +✓: confirmed, (blank): not confirmed ## get topic ### /awapi/vehicle/get/status - get vehicle status -- MessageType: awapi_awiv_adapter/AwapiVehicleStatus.msg - -| type | name | unit | note | -| :------------------------ | :----------------------- | :-------------------------------------------- | :------------------------------------------- | -| std_msgs/Header | header | | | -| geometry_msgs/Pose | pose | position:[m] | | -| awapi_awiv_adapter/Euler | eulerangle | [rad] | roll/pitch/yaw | -| awapi_awiv_adapter/Latlon | latlon | | lat/lon/alt | -| float64 | velocity | [m/s] | | -| float64 | acceleration | [m/ss] | calculate from velocity in awapi_adapter | -| float64 | steering | [rad] | | -| float64 | steering_velocity | [rad/s] | calculate from steering in awapi_adapter | -| float64 | angular_velocity | [rad/s] | | -| int32 | gear | accroding to autoware_vehicle_msgs/Shift | | -| float64 | distance_to_empty | | **not implemented** | -| int32 | turn_signal | accroding to autoware_vehicle_msgs/TurnSignal | | -| float64 | target_velocity | [m/s] | | -| float64 | target_acceleration | [m/ss] | | -| float64 | target_steering | [rad] | | -| float64 | target_steering_velocity | [rad/s] | | +- MessageType: awapi_awiv_adapter/AwapiVehicleStatus + +|✓| type | name | unit | note | +|-| :------------------------ | :----------------------- | :-------------------------------------------- | :------------------------------------------- | +|✓| std_msgs/Header | header | | | +|✓| geometry_msgs/Pose | pose | position:[m] | | +|✓| awapi_awiv_adapter/Euler | eulerangle | [rad] | roll/pitch/yaw | +| | awapi_awiv_adapter/Latlon | latlon | | lat/lon/alt | +|✓| float64 | velocity | [m/s] | | +|✓| float64 | acceleration | [m/ss] | calculate from velocity in awapi_adapter | +|✓| float64 | steering | [rad] | | +|✓| float64 | steering_velocity | [rad/s] | calculate from steering in awapi_adapter | +|✓| float64 | angular_velocity | [rad/s] | | +| | int32 | gear | accroding to autoware_vehicle_msgs/Shift | | +| | float64 | distance_to_empty | | **not implemented** | +| | int32 | turn_signal | accroding to autoware_vehicle_msgs/TurnSignal | | +|✓| float64 | target_velocity | [m/s] | | +|✓| float64 | target_acceleration | [m/ss] | | +|✓| float64 | target_steering | [rad] | | +|✓| float64 | target_steering_velocity | [rad/s] | | ### /awapi/autoware/get/status - get autoware status -- MessageType: awapi_awiv_adapter/AwapiVehicleStatus.msg - -| type | name | unit | note | -| :--------------------------------- | :------------------- | :--------------------------------------------- | :------------------------------------------------------------ | -| std_msgs/Header | header | | | -| string | autoware_state | | | -| int32 | control_mode | accroding to autoware_vehicle_msgs/ControlMode | manual/auto (changed by /awapi/autoware/put/engage) | -| int32 | gate_mode | autoware_vehicle_msgs/GateMode | auto/remote (it is valid only when control_mode=auto)) | -| bool | emergency_stopped | True in emergency mode | | -| StopReasonArray??? | stop_reason | | **not implemented** | -| diagnostic_msgs/DiagnosticStatus[] | diagnostics | | | -| bool | autonomous_overriden | | get info from pacmod msg directly (global_rpt/override_active | -| ) | +- MessageType: awapi_awiv_adapter/AwapiVehicleStatus + +|✓| type | name | unit | note | +|-| :--------------------------------- | :------------------- | :--------------------------------------------- | :------------------------------------------------------------ | +|✓| std_msgs/Header | header | | | +|✓| string | autoware_state | | | +|✓| int32 | control_mode | accroding to autoware_vehicle_msgs/ControlMode | manual/auto (changed by /awapi/autoware/put/engage) | +| | int32 | gate_mode | autoware_vehicle_msgs/GateMode | auto/remote (it is valid only when control_mode=auto)) | +| | bool | emergency_stopped | True in emergency mode | | +| | StopReasonArray??? | stop_reason | | **not implemented** | +|✓| diagnostic_msgs/DiagnosticStatus[] | diagnostics | | | +| | bool | autonomous_overriden | | get info from pacmod msg directly (global_rpt/override_active)| ### /awapi/autoware/get/route - get route - MessageType: autoware_planning_msgs/Route +|✓| type | name | unit | note | +|-| :--------------------------------- | :--- | :--- | :--- | +|✓| autoware_planning_msgs/Route | | | | + ### /awapi/prediction/get/objects - get predicted object - MessageType: autoware_perception_msgs/DynamicObjectArray +|✓| type | name | unit | note | +|-| :------------------------------------------ | :--- | :--- | :--- | +| | autoware_perception_msgs/DynamicObjectArray | | | | + ### /awapi/lane_change/get/status - get lane change information -- MessageType: awapi_awiv_adapter/LaneChangeStatus.msg +- MessageType: awapi_awiv_adapter/LaneChangeStatus -| type | name | unit | note | -| :-------------------------- | :-------------------------- | :--------------------------------------- | :--------------------------------------------------------------------------------- | -| std_msgs/Header | header | | | -| bool | force_lane_change_available | True when lane change is avilable | available: Physically lane changeable state (do not consider other vehicle) | -| bool | lane_change_ready | True when lane change is ready | ready: State that ego-vehicle can change lane without collision with other vehicle | -| autoware_planning_msgs/Path | candidate_path | according to autoware_planning_msgs/Path | | +|✓| type | name | unit | note | +|-| :-------------------------- | :-------------------------- | :--------------------------------------- | :--------------------------------------------------------------------------------- | +| | std_msgs/Header | header | | | +| | bool | force_lane_change_available | True when lane change is avilable | available: Physically lane changeable state (do not consider other vehicle) | +| | bool | lane_change_ready | True when lane change is ready | ready: State that ego-vehicle can change lane without collision with other vehicle | +| | autoware_planning_msgs/Path | candidate_path | according to autoware_planning_msgs/Path | | ### /awapi/object_avoidance/get/status - get obstacle avoidance information -- MessageType: awapi_awiv_adapter/ObstacleAvoidanceStatus.msg +- MessageType: awapi_awiv_adapter/ObstacleAvoidanceStatus -| type | name | unit | note | -| :-------------------------------- | :----------------------- | :--------------------------------------------- | :---------------------------------------------------- | -| std_msgs/Header | header | | | -| bool | obstacle_avoidance_ready | True when obstacle avoidance is ready | | -| autoware_planning_msgs/Trajectory | candidate_path | according to autoware_planning_msgs/Trajectory | Msg type is different from lane change candidate path | +|✓| type | name | unit | note | +|-| :-------------------------------- | :----------------------- | :--------------------------------------------- | :---------------------------------------------------- | +| | std_msgs/Header | header | | | +| | bool | obstacle_avoidance_ready | True when obstacle avoidance is ready | | +| | autoware_planning_msgs/Trajectory | candidate_path | according to autoware_planning_msgs/Trajectory | Msg type is different from lane change candidate path | ### /awapi/traffic_light/get/status - get recognition result of traffic light - MessageType: autoware_perception_msgs/TrafficLightStateArray +|✓| type | name | unit | note | +|-| :---------------------------------------------- | :--- | :--- | :--- | +| | autoware_perception_msgs/TrafficLightStateArray | | | | + + ## put topic ### /awapi/vehicle/put/velocity @@ -88,50 +101,88 @@ - set upper velocity - MessageType: std_msgs/Float32 +|✓| type | name | unit | note | +|-| :--------------- | :--- | :--- | :--- | +| | std_msgs/Float32 | | | | + ### /awapi/autoware/put/gate_mode - send gate mode (auto/remote) - MessageType: autoware_control_msgs/GateMode +|✓| type | name | unit | note | +|-| :----------------------------- | :--- | :--- | :--- | +| | autoware_control_msgs/GateMode | | | | + ### /awapi/autoware/put/emergency - send emergency signal - MessageType: std_msgs/Bool - **To enable this functionality, autoware have to be in the Remote Mode or set _/control/vehicle_cmd_gate/use_emergency_handling_ to true.** +|✓| type | name | unit | note | +|-| :------------ | :--- | :--- | :--- | +| | std_msgs/Bool | | | | + ### /awapi/autoware/put/engage - send engage signal (both of autoware/engage and vehicle/engage) - MessageType: std_msgs/Bool +|✓| type | name | unit | note | +|-| :------------ | :--- | :--- | :--- | +| | std_msgs/Bool | | | | + ### /awapi/autoware/put/goal_pose - send goal pose - MessageType: geometry_msgs/PoseStamped +|✓| type | name | unit | note | +|-| :------------------------ | :--- | :--- | :--- | +| | geometry_msgs/PoseStamped | | | | + ### /awapi/lane_change/put/approval - send lane change approval flag - send True: start lane change when **lane_change_ready** is true - MessageType: std_msgs/Bool +|✓| type | name | unit | note | +|-| :------------ | :--- | :--- | :--- | +| | std_msgs/Bool | | | | + ### /awapi/lane_change/put/force - send force lane change flag - send True: start lane change when **force_lane_change_available** is true - MessageType: std_msgs/Bool +|✓| type | name | unit | note | +|-| :------------ | :--- | :--- | :--- | +| | std_msgs/Bool | | | | + ### /awapi/object_avoidance/put/approval - send object avoidance approval flag - MessageType: std_msgs/Bool +|✓| type | name | unit | note | +|-| :------------ | :--- | :--- | :--- | +| | std_msgs/Bool | | | | + ### /awapi/object_avoidance/put/force - send force object avoidance flag - **not implemented (Autoware does not have corresponded topic)** +|✓| type | name | unit | note | +|-| :------------ | :--- | :--- | :--- | + ### /awapi/traffic_light/put/traffic_light - Overwrite the recognition result of traffic light - **not implemented (Autoware does not have corresponded topic)** + +|✓| type | name | unit | note | +|-| :------------ | :--- | :--- | :--- | From 60eecb17a613a21b59e0ccd6404a9608ba1d23db Mon Sep 17 00:00:00 2001 From: tkimura4 Date: Wed, 5 Aug 2020 16:56:34 +0900 Subject: [PATCH 06/71] Feature/stop reason api (#729) * fix indent of readme * add mock of stop reason aggregator * add constptr to stop reason * aggregate message * add empty handling * add nullptr handling * fix bug * update readme * fix bug * remove unused ifdef * unify same reason msg to stop factor Signed-off-by: tanaka3 --- awapi/awapi_awiv_adapter/CMakeLists.txt | 1 + awapi/awapi_awiv_adapter/Readme.md | 156 +++++++++--------- .../awapi_autoware_state_publisher.h | 5 +- .../awapi_awiv_adapter/awapi_autoware_util.h | 3 +- .../awapi_awiv_adapter_core.h | 5 +- .../awapi_stop_reason_aggregator.h | 45 +++++ .../launch/awapi_awiv_adapter.launch | 3 +- .../src/awapi_autoware_state_publisher.cpp | 19 ++- .../src/awapi_awiv_adapter_core.cpp | 16 +- .../src/awapi_stop_reason_aggregator.cpp | 134 +++++++++++++++ 10 files changed, 286 insertions(+), 101 deletions(-) create mode 100644 awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_stop_reason_aggregator.h create mode 100644 awapi/awapi_awiv_adapter/src/awapi_stop_reason_aggregator.cpp diff --git a/awapi/awapi_awiv_adapter/CMakeLists.txt b/awapi/awapi_awiv_adapter/CMakeLists.txt index 39a6ba4ecc536..e5f36e0a12a31 100644 --- a/awapi/awapi_awiv_adapter/CMakeLists.txt +++ b/awapi/awapi_awiv_adapter/CMakeLists.txt @@ -35,6 +35,7 @@ add_executable(awapi_awiv_adapter src/awapi_awiv_adapter_core.cpp src/awapi_vehicle_state_publisher.cpp src/awapi_autoware_state_publisher.cpp + src/awapi_stop_reason_aggregator.cpp src/awapi_lane_change_state_publisher.cpp src/awapi_obstacle_avoidance_state_publisher.cpp src/awapi_autoware_util.cpp diff --git a/awapi/awapi_awiv_adapter/Readme.md b/awapi/awapi_awiv_adapter/Readme.md index 2e5d809881fb8..607de1a749a4a 100644 --- a/awapi/awapi_awiv_adapter/Readme.md +++ b/awapi/awapi_awiv_adapter/Readme.md @@ -1,4 +1,5 @@ # AWAPI_AWIV_ADAPTER + ✓: confirmed, (blank): not confirmed ## get topic @@ -8,91 +9,90 @@ - get vehicle status - MessageType: awapi_awiv_adapter/AwapiVehicleStatus -|✓| type | name | unit | note | -|-| :------------------------ | :----------------------- | :-------------------------------------------- | :------------------------------------------- | -|✓| std_msgs/Header | header | | | -|✓| geometry_msgs/Pose | pose | position:[m] | | -|✓| awapi_awiv_adapter/Euler | eulerangle | [rad] | roll/pitch/yaw | -| | awapi_awiv_adapter/Latlon | latlon | | lat/lon/alt | -|✓| float64 | velocity | [m/s] | | -|✓| float64 | acceleration | [m/ss] | calculate from velocity in awapi_adapter | -|✓| float64 | steering | [rad] | | -|✓| float64 | steering_velocity | [rad/s] | calculate from steering in awapi_adapter | -|✓| float64 | angular_velocity | [rad/s] | | -| | int32 | gear | accroding to autoware_vehicle_msgs/Shift | | -| | float64 | distance_to_empty | | **not implemented** | -| | int32 | turn_signal | accroding to autoware_vehicle_msgs/TurnSignal | | -|✓| float64 | target_velocity | [m/s] | | -|✓| float64 | target_acceleration | [m/ss] | | -|✓| float64 | target_steering | [rad] | | -|✓| float64 | target_steering_velocity | [rad/s] | | +| ✓ | type | name | unit | note | +| --- | :------------------------ | :----------------------- | :-------------------------------------------- | :------------------------------------------- | +| ✓ | std_msgs/Header | header | | | +| ✓ | geometry_msgs/Pose | pose | position:[m] | | +| ✓ | awapi_awiv_adapter/Euler | eulerangle | [rad] | roll/pitch/yaw | +| | awapi_awiv_adapter/Latlon | latlon | | lat/lon/alt | +| ✓ | float64 | velocity | [m/s] | | +| ✓ | float64 | acceleration | [m/ss] | calculate from velocity in awapi_adapter | +| ✓ | float64 | steering | [rad] | | +| ✓ | float64 | steering_velocity | [rad/s] | calculate from steering in awapi_adapter | +| ✓ | float64 | angular_velocity | [rad/s] | | +| | int32 | gear | accroding to autoware_vehicle_msgs/Shift | | +| | float64 | distance_to_empty | | **not implemented** | +| | int32 | turn_signal | accroding to autoware_vehicle_msgs/TurnSignal | | +| ✓ | float64 | target_velocity | [m/s] | | +| ✓ | float64 | target_acceleration | [m/ss] | | +| ✓ | float64 | target_steering | [rad] | | +| ✓ | float64 | target_steering_velocity | [rad/s] | | ### /awapi/autoware/get/status - get autoware status - MessageType: awapi_awiv_adapter/AwapiVehicleStatus -|✓| type | name | unit | note | -|-| :--------------------------------- | :------------------- | :--------------------------------------------- | :------------------------------------------------------------ | -|✓| std_msgs/Header | header | | | -|✓| string | autoware_state | | | -|✓| int32 | control_mode | accroding to autoware_vehicle_msgs/ControlMode | manual/auto (changed by /awapi/autoware/put/engage) | -| | int32 | gate_mode | autoware_vehicle_msgs/GateMode | auto/remote (it is valid only when control_mode=auto)) | -| | bool | emergency_stopped | True in emergency mode | | -| | StopReasonArray??? | stop_reason | | **not implemented** | -|✓| diagnostic_msgs/DiagnosticStatus[] | diagnostics | | | -| | bool | autonomous_overriden | | get info from pacmod msg directly (global_rpt/override_active)| +| ✓ | type | name | unit | note | +| --- | :------------------------------------- | :------------------- | :--------------------------------------------- | :--------------------------------------------------------------------------- | +| ✓ | std_msgs/Header | header | | | +| ✓ | string | autoware_state | | | +| ✓ | int32 | control_mode | accroding to autoware_vehicle_msgs/ControlMode | manual/auto (changed by /awapi/autoware/put/engage) | +| | int32 | gate_mode | autoware_vehicle_msgs/GateMode | auto/remote (it is valid only when control_mode=auto)) | +| | bool | emergency_stopped | True in emergency mode | | +| | autoware_planning_msgs/StopReasonArray | stop_reason | | "stop_pose" represents the position of "base_link" (not the head of the car) | +| ✓ | diagnostic_msgs/DiagnosticStatus[] | diagnostics | | | +| | bool | autonomous_overriden | | get info from pacmod msg directly (global_rpt/override_active) | ### /awapi/autoware/get/route - get route - MessageType: autoware_planning_msgs/Route -|✓| type | name | unit | note | -|-| :--------------------------------- | :--- | :--- | :--- | -|✓| autoware_planning_msgs/Route | | | | +| ✓ | type | name | unit | note | +| --- | :--------------------------- | :--- | :--- | :--- | +| ✓ | autoware_planning_msgs/Route | | | | ### /awapi/prediction/get/objects - get predicted object - MessageType: autoware_perception_msgs/DynamicObjectArray -|✓| type | name | unit | note | -|-| :------------------------------------------ | :--- | :--- | :--- | -| | autoware_perception_msgs/DynamicObjectArray | | | | +| ✓ | type | name | unit | note | +| --- | :------------------------------------------ | :--- | :--- | :--- | +| | autoware_perception_msgs/DynamicObjectArray | | | | ### /awapi/lane_change/get/status - get lane change information - MessageType: awapi_awiv_adapter/LaneChangeStatus -|✓| type | name | unit | note | -|-| :-------------------------- | :-------------------------- | :--------------------------------------- | :--------------------------------------------------------------------------------- | -| | std_msgs/Header | header | | | -| | bool | force_lane_change_available | True when lane change is avilable | available: Physically lane changeable state (do not consider other vehicle) | -| | bool | lane_change_ready | True when lane change is ready | ready: State that ego-vehicle can change lane without collision with other vehicle | -| | autoware_planning_msgs/Path | candidate_path | according to autoware_planning_msgs/Path | | +| ✓ | type | name | unit | note | +| --- | :-------------------------- | :-------------------------- | :--------------------------------------- | :--------------------------------------------------------------------------------- | +| | std_msgs/Header | header | | | +| | bool | force_lane_change_available | True when lane change is avilable | available: Physically lane changeable state (do not consider other vehicle) | +| | bool | lane_change_ready | True when lane change is ready | ready: State that ego-vehicle can change lane without collision with other vehicle | +| | autoware_planning_msgs/Path | candidate_path | according to autoware_planning_msgs/Path | | ### /awapi/object_avoidance/get/status - get obstacle avoidance information - MessageType: awapi_awiv_adapter/ObstacleAvoidanceStatus -|✓| type | name | unit | note | -|-| :-------------------------------- | :----------------------- | :--------------------------------------------- | :---------------------------------------------------- | -| | std_msgs/Header | header | | | -| | bool | obstacle_avoidance_ready | True when obstacle avoidance is ready | | -| | autoware_planning_msgs/Trajectory | candidate_path | according to autoware_planning_msgs/Trajectory | Msg type is different from lane change candidate path | +| ✓ | type | name | unit | note | +| --- | :-------------------------------- | :----------------------- | :--------------------------------------------- | :---------------------------------------------------- | +| | std_msgs/Header | header | | | +| | bool | obstacle_avoidance_ready | True when obstacle avoidance is ready | | +| | autoware_planning_msgs/Trajectory | candidate_path | according to autoware_planning_msgs/Trajectory | Msg type is different from lane change candidate path | ### /awapi/traffic_light/get/status - get recognition result of traffic light - MessageType: autoware_perception_msgs/TrafficLightStateArray -|✓| type | name | unit | note | -|-| :---------------------------------------------- | :--- | :--- | :--- | -| | autoware_perception_msgs/TrafficLightStateArray | | | | - +| ✓ | type | name | unit | note | +| --- | :---------------------------------------------- | :--- | :--- | :--- | +| | autoware_perception_msgs/TrafficLightStateArray | | | | ## put topic @@ -101,18 +101,18 @@ - set upper velocity - MessageType: std_msgs/Float32 -|✓| type | name | unit | note | -|-| :--------------- | :--- | :--- | :--- | -| | std_msgs/Float32 | | | | +| ✓ | type | name | unit | note | +| --- | :--------------- | :--- | :--- | :--- | +| | std_msgs/Float32 | | | | ### /awapi/autoware/put/gate_mode - send gate mode (auto/remote) - MessageType: autoware_control_msgs/GateMode -|✓| type | name | unit | note | -|-| :----------------------------- | :--- | :--- | :--- | -| | autoware_control_msgs/GateMode | | | | +| ✓ | type | name | unit | note | +| --- | :----------------------------- | :--- | :--- | :--- | +| | autoware_control_msgs/GateMode | | | | ### /awapi/autoware/put/emergency @@ -120,27 +120,27 @@ - MessageType: std_msgs/Bool - **To enable this functionality, autoware have to be in the Remote Mode or set _/control/vehicle_cmd_gate/use_emergency_handling_ to true.** -|✓| type | name | unit | note | -|-| :------------ | :--- | :--- | :--- | -| | std_msgs/Bool | | | | +| ✓ | type | name | unit | note | +| --- | :------------ | :--- | :--- | :--- | +| | std_msgs/Bool | | | | ### /awapi/autoware/put/engage - send engage signal (both of autoware/engage and vehicle/engage) - MessageType: std_msgs/Bool -|✓| type | name | unit | note | -|-| :------------ | :--- | :--- | :--- | -| | std_msgs/Bool | | | | +| ✓ | type | name | unit | note | +| --- | :------------ | :--- | :--- | :--- | +| | std_msgs/Bool | | | | ### /awapi/autoware/put/goal_pose - send goal pose - MessageType: geometry_msgs/PoseStamped -|✓| type | name | unit | note | -|-| :------------------------ | :--- | :--- | :--- | -| | geometry_msgs/PoseStamped | | | | +| ✓ | type | name | unit | note | +| --- | :------------------------ | :--- | :--- | :--- | +| | geometry_msgs/PoseStamped | | | | ### /awapi/lane_change/put/approval @@ -148,9 +148,9 @@ - send True: start lane change when **lane_change_ready** is true - MessageType: std_msgs/Bool -|✓| type | name | unit | note | -|-| :------------ | :--- | :--- | :--- | -| | std_msgs/Bool | | | | +| ✓ | type | name | unit | note | +| --- | :------------ | :--- | :--- | :--- | +| | std_msgs/Bool | | | | ### /awapi/lane_change/put/force @@ -158,31 +158,33 @@ - send True: start lane change when **force_lane_change_available** is true - MessageType: std_msgs/Bool -|✓| type | name | unit | note | -|-| :------------ | :--- | :--- | :--- | -| | std_msgs/Bool | | | | +| ✓ | type | name | unit | note | +| --- | :------------ | :--- | :--- | :--- | +| | std_msgs/Bool | | | | ### /awapi/object_avoidance/put/approval - send object avoidance approval flag - MessageType: std_msgs/Bool -|✓| type | name | unit | note | -|-| :------------ | :--- | :--- | :--- | -| | std_msgs/Bool | | | | +| ✓ | type | name | unit | note | +| --- | :------------ | :--- | :--- | :--- | +| | std_msgs/Bool | | | | ### /awapi/object_avoidance/put/force - send force object avoidance flag - **not implemented (Autoware does not have corresponded topic)** -|✓| type | name | unit | note | -|-| :------------ | :--- | :--- | :--- | +| ✓ | type | name | unit | note | +| --- | :--- | :--- | :--- | :--- | + ### /awapi/traffic_light/put/traffic_light - Overwrite the recognition result of traffic light - **not implemented (Autoware does not have corresponded topic)** -|✓| type | name | unit | note | -|-| :------------ | :--- | :--- | :--- | +| ✓ | type | name | unit | note | +| --- | :--- | :--- | :--- | :--- | + diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.h b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.h index 6b2fbc5359538..7e1fdd5fcfebf 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.h +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.h @@ -45,12 +45,9 @@ class AutowareIvAutowareStatePublisher void getIsEmergencyInfo( const std_msgs::Bool::ConstPtr & is_emergency_ptr, autoware_api_msgs::AwapiAutowareStatus * status); - // TODO: not implemented - /* void getStopReasonInfo( - const - & stop_reason_ptr, + const autoware_planning_msgs::StopReasonArray::ConstPtr & stop_reason_ptr, autoware_api_msgs::AwapiAutowareStatus * status); - */ void getDiagInfo( const diagnostic_msgs::DiagnosticArray::ConstPtr & diag_ptr, autoware_api_msgs::AwapiAutowareStatus * status); diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.h b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.h index 1dff31e75fa9e..a13af3b02c976 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.h +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.h @@ -31,6 +31,7 @@ #include #include +#include #include #include #include @@ -55,7 +56,7 @@ struct AutowareInfo autoware_vehicle_msgs::ControlMode::ConstPtr control_mode_ptr; autoware_control_msgs::GateMode::ConstPtr gate_mode_ptr; std_msgs::Bool::ConstPtr is_emergency_ptr; - // TODO ?? stop_reason_ptr; + autoware_planning_msgs::StopReasonArray::ConstPtr stop_reason_ptr; diagnostic_msgs::DiagnosticArray::ConstPtr diagnostic_ptr; pacmod_msgs::GlobalRpt::ConstPtr global_rpt_ptr; std_msgs::Bool::ConstPtr lane_change_available_ptr; diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.h b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.h index d402a58bbdcbe..87ec85ada960b 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.h +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.h @@ -18,6 +18,7 @@ #include #include #include +#include #include namespace autoware_api @@ -69,7 +70,7 @@ class AutowareIvAdapter void callbackControlMode(const autoware_vehicle_msgs::ControlMode::ConstPtr & msg_ptr); void callbackGateMode(const autoware_control_msgs::GateMode::ConstPtr & msg_ptr); void callbackIsEmergency(const std_msgs::Bool::ConstPtr & msg_ptr); - // void callbackStopReason(); // TODO: not implemented + void callbackStopReason(const autoware_planning_msgs::StopReasonArray::ConstPtr & msg_ptr); void callbackDiagnostics(const diagnostic_msgs::DiagnosticArray::ConstPtr & msg_ptr); void callbackGlobalRpt(const pacmod_msgs::GlobalRpt::ConstPtr & msg_ptr); void callbackLaneChangeAvailable(const std_msgs::Bool::ConstPtr & msg_ptr); @@ -89,9 +90,11 @@ class AutowareIvAdapter AutowareInfo aw_info_; std::unique_ptr vehicle_state_publisher_; std::unique_ptr autoware_state_publisher_; + std::unique_ptr stop_reason_aggreagator_; std::unique_ptr lane_change_state_publisher_; std::unique_ptr obstacle_avoidance_state_publisher_; double status_pub_hz_; + double stop_reason_timeout_; }; } // namespace autoware_api diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_stop_reason_aggregator.h b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_stop_reason_aggregator.h new file mode 100644 index 0000000000000..32e1f4abfcebf --- /dev/null +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_stop_reason_aggregator.h @@ -0,0 +1,45 @@ +/* + * Copyright 2020 Tier IV, Inc. All rights reserved. + * + * 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. + */ + +#pragma once + +#include + +namespace autoware_api +{ +class AutowareIvStopReasonAggregator +{ +public: + AutowareIvStopReasonAggregator(const double timeout); + autoware_planning_msgs::StopReasonArray::ConstPtr updateStopReasonArray( + const autoware_planning_msgs::StopReasonArray::ConstPtr & msg_ptr); + +private: + void applyUpdate(const autoware_planning_msgs::StopReasonArray::ConstPtr & msg_ptr); + bool checkMatchingReason( + const autoware_planning_msgs::StopReasonArray::ConstPtr & msg_stop_reason_array, + const autoware_planning_msgs::StopReasonArray & stop_reason_array); + void applyTimeOut(); + void appendStopReasonToArray( + const autoware_planning_msgs::StopReason & stop_reason, + autoware_planning_msgs::StopReasonArray * stop_reason_array); + autoware_planning_msgs::StopReasonArray::ConstPtr makeStopReasonArray(); + + double timeout_; + std::vector stop_reason_array_vec_; +}; + +} // namespace autoware_api diff --git a/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch b/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch index c9ac2b436e6ce..6cc0d0f9f89c9 100644 --- a/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch +++ b/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch @@ -2,6 +2,7 @@ + @@ -13,7 +14,7 @@ - + diff --git a/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp b/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp index 40a2168d82502..246dd434640f7 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp @@ -37,7 +37,7 @@ void AutowareIvAutowareStatePublisher::statePublisher(const AutowareInfo & aw_in getControlModeInfo(aw_info.control_mode_ptr, &status); getGateModeInfo(aw_info.gate_mode_ptr, &status); getIsEmergencyInfo(aw_info.is_emergency_ptr, &status); - // getStopReasonInfo(aw_info.stop_reason_ptr, &status); // TODO: not implemented + getStopReasonInfo(aw_info.stop_reason_ptr, &status); getDiagInfo(aw_info.diagnostic_ptr, &status); getGlobalRptInfo(aw_info.global_rpt_ptr, &status); @@ -97,12 +97,17 @@ void AutowareIvAutowareStatePublisher::getIsEmergencyInfo( status->emergency_stopped = is_emergency_ptr->data; } -// TODO: not implemented -/* - void AutowareIvAutowareStatePublisher::getStopReasonInfo( - const - & stop_reason_ptr, - autoware_api_msgs::AwapiAutowareStatus * status){} - */ +void AutowareIvAutowareStatePublisher::getStopReasonInfo( + const autoware_planning_msgs::StopReasonArray::ConstPtr & stop_reason_ptr, + autoware_api_msgs::AwapiAutowareStatus * status) +{ + if (!stop_reason_ptr) { + ROS_WARN_STREAM_THROTTLE(5.0, "[AutowareIvAutowareStatePublisher] stop reason is nullptr"); + return; + } + + status->stop_reason = *stop_reason_ptr; +} void AutowareIvAutowareStatePublisher::getDiagInfo( const diagnostic_msgs::DiagnosticArray::ConstPtr & diag_ptr, diff --git a/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp b/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp index 23a0b51449616..8b527b0901a45 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp @@ -22,12 +22,14 @@ AutowareIvAdapter::AutowareIvAdapter() : nh_(), pnh_("~"), tf_listener_(tf_buffe { // get param pnh_.param("status_pub_hz", status_pub_hz_, 5.0); + pnh_.param("stop_reason_timeout", stop_reason_timeout_, 0.5); const bool em_handle_param = waitForParam(pnh_, "param/emergency_handling"); emergencyParamCheck(em_handle_param); // setup instance vehicle_state_publisher_ = std::make_unique(); autoware_state_publisher_ = std::make_unique(); + stop_reason_aggreagator_ = std::make_unique(stop_reason_timeout_); lane_change_state_publisher_ = std::make_unique(); obstacle_avoidance_state_publisher_ = std::make_unique(); @@ -48,11 +50,8 @@ AutowareIvAdapter::AutowareIvAdapter() : nh_(), pnh_("~"), tf_listener_(tf_buffe sub_gate_mode_ = pnh_.subscribe("input/gate_mode", 1, &AutowareIvAdapter::callbackGateMode, this); sub_emergency_ = pnh_.subscribe("input/is_emergency", 1, &AutowareIvAdapter::callbackIsEmergency, this); - //TODO: not implemented - /* sub_stop_reason_ = - pnh_.subscribe("input/stop_reason", 1, &AutowareIvAdapter::callbackStopReason, this); - */ + pnh_.subscribe("input/stop_reason", 100, &AutowareIvAdapter::callbackStopReason, this); sub_diagnostics_ = pnh_.subscribe("input/diagnostics", 1, &AutowareIvAdapter::callbackDiagnostics, this); sub_global_rpt_ = @@ -170,13 +169,11 @@ void AutowareIvAdapter::callbackIsEmergency(const std_msgs::Bool::ConstPtr & msg aw_info_.is_emergency_ptr = msg_ptr; } -// TODO: not implemented -/* -void AutowareIvAdapter::callbackStopReason(& msg_ptr) +void AutowareIvAdapter::callbackStopReason( + const autoware_planning_msgs::StopReasonArrayConstPtr & msg_ptr) { - aw_info_.stop_reason_ptr = msg_ptr; + aw_info_.stop_reason_ptr = stop_reason_aggreagator_->updateStopReasonArray(msg_ptr); } -*/ void AutowareIvAdapter::callbackDiagnostics( const diagnostic_msgs::DiagnosticArray::ConstPtr & msg_ptr) @@ -186,7 +183,6 @@ void AutowareIvAdapter::callbackDiagnostics( void AutowareIvAdapter::callbackGlobalRpt(const pacmod_msgs::GlobalRpt::ConstPtr & msg_ptr) { - // TODO: now, it is not used aw_info_.global_rpt_ptr = msg_ptr; } diff --git a/awapi/awapi_awiv_adapter/src/awapi_stop_reason_aggregator.cpp b/awapi/awapi_awiv_adapter/src/awapi_stop_reason_aggregator.cpp new file mode 100644 index 0000000000000..07396e51f8c31 --- /dev/null +++ b/awapi/awapi_awiv_adapter/src/awapi_stop_reason_aggregator.cpp @@ -0,0 +1,134 @@ +/* + * Copyright 2020 Tier IV, Inc. All rights reserved. + * + * 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. + */ + +#include + +namespace autoware_api +{ +AutowareIvStopReasonAggregator::AutowareIvStopReasonAggregator(const double timeout) +: timeout_(timeout) +{ +} + +autoware_planning_msgs::StopReasonArray::ConstPtr +AutowareIvStopReasonAggregator::updateStopReasonArray( + const autoware_planning_msgs::StopReasonArray::ConstPtr & msg_ptr) +{ + applyUpdate(msg_ptr); + applyTimeOut(); + return makeStopReasonArray(); +} + +void AutowareIvStopReasonAggregator::applyUpdate( + const autoware_planning_msgs::StopReasonArray::ConstPtr & msg_ptr) +{ + /* remove old stop_reason that matches reason with received msg */ + //make reason-matching msg list + + std::vector remove_idx; + if (!stop_reason_array_vec_.empty()) { + for (int i = stop_reason_array_vec_.size() - 1; i >= 0; i--) { + if (checkMatchingReason(msg_ptr, stop_reason_array_vec_.at(i))) { + remove_idx.emplace_back(i); + } + } + } + + // remove reason matching msg + for (const auto idx : remove_idx) { + stop_reason_array_vec_.erase(stop_reason_array_vec_.begin() + idx); + } + + // add new reason msg + stop_reason_array_vec_.emplace_back(*msg_ptr); +} + +bool AutowareIvStopReasonAggregator::checkMatchingReason( + const autoware_planning_msgs::StopReasonArray::ConstPtr & msg_stop_reason_array, + const autoware_planning_msgs::StopReasonArray & stop_reason_array) +{ + for (const auto msg_stop_reason : msg_stop_reason_array->stop_reasons) { + for (const auto stop_reason : stop_reason_array.stop_reasons) { + if (msg_stop_reason.reason == stop_reason.reason) { + //find matching reason + return true; + } + } + } + // cannot find matching reason + return false; +} + +void AutowareIvStopReasonAggregator::applyTimeOut() +{ + const auto current_time = ros::Time::now(); + + //make timeout-msg list + std::vector remove_idx; + if (!stop_reason_array_vec_.empty()) { + for (int i = stop_reason_array_vec_.size() - 1; i >= 0; i--) { + if ((current_time - stop_reason_array_vec_.at(i).header.stamp).toSec() > timeout_) { + remove_idx.emplace_back(i); + } + } + } + //remove timeout-msg + for (const auto idx : remove_idx) { + stop_reason_array_vec_.erase(stop_reason_array_vec_.begin() + idx); + } +} + +void AutowareIvStopReasonAggregator::appendStopReasonToArray( + const autoware_planning_msgs::StopReason & stop_reason, + autoware_planning_msgs::StopReasonArray * stop_reason_array) +{ + //if stop factors is empty, not append + if (stop_reason.stop_factors.empty()) { + return; + } + + //if already exists same reason msg in stop_reason_array_msg, append stop_factors to there + for (int i = 0; i < stop_reason_array->stop_reasons.size(); i++) { + if (stop_reason_array->stop_reasons.at(i).reason == stop_reason.reason) { + stop_reason_array->stop_reasons.at(i).stop_factors.insert( + stop_reason_array->stop_reasons.at(i).stop_factors.end(), stop_reason.stop_factors.begin(), + stop_reason.stop_factors.end()); + return; + } + } + + //if not exist same reason msg, append new stop reason + stop_reason_array->stop_reasons.emplace_back(stop_reason); +} + +autoware_planning_msgs::StopReasonArray::ConstPtr +AutowareIvStopReasonAggregator::makeStopReasonArray() +{ + autoware_planning_msgs::StopReasonArray stop_reason_array_msg; + // input header + stop_reason_array_msg.header.frame_id = "map"; + stop_reason_array_msg.header.stamp = ros::Time::now(); + + // input stop reason + for (const auto stop_reason_array : stop_reason_array_vec_) { + for (const auto stop_reason : stop_reason_array.stop_reasons) { + appendStopReasonToArray(stop_reason, &stop_reason_array_msg); + } + } + return boost::make_shared(stop_reason_array_msg); +} + +} // namespace autoware_api \ No newline at end of file From 3eee0e6ee395385bd5565405b59865fe614092ea Mon Sep 17 00:00:00 2001 From: hiroyuki obinata <58019445+obi-t4@users.noreply.github.com> Date: Thu, 6 Aug 2020 00:53:13 +0900 Subject: [PATCH 07/71] modify put/goal -> put/route (#736) Signed-off-by: tanaka3 --- awapi/awapi_awiv_adapter/Readme.md | 20 +++++++++---------- .../launch/awapi_awiv_adapter.launch | 6 +++--- 2 files changed, 13 insertions(+), 13 deletions(-) diff --git a/awapi/awapi_awiv_adapter/Readme.md b/awapi/awapi_awiv_adapter/Readme.md index 607de1a749a4a..d09fe33579091 100644 --- a/awapi/awapi_awiv_adapter/Readme.md +++ b/awapi/awapi_awiv_adapter/Readme.md @@ -40,7 +40,7 @@ | ✓ | int32 | control_mode | accroding to autoware_vehicle_msgs/ControlMode | manual/auto (changed by /awapi/autoware/put/engage) | | | int32 | gate_mode | autoware_vehicle_msgs/GateMode | auto/remote (it is valid only when control_mode=auto)) | | | bool | emergency_stopped | True in emergency mode | | -| | autoware_planning_msgs/StopReasonArray | stop_reason | | "stop_pose" represents the position of "base_link" (not the head of the car) | +| ✓ | autoware_planning_msgs/StopReasonArray | stop_reason | | "stop_pose" represents the position of "base_link" (not the head of the car) | | ✓ | diagnostic_msgs/DiagnosticStatus[] | diagnostics | | | | | bool | autonomous_overriden | | get info from pacmod msg directly (global_rpt/override_active) | @@ -60,7 +60,7 @@ | ✓ | type | name | unit | note | | --- | :------------------------------------------ | :--- | :--- | :--- | -| | autoware_perception_msgs/DynamicObjectArray | | | | +| ✓ | autoware_perception_msgs/DynamicObjectArray | | | | ### /awapi/lane_change/get/status @@ -101,9 +101,9 @@ - set upper velocity - MessageType: std_msgs/Float32 -| ✓ | type | name | unit | note | -| --- | :--------------- | :--- | :--- | :--- | -| | std_msgs/Float32 | | | | +| ✓ | type | name | unit | note | +| --- | :--------------- | :--- | :--- | :----------- | +| ✓ | std_msgs/Float32 | | | max velocity | ### /awapi/autoware/put/gate_mode @@ -133,14 +133,14 @@ | --- | :------------ | :--- | :--- | :--- | | | std_msgs/Bool | | | | -### /awapi/autoware/put/goal_pose +### /awapi/autoware/put/route - send goal pose -- MessageType: geometry_msgs/PoseStamped +- MessageType: autoware_planning_msgs/Route -| ✓ | type | name | unit | note | -| --- | :------------------------ | :--- | :--- | :--- | -| | geometry_msgs/PoseStamped | | | | +| ✓ | type | name | unit | note | +| --- | :--------------------------- | :--- | :--- | :--- | +| | autoware_planning_msgs/Route | | | | ### /awapi/lane_change/put/approval diff --git a/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch b/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch index 6cc0d0f9f89c9..4dca2380123f2 100644 --- a/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch +++ b/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch @@ -35,7 +35,7 @@ - + @@ -48,7 +48,7 @@ - + @@ -97,7 +97,7 @@ - + From 0a7d95f2fe3632ec4b1732824b2170156f4778bf Mon Sep 17 00:00:00 2001 From: tkimura4 Date: Thu, 6 Aug 2020 10:40:13 +0900 Subject: [PATCH 08/71] Feature/add goal api (#734) * add goal api * add msg * update readme * add const auto * clean code Signed-off-by: tanaka3 --- awapi/awapi_awiv_adapter/Readme.md | 1 + .../awapi_autoware_state_publisher.h | 8 +++++ .../src/awapi_autoware_state_publisher.cpp | 30 ++++++++++++++++++- 3 files changed, 38 insertions(+), 1 deletion(-) diff --git a/awapi/awapi_awiv_adapter/Readme.md b/awapi/awapi_awiv_adapter/Readme.md index d09fe33579091..8841bb102ff9e 100644 --- a/awapi/awapi_awiv_adapter/Readme.md +++ b/awapi/awapi_awiv_adapter/Readme.md @@ -43,6 +43,7 @@ | ✓ | autoware_planning_msgs/StopReasonArray | stop_reason | | "stop_pose" represents the position of "base_link" (not the head of the car) | | ✓ | diagnostic_msgs/DiagnosticStatus[] | diagnostics | | | | | bool | autonomous_overriden | | get info from pacmod msg directly (global_rpt/override_active) | +| | bool | arrived_goal | | | ### /awapi/autoware/get/route diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.h b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.h index 7e1fdd5fcfebf..0a13c65d8fb90 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.h +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.h @@ -33,6 +33,12 @@ class AutowareIvAutowareStatePublisher // publisher ros::Publisher pub_state_; + // parameter + + /* parameter for judging goal now */ + bool arrived_goal_; + autoware_system_msgs::AutowareState::_state_type prev_state_; + void getAutowareStateInfo( const autoware_system_msgs::AutowareState::ConstPtr & autoware_state_ptr, autoware_api_msgs::AwapiAutowareStatus * status); @@ -55,6 +61,8 @@ class AutowareIvAutowareStatePublisher void getGlobalRptInfo( const pacmod_msgs::GlobalRpt::ConstPtr & global_rpt_ptr, autoware_api_msgs::AwapiAutowareStatus * status); + + bool isGoal(const autoware_system_msgs::AutowareState::ConstPtr & autoware_state); }; } // namespace autoware_api diff --git a/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp b/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp index 246dd434640f7..fcbc49daa555a 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp @@ -18,7 +18,8 @@ namespace autoware_api { -AutowareIvAutowareStatePublisher::AutowareIvAutowareStatePublisher() : nh_(), pnh_("~") +AutowareIvAutowareStatePublisher::AutowareIvAutowareStatePublisher() +: nh_(), pnh_("~"), arrived_goal_(false) { // publisher pub_state_ = pnh_.advertise("output/autoware_status", 1); @@ -56,6 +57,7 @@ void AutowareIvAutowareStatePublisher::getAutowareStateInfo( // get autoware_state status->autoware_state = autoware_state_ptr->state; + status->arrived_goal = isGoal(autoware_state_ptr); } void AutowareIvAutowareStatePublisher::getControlModeInfo( @@ -135,4 +137,30 @@ void AutowareIvAutowareStatePublisher::getGlobalRptInfo( status->autonomous_overriden = global_rpt_ptr->override_active; } +bool AutowareIvAutowareStatePublisher::isGoal( + const autoware_system_msgs::AutowareState::ConstPtr & autoware_state) +{ + //rename + const auto & aw_state = autoware_state->state; + + if (aw_state == autoware_system_msgs::AutowareState::ArrivedGoal) { + arrived_goal_ = true; + } else if ( + prev_state_ == autoware_system_msgs::AutowareState::Driving && + aw_state == autoware_system_msgs::AutowareState::WaitingForRoute) { + arrived_goal_ = true; + } + + if ( + aw_state == autoware_system_msgs::AutowareState::WaitingForEngage || + aw_state == autoware_system_msgs::AutowareState::Driving) { + //cancel goal state + arrived_goal_ = false; + } + + prev_state_ = aw_state; + + return arrived_goal_; +} + } // namespace autoware_api From 65a9135395802a66a86c11eee379412e431d56d3 Mon Sep 17 00:00:00 2001 From: tkimura4 Date: Wed, 12 Aug 2020 12:32:35 +0900 Subject: [PATCH 09/71] Feature/extract leaf diag info (#741) * add mock function of extract leaf diag * remove parent diag * change method of isLeaf * clean code * change varibale/function name * remove string predefine * update readme Signed-off-by: tanaka3 --- awapi/awapi_awiv_adapter/Readme.md | 2 +- .../awapi_autoware_state_publisher.h | 8 ++++ .../src/awapi_autoware_state_publisher.cpp | 44 ++++++++++++++++++- 3 files changed, 52 insertions(+), 2 deletions(-) diff --git a/awapi/awapi_awiv_adapter/Readme.md b/awapi/awapi_awiv_adapter/Readme.md index 8841bb102ff9e..6eb8f0064ea67 100644 --- a/awapi/awapi_awiv_adapter/Readme.md +++ b/awapi/awapi_awiv_adapter/Readme.md @@ -41,7 +41,7 @@ | | int32 | gate_mode | autoware_vehicle_msgs/GateMode | auto/remote (it is valid only when control_mode=auto)) | | | bool | emergency_stopped | True in emergency mode | | | ✓ | autoware_planning_msgs/StopReasonArray | stop_reason | | "stop_pose" represents the position of "base_link" (not the head of the car) | -| ✓ | diagnostic_msgs/DiagnosticStatus[] | diagnostics | | | +| ✓ | diagnostic_msgs/DiagnosticStatus[] | diagnostics | | output only diag. of leaf node (diag. of parent node are cut) | | | bool | autonomous_overriden | | get info from pacmod msg directly (global_rpt/override_active) | | | bool | arrived_goal | | | diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.h b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.h index 0a13c65d8fb90..32a95428a4a35 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.h +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.h @@ -39,6 +39,9 @@ class AutowareIvAutowareStatePublisher bool arrived_goal_; autoware_system_msgs::AutowareState::_state_type prev_state_; + /* parameter for judging diag leaf */ + std::set diag_name_set_; + void getAutowareStateInfo( const autoware_system_msgs::AutowareState::ConstPtr & autoware_state_ptr, autoware_api_msgs::AwapiAutowareStatus * status); @@ -63,6 +66,11 @@ class AutowareIvAutowareStatePublisher autoware_api_msgs::AwapiAutowareStatus * status); bool isGoal(const autoware_system_msgs::AutowareState::ConstPtr & autoware_state); + std::vector extractLeafDiag( + const std::vector & diag_vec); + std::string splitStringByLastSlash(const std::string & str); + void updateDiagNameSet(const std::vector & diag_vec); + bool isLeaf(const diagnostic_msgs::DiagnosticStatus & diag); }; } // namespace autoware_api diff --git a/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp b/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp index fcbc49daa555a..df09aad032096 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp @@ -15,6 +15,7 @@ */ #include +#include namespace autoware_api { @@ -121,7 +122,7 @@ void AutowareIvAutowareStatePublisher::getDiagInfo( } // get diag - status->diagnostics = diag_ptr->status; + status->diagnostics = extractLeafDiag(diag_ptr->status); } void AutowareIvAutowareStatePublisher::getGlobalRptInfo( @@ -163,4 +164,45 @@ bool AutowareIvAutowareStatePublisher::isGoal( return arrived_goal_; } +std::vector AutowareIvAutowareStatePublisher::extractLeafDiag( + const std::vector & diag_vec) +{ + updateDiagNameSet(diag_vec); + + std::vector leaf_diag_info; + for (const auto diag : diag_vec) { + if (isLeaf(diag)) { + leaf_diag_info.emplace_back(diag); + } + } + return leaf_diag_info; +} + +std::string AutowareIvAutowareStatePublisher::splitStringByLastSlash(const std::string & str) +{ + const auto last_slash = str.find_last_of("/"); + + if (last_slash == std::string::npos) { + // if not find slash + return str; + } + + return str.substr(0, last_slash); +} + +void AutowareIvAutowareStatePublisher::updateDiagNameSet( + const std::vector & diag_vec) +{ + // set diag name to diag_name_set_ + for (const auto & diag : diag_vec) { + diag_name_set_.insert(splitStringByLastSlash(diag.name)); + } +} + +bool AutowareIvAutowareStatePublisher::isLeaf(const diagnostic_msgs::DiagnosticStatus & diag) +{ + //if not find diag.name in diag set, diag is leaf. + return diag_name_set_.find(diag.name) == diag_name_set_.end(); +} + } // namespace autoware_api From 5e2f2c2bd3dd32f4c82108b9e6c31fb2017c837b Mon Sep 17 00:00:00 2001 From: hiroyuki obinata <58019445+obi-t4@users.noreply.github.com> Date: Tue, 18 Aug 2020 10:26:59 +0900 Subject: [PATCH 10/71] update awapi readme (#747) Signed-off-by: tanaka3 --- awapi/awapi_awiv_adapter/Readme.md | 30 ++++++++++++++---------------- 1 file changed, 14 insertions(+), 16 deletions(-) diff --git a/awapi/awapi_awiv_adapter/Readme.md b/awapi/awapi_awiv_adapter/Readme.md index 6eb8f0064ea67..85672ea05ca1e 100644 --- a/awapi/awapi_awiv_adapter/Readme.md +++ b/awapi/awapi_awiv_adapter/Readme.md @@ -21,8 +21,7 @@ | ✓ | float64 | steering_velocity | [rad/s] | calculate from steering in awapi_adapter | | ✓ | float64 | angular_velocity | [rad/s] | | | | int32 | gear | accroding to autoware_vehicle_msgs/Shift | | -| | float64 | distance_to_empty | | **not implemented** | -| | int32 | turn_signal | accroding to autoware_vehicle_msgs/TurnSignal | | +| ✓ | int32 | turn_signal | accroding to autoware_vehicle_msgs/TurnSignal | | | ✓ | float64 | target_velocity | [m/s] | | | ✓ | float64 | target_acceleration | [m/ss] | | | ✓ | float64 | target_steering | [rad] | | @@ -33,17 +32,16 @@ - get autoware status - MessageType: awapi_awiv_adapter/AwapiVehicleStatus -| ✓ | type | name | unit | note | -| --- | :------------------------------------- | :------------------- | :--------------------------------------------- | :--------------------------------------------------------------------------- | -| ✓ | std_msgs/Header | header | | | -| ✓ | string | autoware_state | | | -| ✓ | int32 | control_mode | accroding to autoware_vehicle_msgs/ControlMode | manual/auto (changed by /awapi/autoware/put/engage) | -| | int32 | gate_mode | autoware_vehicle_msgs/GateMode | auto/remote (it is valid only when control_mode=auto)) | -| | bool | emergency_stopped | True in emergency mode | | -| ✓ | autoware_planning_msgs/StopReasonArray | stop_reason | | "stop_pose" represents the position of "base_link" (not the head of the car) | -| ✓ | diagnostic_msgs/DiagnosticStatus[] | diagnostics | | output only diag. of leaf node (diag. of parent node are cut) | -| | bool | autonomous_overriden | | get info from pacmod msg directly (global_rpt/override_active) | -| | bool | arrived_goal | | | +| ✓ | type | name | unit | note | +| --- | :------------------------------------- | :------------------- | :--------------------------------------------- | :-------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| ✓ | std_msgs/Header | header | | | +| ✓ | string | autoware_state | | | +| ✓ | int32 | control_mode | accroding to autoware_vehicle_msgs/ControlMode | manual/auto (changed by /awapi/autoware/put/engage) | +| | int32 | gate_mode | autoware_vehicle_msgs/GateMode | auto/remote (it is valid only when control_mode=auto)) | +| ✓ | bool | emergency_stopped | True in emergency mode | | +| ✓ | autoware_planning_msgs/StopReasonArray | stop_reason | | "stop_pose" represents the position of "base_link" (not the head of the car) | +| ✓ | diagnostic_msgs/DiagnosticStatus[] | diagnostics | | output only diag. of leaf node (diag. of parent node are cut) | +| ✓ | bool | arrived_goal | | True if the autoware_state is changed from Driving to ArrivedGoal or WairingForRoute. False if the autoware_state is changed to WaitingForEngage or Driving. Default False. | ### /awapi/autoware/get/route @@ -123,7 +121,7 @@ | ✓ | type | name | unit | note | | --- | :------------ | :--- | :--- | :--- | -| | std_msgs/Bool | | | | +| ✓ | std_msgs/Bool | | | | ### /awapi/autoware/put/engage @@ -132,7 +130,7 @@ | ✓ | type | name | unit | note | | --- | :------------ | :--- | :--- | :--- | -| | std_msgs/Bool | | | | +| ✓ | std_msgs/Bool | | | | ### /awapi/autoware/put/route @@ -141,7 +139,7 @@ | ✓ | type | name | unit | note | | --- | :--------------------------- | :--- | :--- | :--- | -| | autoware_planning_msgs/Route | | | | +| ✓ | autoware_planning_msgs/Route | | | | ### /awapi/lane_change/put/approval From 8bf517a92209b71b405549608cb79bdac5512394 Mon Sep 17 00:00:00 2001 From: tkimura4 Date: Thu, 20 Aug 2020 19:12:22 +0900 Subject: [PATCH 11/71] Feature/change output awapi (#782) * change ouput of awapi * change error output to warn Signed-off-by: tanaka3 --- .../src/awapi_autoware_state_publisher.cpp | 14 +++++++------- .../src/awapi_awiv_adapter_core.cpp | 6 +++--- .../src/awapi_lane_change_state_publisher.cpp | 6 +++--- .../awapi_obstacle_avoidance_state_publisher.cpp | 4 ++-- .../src/awapi_vehicle_state_publisher.cpp | 14 +++++++------- 5 files changed, 22 insertions(+), 22 deletions(-) diff --git a/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp b/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp index df09aad032096..35cf84193c772 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp @@ -52,7 +52,7 @@ void AutowareIvAutowareStatePublisher::getAutowareStateInfo( autoware_api_msgs::AwapiAutowareStatus * status) { if (!autoware_state_ptr) { - ROS_WARN_STREAM_THROTTLE(5.0, "[AutowareIvAutowareStatePublisher] autoware_state is nullptr"); + ROS_DEBUG_STREAM_THROTTLE(5.0, "[AutowareIvAutowareStatePublisher] autoware_state is nullptr"); return; } @@ -66,7 +66,7 @@ void AutowareIvAutowareStatePublisher::getControlModeInfo( autoware_api_msgs::AwapiAutowareStatus * status) { if (!control_mode_ptr) { - ROS_WARN_STREAM_THROTTLE(5.0, "[AutowareIvAutowareStatePublisher] control mode is nullptr"); + ROS_DEBUG_STREAM_THROTTLE(5.0, "[AutowareIvAutowareStatePublisher] control mode is nullptr"); return; } @@ -79,7 +79,7 @@ void AutowareIvAutowareStatePublisher::getGateModeInfo( autoware_api_msgs::AwapiAutowareStatus * status) { if (!gate_mode_ptr) { - ROS_WARN_STREAM_THROTTLE(5.0, "[AutowareIvAutowareStatePublisher] gate mode is nullptr"); + ROS_DEBUG_STREAM_THROTTLE(5.0, "[AutowareIvAutowareStatePublisher] gate mode is nullptr"); return; } @@ -92,7 +92,7 @@ void AutowareIvAutowareStatePublisher::getIsEmergencyInfo( autoware_api_msgs::AwapiAutowareStatus * status) { if (!is_emergency_ptr) { - ROS_WARN_STREAM_THROTTLE(5.0, "[AutowareIvAutowareStatePublisher] is_emergency is nullptr"); + ROS_DEBUG_STREAM_THROTTLE(5.0, "[AutowareIvAutowareStatePublisher] is_emergency is nullptr"); return; } @@ -105,7 +105,7 @@ void AutowareIvAutowareStatePublisher::getStopReasonInfo( autoware_api_msgs::AwapiAutowareStatus * status) { if (!stop_reason_ptr) { - ROS_WARN_STREAM_THROTTLE(5.0, "[AutowareIvAutowareStatePublisher] stop reason is nullptr"); + ROS_DEBUG_STREAM_THROTTLE(5.0, "[AutowareIvAutowareStatePublisher] stop reason is nullptr"); return; } @@ -117,7 +117,7 @@ void AutowareIvAutowareStatePublisher::getDiagInfo( autoware_api_msgs::AwapiAutowareStatus * status) { if (!diag_ptr) { - ROS_WARN_STREAM_THROTTLE(5.0, "[AutowareIvAutowareStatePublisher] diagnostics is nullptr"); + ROS_DEBUG_STREAM_THROTTLE(5.0, "[AutowareIvAutowareStatePublisher] diagnostics is nullptr"); return; } @@ -130,7 +130,7 @@ void AutowareIvAutowareStatePublisher::getGlobalRptInfo( autoware_api_msgs::AwapiAutowareStatus * status) { if (!global_rpt_ptr) { - ROS_WARN_STREAM_THROTTLE(5.0, "[AutowareIvAutowareStatePublisher] global_rpt is nullptr"); + ROS_DEBUG_STREAM_THROTTLE(5.0, "[AutowareIvAutowareStatePublisher] global_rpt is nullptr"); return; } diff --git a/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp b/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp index 8b527b0901a45..1e4609ecb9753 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp @@ -77,8 +77,8 @@ AutowareIvAdapter::AutowareIvAdapter() : nh_(), pnh_("~"), tf_listener_(tf_buffe void AutowareIvAdapter::emergencyParamCheck(const bool emergency_handling_param) { if (!emergency_handling_param) { - ROS_ERROR_STREAM("parameter[use_emergency_handling] is false."); - ROS_ERROR_STREAM("autoware/put/emergency is not valid"); + ROS_WARN_STREAM("parameter[use_emergency_handling] is false."); + ROS_WARN_STREAM("autoware/put/emergency is not valid"); } } @@ -144,7 +144,7 @@ void AutowareIvAdapter::getCurrentPose() ps.pose.orientation = transform.transform.rotation; aw_info_.current_pose_ptr = std::make_shared(ps); } catch (tf2::TransformException & ex) { - ROS_WARN_STREAM_THROTTLE(2.0, "[awapi_autoware_adapter] cannot get self pose"); + ROS_DEBUG_STREAM_THROTTLE(2.0, "[awapi_autoware_adapter] cannot get self pose"); } } diff --git a/awapi/awapi_awiv_adapter/src/awapi_lane_change_state_publisher.cpp b/awapi/awapi_awiv_adapter/src/awapi_lane_change_state_publisher.cpp index e69e8cb6050e6..8a3f7f7fd1ca7 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_lane_change_state_publisher.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_lane_change_state_publisher.cpp @@ -45,7 +45,7 @@ void AutowareIvLaneChangeStatePublisher::getLaneChangeAvailableInfo( const std_msgs::Bool::ConstPtr & available_ptr, autoware_api_msgs::LaneChangeStatus * status) { if (!available_ptr) { - ROS_WARN_STREAM_THROTTLE( + ROS_DEBUG_STREAM_THROTTLE( 5.0, "[AutowareIvLaneChangeStatePublisher] lane change available is nullptr"); return; } @@ -58,7 +58,7 @@ void AutowareIvLaneChangeStatePublisher::getLaneChangeReadyInfo( const std_msgs::Bool::ConstPtr & ready_ptr, autoware_api_msgs::LaneChangeStatus * status) { if (!ready_ptr) { - ROS_WARN_STREAM_THROTTLE( + ROS_DEBUG_STREAM_THROTTLE( 5.0, "[AutowareIvLaneChangeStatePublisher] lane change ready is nullptr"); return; } @@ -72,7 +72,7 @@ void AutowareIvLaneChangeStatePublisher::getCandidatePathInfo( autoware_api_msgs::LaneChangeStatus * status) { if (!path_ptr) { - ROS_WARN_STREAM_THROTTLE( + ROS_DEBUG_STREAM_THROTTLE( 5.0, "[AutowareIvLaneChangeStatePublisher] lane_change_candidate_path is " "nullptr"); diff --git a/awapi/awapi_awiv_adapter/src/awapi_obstacle_avoidance_state_publisher.cpp b/awapi/awapi_awiv_adapter/src/awapi_obstacle_avoidance_state_publisher.cpp index a849a8962e92e..4a00761d72fe6 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_obstacle_avoidance_state_publisher.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_obstacle_avoidance_state_publisher.cpp @@ -46,7 +46,7 @@ void AutowareIvObstacleAvoidanceStatePublisher::getObstacleAvoidReadyInfo( const std_msgs::Bool::ConstPtr & ready_ptr, autoware_api_msgs::ObstacleAvoidanceStatus * status) { if (!ready_ptr) { - ROS_WARN_STREAM_THROTTLE( + ROS_DEBUG_STREAM_THROTTLE( 5.0, "[AutowareIvObstacleAvoidanceStatePublisher] obstacle_avoidance_ready is nullptr"); return; } @@ -59,7 +59,7 @@ void AutowareIvObstacleAvoidanceStatePublisher::getCandidatePathInfo( autoware_api_msgs::ObstacleAvoidanceStatus * status) { if (!path_ptr) { - ROS_WARN_STREAM_THROTTLE( + ROS_DEBUG_STREAM_THROTTLE( 5.0, "[AutowareIvObstacleAvoidanceStatePublisher] obstacle_avoidance_candidate_path is " "nullptr"); diff --git a/awapi/awapi_awiv_adapter/src/awapi_vehicle_state_publisher.cpp b/awapi/awapi_awiv_adapter/src/awapi_vehicle_state_publisher.cpp index c1adb24e7a1bf..606f636b38010 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_vehicle_state_publisher.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_vehicle_state_publisher.cpp @@ -51,7 +51,7 @@ void AutowareIvVehicleStatePublisher::getPoseInfo( autoware_api_msgs::AwapiVehicleStatus * status) { if (!pose_ptr) { - ROS_WARN_STREAM_THROTTLE(5.0, "[AutowareIvVehicleStatePublisher] current pose is nullptr"); + ROS_DEBUG_STREAM_THROTTLE(5.0, "[AutowareIvVehicleStatePublisher] current pose is nullptr"); return; } @@ -71,7 +71,7 @@ void AutowareIvVehicleStatePublisher::getSteerInfo( autoware_api_msgs::AwapiVehicleStatus * status) { if (!steer_ptr) { - ROS_WARN_STREAM_THROTTLE(5.0, "[AutowareIvVehicleStatePublisher] steer is nullptr"); + ROS_DEBUG_STREAM_THROTTLE(5.0, "[AutowareIvVehicleStatePublisher] steer is nullptr"); return; } @@ -99,7 +99,7 @@ void AutowareIvVehicleStatePublisher::getVehicleCmdInfo( autoware_api_msgs::AwapiVehicleStatus * status) { if (!vehicle_cmd_ptr) { - ROS_WARN_STREAM_THROTTLE(5.0, "[AutowareIvVehicleStatePublisher] vehicle cmd is nullptr"); + ROS_DEBUG_STREAM_THROTTLE(5.0, "[AutowareIvVehicleStatePublisher] vehicle cmd is nullptr"); return; } @@ -115,7 +115,7 @@ void AutowareIvVehicleStatePublisher::getTurnSignalInfo( autoware_api_msgs::AwapiVehicleStatus * status) { if (!turn_signal_ptr) { - ROS_WARN_STREAM_THROTTLE(5.0, "[AutowareIvVehicleStatePublisher] turn signal is nullptr"); + ROS_DEBUG_STREAM_THROTTLE(5.0, "[AutowareIvVehicleStatePublisher] turn signal is nullptr"); return; } @@ -128,7 +128,7 @@ void AutowareIvVehicleStatePublisher::getTwistInfo( autoware_api_msgs::AwapiVehicleStatus * status) { if (!twist_ptr) { - ROS_WARN_STREAM_THROTTLE(5.0, "[AutowareIvVehicleStatePublisher] twist is nullptr"); + ROS_DEBUG_STREAM_THROTTLE(5.0, "[AutowareIvVehicleStatePublisher] twist is nullptr"); return; } @@ -157,7 +157,7 @@ void AutowareIvVehicleStatePublisher::getGearInfo( autoware_api_msgs::AwapiVehicleStatus * status) { if (!gear_ptr) { - ROS_WARN_STREAM_THROTTLE(5.0, "[AutowareIvVehicleStatePublisher] gear is nullptr"); + ROS_DEBUG_STREAM_THROTTLE(5.0, "[AutowareIvVehicleStatePublisher] gear is nullptr"); return; } @@ -170,7 +170,7 @@ void AutowareIvVehicleStatePublisher::getGpsInfo( autoware_api_msgs::AwapiVehicleStatus * status) { if (!nav_sat_ptr) { - ROS_WARN_STREAM_THROTTLE(5.0, "[AutowareIvVehicleStatePublisher] nav_sat(gps) is nullptr"); + ROS_DEBUG_STREAM_THROTTLE(5.0, "[AutowareIvVehicleStatePublisher] nav_sat(gps) is nullptr"); return; } From 9131964bd5b2340ac401de4eb235c833277ef5e1 Mon Sep 17 00:00:00 2001 From: tkimura4 Date: Wed, 26 Aug 2020 16:34:58 +0900 Subject: [PATCH 12/71] Feature/add battery to awapi (#804) * implement battery fuel * update readme * add getGearInfo * fix message name * set default value:nan to battery Signed-off-by: tanaka3 --- awapi/awapi_awiv_adapter/Readme.md | 55 ++++++++++--------- .../awapi_awiv_adapter/awapi_autoware_util.h | 2 + .../awapi_awiv_adapter_core.h | 2 + .../awapi_vehicle_state_publisher.h | 4 ++ .../launch/awapi_awiv_adapter.launch | 2 + .../src/awapi_awiv_adapter_core.cpp | 6 ++ .../src/awapi_vehicle_state_publisher.cpp | 25 ++++++++- 7 files changed, 68 insertions(+), 28 deletions(-) diff --git a/awapi/awapi_awiv_adapter/Readme.md b/awapi/awapi_awiv_adapter/Readme.md index 85672ea05ca1e..ad534f2ec4a2c 100644 --- a/awapi/awapi_awiv_adapter/Readme.md +++ b/awapi/awapi_awiv_adapter/Readme.md @@ -9,39 +9,40 @@ - get vehicle status - MessageType: awapi_awiv_adapter/AwapiVehicleStatus -| ✓ | type | name | unit | note | -| --- | :------------------------ | :----------------------- | :-------------------------------------------- | :------------------------------------------- | -| ✓ | std_msgs/Header | header | | | -| ✓ | geometry_msgs/Pose | pose | position:[m] | | -| ✓ | awapi_awiv_adapter/Euler | eulerangle | [rad] | roll/pitch/yaw | -| | awapi_awiv_adapter/Latlon | latlon | | lat/lon/alt | -| ✓ | float64 | velocity | [m/s] | | -| ✓ | float64 | acceleration | [m/ss] | calculate from velocity in awapi_adapter | -| ✓ | float64 | steering | [rad] | | -| ✓ | float64 | steering_velocity | [rad/s] | calculate from steering in awapi_adapter | -| ✓ | float64 | angular_velocity | [rad/s] | | -| | int32 | gear | accroding to autoware_vehicle_msgs/Shift | | -| ✓ | int32 | turn_signal | accroding to autoware_vehicle_msgs/TurnSignal | | -| ✓ | float64 | target_velocity | [m/s] | | -| ✓ | float64 | target_acceleration | [m/ss] | | -| ✓ | float64 | target_steering | [rad] | | -| ✓ | float64 | target_steering_velocity | [rad/s] | | +| ✓ | type | name | unit | note | +| --- | :------------------------ | :----------------------- | :-------------------------------------------- | :--------------------------------------- | +| ✓ | std_msgs/Header | header | | | +| ✓ | geometry_msgs/Pose | pose | position:[m] | | +| ✓ | awapi_awiv_adapter/Euler | eulerangle | [rad] | roll/pitch/yaw | +| | awapi_awiv_adapter/Latlon | latlon | | lat/lon/alt | +| ✓ | float64 | velocity | [m/s] | | +| ✓ | float64 | acceleration | [m/ss] | calculate from velocity in awapi_adapter | +| ✓ | float64 | steering | [rad] | | +| ✓ | float64 | steering_velocity | [rad/s] | calculate from steering in awapi_adapter | +| ✓ | float64 | angular_velocity | [rad/s] | | +| | int32 | gear | accroding to autoware_vehicle_msgs/Shift | | +| | float32 | energy_level | | available only for golf-cart | +| ✓ | int32 | turn_signal | accroding to autoware_vehicle_msgs/TurnSignal | | +| ✓ | float64 | target_velocity | [m/s] | | +| ✓ | float64 | target_acceleration | [m/ss] | | +| ✓ | float64 | target_steering | [rad] | | +| ✓ | float64 | target_steering_velocity | [rad/s] | | ### /awapi/autoware/get/status - get autoware status - MessageType: awapi_awiv_adapter/AwapiVehicleStatus -| ✓ | type | name | unit | note | -| --- | :------------------------------------- | :------------------- | :--------------------------------------------- | :-------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -| ✓ | std_msgs/Header | header | | | -| ✓ | string | autoware_state | | | -| ✓ | int32 | control_mode | accroding to autoware_vehicle_msgs/ControlMode | manual/auto (changed by /awapi/autoware/put/engage) | -| | int32 | gate_mode | autoware_vehicle_msgs/GateMode | auto/remote (it is valid only when control_mode=auto)) | -| ✓ | bool | emergency_stopped | True in emergency mode | | -| ✓ | autoware_planning_msgs/StopReasonArray | stop_reason | | "stop_pose" represents the position of "base_link" (not the head of the car) | -| ✓ | diagnostic_msgs/DiagnosticStatus[] | diagnostics | | output only diag. of leaf node (diag. of parent node are cut) | -| ✓ | bool | arrived_goal | | True if the autoware_state is changed from Driving to ArrivedGoal or WairingForRoute. False if the autoware_state is changed to WaitingForEngage or Driving. Default False. | +| ✓ | type | name | unit | note | +| --- | :------------------------------------- | :---------------- | :--------------------------------------------- | :-------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| ✓ | std_msgs/Header | header | | | +| ✓ | string | autoware_state | | | +| ✓ | int32 | control_mode | accroding to autoware_vehicle_msgs/ControlMode | manual/auto (changed by /awapi/autoware/put/engage) | +| | int32 | gate_mode | autoware_vehicle_msgs/GateMode | auto/remote (it is valid only when control_mode=auto)) | +| ✓ | bool | emergency_stopped | True in emergency mode | | +| ✓ | autoware_planning_msgs/StopReasonArray | stop_reason | | "stop_pose" represents the position of "base_link" (not the head of the car) | +| ✓ | diagnostic_msgs/DiagnosticStatus[] | diagnostics | | output only diag. of leaf node (diag. of parent node are cut) | +| ✓ | bool | arrived_goal | | True if the autoware_state is changed from Driving to ArrivedGoal or WairingForRoute. False if the autoware_state is changed to WaitingForEngage or Driving. Default False. | ### /awapi/autoware/get/route diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.h b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.h index a13af3b02c976..bf33ec2bd9655 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.h +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.h @@ -25,6 +25,7 @@ #include #include #include +#include #include #include #include @@ -51,6 +52,7 @@ struct AutowareInfo autoware_vehicle_msgs::TurnSignal::ConstPtr turn_signal_ptr; geometry_msgs::TwistStamped::ConstPtr twist_ptr; autoware_vehicle_msgs::ShiftStamped::ConstPtr gear_ptr; + std_msgs::Float32::ConstPtr battery_ptr; sensor_msgs::NavSatFix::ConstPtr nav_sat_ptr; autoware_system_msgs::AutowareState::ConstPtr autoware_state_ptr; autoware_vehicle_msgs::ControlMode::ConstPtr control_mode_ptr; diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.h b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.h index 87ec85ada960b..d6d26bbb536b9 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.h +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.h @@ -39,6 +39,7 @@ class AutowareIvAdapter ros::Subscriber sub_turn_signal_cmd_; ros::Subscriber sub_twist_; ros::Subscriber sub_gear_; + ros::Subscriber sub_battery_; ros::Subscriber sub_nav_sat_; ros::Subscriber sub_autoware_state_; ros::Subscriber sub_control_mode_; @@ -65,6 +66,7 @@ class AutowareIvAdapter void callbackTurnSignal(const autoware_vehicle_msgs::TurnSignal::ConstPtr & msg_ptr); void callbackTwist(const geometry_msgs::TwistStamped::ConstPtr & msg_ptr); void callbackGear(const autoware_vehicle_msgs::ShiftStamped::ConstPtr & msg_ptr); + void callbackBattery(const std_msgs::Float32::ConstPtr & msg_ptr); void callbackNavSat(const sensor_msgs::NavSatFix::ConstPtr & msg_ptr); void callbackAutowareState(const autoware_system_msgs::AutowareState::ConstPtr & msg_ptr); void callbackControlMode(const autoware_vehicle_msgs::ControlMode::ConstPtr & msg_ptr); diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_vehicle_state_publisher.h b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_vehicle_state_publisher.h index 883c4d4b555f4..e30f39ffcf604 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_vehicle_state_publisher.h +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_vehicle_state_publisher.h @@ -33,6 +33,7 @@ class AutowareIvVehicleStatePublisher // publisher ros::Publisher pub_state_; + autoware_api_msgs::AwapiVehicleStatus initVehicleStatus(); void getPoseInfo( const std::shared_ptr & pose_ptr, autoware_api_msgs::AwapiVehicleStatus * status); @@ -51,6 +52,9 @@ class AutowareIvVehicleStatePublisher void getGearInfo( const autoware_vehicle_msgs::ShiftStamped::ConstPtr & gear_ptr, autoware_api_msgs::AwapiVehicleStatus * status); + void getBatteryInfo( + const std_msgs::Float32::ConstPtr & battery_ptr, + autoware_api_msgs::AwapiVehicleStatus * status); void getGpsInfo( const sensor_msgs::NavSatFix::ConstPtr & nav_sat_ptr, autoware_api_msgs::AwapiVehicleStatus * status); diff --git a/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch b/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch index 4dca2380123f2..47c9be49b8557 100644 --- a/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch +++ b/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch @@ -9,6 +9,7 @@ + @@ -66,6 +67,7 @@ + diff --git a/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp b/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp index 1e4609ecb9753..1ef8801a99449 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp @@ -42,6 +42,7 @@ AutowareIvAdapter::AutowareIvAdapter() : nh_(), pnh_("~"), tf_listener_(tf_buffe pnh_.subscribe("input/turn_signal", 1, &AutowareIvAdapter::callbackTurnSignal, this); sub_twist_ = pnh_.subscribe("input/twist", 1, &AutowareIvAdapter::callbackTwist, this); sub_gear_ = pnh_.subscribe("input/gear", 1, &AutowareIvAdapter::callbackGear, this); + sub_battery_ = pnh_.subscribe("input/battery", 1, &AutowareIvAdapter::callbackBattery, this); sub_nav_sat_ = pnh_.subscribe("input/nav_sat", 1, &AutowareIvAdapter::callbackNavSat, this); sub_autoware_state_ = pnh_.subscribe("input/autoware_state", 1, &AutowareIvAdapter::callbackAutowareState, this); @@ -127,6 +128,11 @@ void AutowareIvAdapter::callbackGear(const autoware_vehicle_msgs::ShiftStamped:: aw_info_.gear_ptr = msg_ptr; } +void AutowareIvAdapter::callbackBattery(const std_msgs::Float32::ConstPtr & msg_ptr) +{ + aw_info_.battery_ptr = msg_ptr; +} + void AutowareIvAdapter::callbackNavSat(const sensor_msgs::NavSatFix::ConstPtr & msg_ptr) { aw_info_.nav_sat_ptr = msg_ptr; diff --git a/awapi/awapi_awiv_adapter/src/awapi_vehicle_state_publisher.cpp b/awapi/awapi_awiv_adapter/src/awapi_vehicle_state_publisher.cpp index 606f636b38010..0d6fc747d60b0 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_vehicle_state_publisher.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_vehicle_state_publisher.cpp @@ -27,7 +27,7 @@ AutowareIvVehicleStatePublisher::AutowareIvVehicleStatePublisher() void AutowareIvVehicleStatePublisher::statePublisher(const AutowareInfo & aw_info) { - autoware_api_msgs::AwapiVehicleStatus status; + autoware_api_msgs::AwapiVehicleStatus status = initVehicleStatus(); //input header status.header.frame_id = "base_link"; @@ -40,12 +40,23 @@ void AutowareIvVehicleStatePublisher::statePublisher(const AutowareInfo & aw_inf getTurnSignalInfo(aw_info.turn_signal_ptr, &status); getTwistInfo(aw_info.twist_ptr, &status); getGearInfo(aw_info.gear_ptr, &status); + getBatteryInfo(aw_info.battery_ptr, &status); getGpsInfo(aw_info.nav_sat_ptr, &status); // publish info pub_state_.publish(status); } +autoware_api_msgs::AwapiVehicleStatus AutowareIvVehicleStatePublisher::initVehicleStatus() +{ + autoware_api_msgs::AwapiVehicleStatus status; + // set default value + if (std::numeric_limits::has_quiet_NaN) { + status.energy_level = std::numeric_limits::quiet_NaN(); + } + return status; +} + void AutowareIvVehicleStatePublisher::getPoseInfo( const std::shared_ptr & pose_ptr, autoware_api_msgs::AwapiVehicleStatus * status) @@ -165,6 +176,18 @@ void AutowareIvVehicleStatePublisher::getGearInfo( status->gear = gear_ptr->shift.data; } +void AutowareIvVehicleStatePublisher::getBatteryInfo( + const std_msgs::Float32::ConstPtr & battery_ptr, autoware_api_msgs::AwapiVehicleStatus * status) +{ + if (!battery_ptr) { + ROS_DEBUG_STREAM_THROTTLE(5.0, "[AutowareIvVehicleStatePublisher] battery is nullptr"); + return; + } + + // get battery + status->energy_level = battery_ptr->data; +} + void AutowareIvVehicleStatePublisher::getGpsInfo( const sensor_msgs::NavSatFix::ConstPtr & nav_sat_ptr, autoware_api_msgs::AwapiVehicleStatus * status) From 2d4b7643835f2c5a3d74883e121debf0886d8b62 Mon Sep 17 00:00:00 2001 From: mitsudome-r Date: Tue, 29 Sep 2020 19:22:18 +0900 Subject: [PATCH 13/71] remove ROS1 packages temporarily Signed-off-by: mitsudome-r Signed-off-by: tanaka3 --- awapi/awapi_awiv_adapter/CMakeLists.txt | 70 ------ awapi/awapi_awiv_adapter/Readme.md | 190 --------------- .../awapi_autoware_state_publisher.h | 76 ------ .../awapi_awiv_adapter/awapi_autoware_util.h | 102 -------- .../awapi_awiv_adapter_core.h | 102 -------- .../awapi_lane_change_state_publisher.h | 45 ---- ...awapi_obstacle_avoidance_state_publisher.h | 44 ---- .../awapi_stop_reason_aggregator.h | 45 ---- .../awapi_vehicle_state_publisher.h | 73 ------ .../launch/awapi_awiv_adapter.launch | 111 --------- awapi/awapi_awiv_adapter/package.xml | 25 -- .../src/awapi_autoware_state_publisher.cpp | 208 ---------------- .../src/awapi_autoware_util.cpp | 26 -- .../src/awapi_awiv_adapter_core.cpp | 222 ------------------ .../src/awapi_awiv_adapter_node.cpp | 30 --- .../src/awapi_lane_change_state_publisher.cpp | 85 ------- ...api_obstacle_avoidance_state_publisher.cpp | 72 ------ .../src/awapi_stop_reason_aggregator.cpp | 134 ----------- .../src/awapi_vehicle_state_publisher.cpp | 206 ---------------- 19 files changed, 1866 deletions(-) delete mode 100644 awapi/awapi_awiv_adapter/CMakeLists.txt delete mode 100644 awapi/awapi_awiv_adapter/Readme.md delete mode 100644 awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.h delete mode 100644 awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.h delete mode 100644 awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.h delete mode 100644 awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_lane_change_state_publisher.h delete mode 100644 awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_obstacle_avoidance_state_publisher.h delete mode 100644 awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_stop_reason_aggregator.h delete mode 100644 awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_vehicle_state_publisher.h delete mode 100644 awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch delete mode 100644 awapi/awapi_awiv_adapter/package.xml delete mode 100644 awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp delete mode 100644 awapi/awapi_awiv_adapter/src/awapi_autoware_util.cpp delete mode 100644 awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp delete mode 100644 awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_node.cpp delete mode 100644 awapi/awapi_awiv_adapter/src/awapi_lane_change_state_publisher.cpp delete mode 100644 awapi/awapi_awiv_adapter/src/awapi_obstacle_avoidance_state_publisher.cpp delete mode 100644 awapi/awapi_awiv_adapter/src/awapi_stop_reason_aggregator.cpp delete mode 100644 awapi/awapi_awiv_adapter/src/awapi_vehicle_state_publisher.cpp diff --git a/awapi/awapi_awiv_adapter/CMakeLists.txt b/awapi/awapi_awiv_adapter/CMakeLists.txt deleted file mode 100644 index e5f36e0a12a31..0000000000000 --- a/awapi/awapi_awiv_adapter/CMakeLists.txt +++ /dev/null @@ -1,70 +0,0 @@ -cmake_minimum_required(VERSION 3.0.2) -project(awapi_awiv_adapter) - -add_compile_options(-std=c++14) - -find_package(catkin REQUIRED COMPONENTS - message_generation - message_runtime - roscpp - autoware_api_msgs - autoware_system_msgs - autoware_planning_msgs - autoware_control_msgs - autoware_vehicle_msgs - diagnostic_msgs - geometry_msgs - pacmod_msgs - std_msgs - sensor_msgs - tf2 - tf2_geometry_msgs -) - -catkin_package( - INCLUDE_DIRS include -) - -include_directories( - include - ${catkin_INCLUDE_DIRS} -) - -add_executable(awapi_awiv_adapter - src/awapi_awiv_adapter_node.cpp - src/awapi_awiv_adapter_core.cpp - src/awapi_vehicle_state_publisher.cpp - src/awapi_autoware_state_publisher.cpp - src/awapi_stop_reason_aggregator.cpp - src/awapi_lane_change_state_publisher.cpp - src/awapi_obstacle_avoidance_state_publisher.cpp - src/awapi_autoware_util.cpp -) - -target_link_libraries(awapi_awiv_adapter - ${catkin_LIBRARIES} -) - -add_dependencies(awapi_awiv_adapter - ${catkin_EXPORTED_TARGETS} -) - -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) - -install( - DIRECTORY - launch - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install(TARGETS awapi_awiv_adapter - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -if(CATKIN_ENABLE_TESTING) - find_package(rostest REQUIRED) -endif() diff --git a/awapi/awapi_awiv_adapter/Readme.md b/awapi/awapi_awiv_adapter/Readme.md deleted file mode 100644 index ad534f2ec4a2c..0000000000000 --- a/awapi/awapi_awiv_adapter/Readme.md +++ /dev/null @@ -1,190 +0,0 @@ -# AWAPI_AWIV_ADAPTER - -✓: confirmed, (blank): not confirmed - -## get topic - -### /awapi/vehicle/get/status - -- get vehicle status -- MessageType: awapi_awiv_adapter/AwapiVehicleStatus - -| ✓ | type | name | unit | note | -| --- | :------------------------ | :----------------------- | :-------------------------------------------- | :--------------------------------------- | -| ✓ | std_msgs/Header | header | | | -| ✓ | geometry_msgs/Pose | pose | position:[m] | | -| ✓ | awapi_awiv_adapter/Euler | eulerangle | [rad] | roll/pitch/yaw | -| | awapi_awiv_adapter/Latlon | latlon | | lat/lon/alt | -| ✓ | float64 | velocity | [m/s] | | -| ✓ | float64 | acceleration | [m/ss] | calculate from velocity in awapi_adapter | -| ✓ | float64 | steering | [rad] | | -| ✓ | float64 | steering_velocity | [rad/s] | calculate from steering in awapi_adapter | -| ✓ | float64 | angular_velocity | [rad/s] | | -| | int32 | gear | accroding to autoware_vehicle_msgs/Shift | | -| | float32 | energy_level | | available only for golf-cart | -| ✓ | int32 | turn_signal | accroding to autoware_vehicle_msgs/TurnSignal | | -| ✓ | float64 | target_velocity | [m/s] | | -| ✓ | float64 | target_acceleration | [m/ss] | | -| ✓ | float64 | target_steering | [rad] | | -| ✓ | float64 | target_steering_velocity | [rad/s] | | - -### /awapi/autoware/get/status - -- get autoware status -- MessageType: awapi_awiv_adapter/AwapiVehicleStatus - -| ✓ | type | name | unit | note | -| --- | :------------------------------------- | :---------------- | :--------------------------------------------- | :-------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -| ✓ | std_msgs/Header | header | | | -| ✓ | string | autoware_state | | | -| ✓ | int32 | control_mode | accroding to autoware_vehicle_msgs/ControlMode | manual/auto (changed by /awapi/autoware/put/engage) | -| | int32 | gate_mode | autoware_vehicle_msgs/GateMode | auto/remote (it is valid only when control_mode=auto)) | -| ✓ | bool | emergency_stopped | True in emergency mode | | -| ✓ | autoware_planning_msgs/StopReasonArray | stop_reason | | "stop_pose" represents the position of "base_link" (not the head of the car) | -| ✓ | diagnostic_msgs/DiagnosticStatus[] | diagnostics | | output only diag. of leaf node (diag. of parent node are cut) | -| ✓ | bool | arrived_goal | | True if the autoware_state is changed from Driving to ArrivedGoal or WairingForRoute. False if the autoware_state is changed to WaitingForEngage or Driving. Default False. | - -### /awapi/autoware/get/route - -- get route -- MessageType: autoware_planning_msgs/Route - -| ✓ | type | name | unit | note | -| --- | :--------------------------- | :--- | :--- | :--- | -| ✓ | autoware_planning_msgs/Route | | | | - -### /awapi/prediction/get/objects - -- get predicted object -- MessageType: autoware_perception_msgs/DynamicObjectArray - -| ✓ | type | name | unit | note | -| --- | :------------------------------------------ | :--- | :--- | :--- | -| ✓ | autoware_perception_msgs/DynamicObjectArray | | | | - -### /awapi/lane_change/get/status - -- get lane change information -- MessageType: awapi_awiv_adapter/LaneChangeStatus - -| ✓ | type | name | unit | note | -| --- | :-------------------------- | :-------------------------- | :--------------------------------------- | :--------------------------------------------------------------------------------- | -| | std_msgs/Header | header | | | -| | bool | force_lane_change_available | True when lane change is avilable | available: Physically lane changeable state (do not consider other vehicle) | -| | bool | lane_change_ready | True when lane change is ready | ready: State that ego-vehicle can change lane without collision with other vehicle | -| | autoware_planning_msgs/Path | candidate_path | according to autoware_planning_msgs/Path | | - -### /awapi/object_avoidance/get/status - -- get obstacle avoidance information -- MessageType: awapi_awiv_adapter/ObstacleAvoidanceStatus - -| ✓ | type | name | unit | note | -| --- | :-------------------------------- | :----------------------- | :--------------------------------------------- | :---------------------------------------------------- | -| | std_msgs/Header | header | | | -| | bool | obstacle_avoidance_ready | True when obstacle avoidance is ready | | -| | autoware_planning_msgs/Trajectory | candidate_path | according to autoware_planning_msgs/Trajectory | Msg type is different from lane change candidate path | - -### /awapi/traffic_light/get/status - -- get recognition result of traffic light -- MessageType: autoware_perception_msgs/TrafficLightStateArray - -| ✓ | type | name | unit | note | -| --- | :---------------------------------------------- | :--- | :--- | :--- | -| | autoware_perception_msgs/TrafficLightStateArray | | | | - -## put topic - -### /awapi/vehicle/put/velocity - -- set upper velocity -- MessageType: std_msgs/Float32 - -| ✓ | type | name | unit | note | -| --- | :--------------- | :--- | :--- | :----------- | -| ✓ | std_msgs/Float32 | | | max velocity | - -### /awapi/autoware/put/gate_mode - -- send gate mode (auto/remote) -- MessageType: autoware_control_msgs/GateMode - -| ✓ | type | name | unit | note | -| --- | :----------------------------- | :--- | :--- | :--- | -| | autoware_control_msgs/GateMode | | | | - -### /awapi/autoware/put/emergency - -- send emergency signal -- MessageType: std_msgs/Bool -- **To enable this functionality, autoware have to be in the Remote Mode or set _/control/vehicle_cmd_gate/use_emergency_handling_ to true.** - -| ✓ | type | name | unit | note | -| --- | :------------ | :--- | :--- | :--- | -| ✓ | std_msgs/Bool | | | | - -### /awapi/autoware/put/engage - -- send engage signal (both of autoware/engage and vehicle/engage) -- MessageType: std_msgs/Bool - -| ✓ | type | name | unit | note | -| --- | :------------ | :--- | :--- | :--- | -| ✓ | std_msgs/Bool | | | | - -### /awapi/autoware/put/route - -- send goal pose -- MessageType: autoware_planning_msgs/Route - -| ✓ | type | name | unit | note | -| --- | :--------------------------- | :--- | :--- | :--- | -| ✓ | autoware_planning_msgs/Route | | | | - -### /awapi/lane_change/put/approval - -- send lane change approval flag -- send True: start lane change when **lane_change_ready** is true -- MessageType: std_msgs/Bool - -| ✓ | type | name | unit | note | -| --- | :------------ | :--- | :--- | :--- | -| | std_msgs/Bool | | | | - -### /awapi/lane_change/put/force - -- send force lane change flag -- send True: start lane change when **force_lane_change_available** is true -- MessageType: std_msgs/Bool - -| ✓ | type | name | unit | note | -| --- | :------------ | :--- | :--- | :--- | -| | std_msgs/Bool | | | | - -### /awapi/object_avoidance/put/approval - -- send object avoidance approval flag -- MessageType: std_msgs/Bool - -| ✓ | type | name | unit | note | -| --- | :------------ | :--- | :--- | :--- | -| | std_msgs/Bool | | | | - -### /awapi/object_avoidance/put/force - -- send force object avoidance flag -- **not implemented (Autoware does not have corresponded topic)** - -| ✓ | type | name | unit | note | -| --- | :--- | :--- | :--- | :--- | - - -### /awapi/traffic_light/put/traffic_light - -- Overwrite the recognition result of traffic light -- **not implemented (Autoware does not have corresponded topic)** - -| ✓ | type | name | unit | note | -| --- | :--- | :--- | :--- | :--- | - diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.h b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.h deleted file mode 100644 index 32a95428a4a35..0000000000000 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.h +++ /dev/null @@ -1,76 +0,0 @@ -/* - * Copyright 2020 Tier IV, Inc. All rights reserved. - * - * 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. - */ - -#include -#include - -namespace autoware_api -{ -class AutowareIvAutowareStatePublisher -{ -public: - AutowareIvAutowareStatePublisher(); - void statePublisher(const AutowareInfo & aw_info); - -private: - // node handle - ros::NodeHandle nh_; - ros::NodeHandle pnh_; - - // publisher - ros::Publisher pub_state_; - - // parameter - - /* parameter for judging goal now */ - bool arrived_goal_; - autoware_system_msgs::AutowareState::_state_type prev_state_; - - /* parameter for judging diag leaf */ - std::set diag_name_set_; - - void getAutowareStateInfo( - const autoware_system_msgs::AutowareState::ConstPtr & autoware_state_ptr, - autoware_api_msgs::AwapiAutowareStatus * status); - void getControlModeInfo( - const autoware_vehicle_msgs::ControlMode::ConstPtr & control_mode_ptr, - autoware_api_msgs::AwapiAutowareStatus * status); - void getGateModeInfo( - const autoware_control_msgs::GateMode::ConstPtr & gate_mode_ptr, - autoware_api_msgs::AwapiAutowareStatus * status); - void getIsEmergencyInfo( - const std_msgs::Bool::ConstPtr & is_emergency_ptr, - autoware_api_msgs::AwapiAutowareStatus * status); - void getStopReasonInfo( - const autoware_planning_msgs::StopReasonArray::ConstPtr & stop_reason_ptr, - autoware_api_msgs::AwapiAutowareStatus * status); - void getDiagInfo( - const diagnostic_msgs::DiagnosticArray::ConstPtr & diag_ptr, - autoware_api_msgs::AwapiAutowareStatus * status); - - void getGlobalRptInfo( - const pacmod_msgs::GlobalRpt::ConstPtr & global_rpt_ptr, - autoware_api_msgs::AwapiAutowareStatus * status); - - bool isGoal(const autoware_system_msgs::AutowareState::ConstPtr & autoware_state); - std::vector extractLeafDiag( - const std::vector & diag_vec); - std::string splitStringByLastSlash(const std::string & str); - void updateDiagNameSet(const std::vector & diag_vec); - bool isLeaf(const diagnostic_msgs::DiagnosticStatus & diag); -}; - -} // namespace autoware_api diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.h b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.h deleted file mode 100644 index bf33ec2bd9655..0000000000000 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.h +++ /dev/null @@ -1,102 +0,0 @@ -/* - * Copyright 2020 Tier IV, Inc. All rights reserved. - * - * 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. - */ - -#pragma once - -#ifndef AWAPI_AUTOWARE_UTIL_H -#define AWAPI_AUTOWARE_UTIL_H - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace autoware_api -{ -struct AutowareInfo -{ - std::shared_ptr current_pose_ptr; - autoware_vehicle_msgs::Steering::ConstPtr steer_ptr; - autoware_vehicle_msgs::VehicleCommand::ConstPtr vehicle_cmd_ptr; - autoware_vehicle_msgs::TurnSignal::ConstPtr turn_signal_ptr; - geometry_msgs::TwistStamped::ConstPtr twist_ptr; - autoware_vehicle_msgs::ShiftStamped::ConstPtr gear_ptr; - std_msgs::Float32::ConstPtr battery_ptr; - sensor_msgs::NavSatFix::ConstPtr nav_sat_ptr; - autoware_system_msgs::AutowareState::ConstPtr autoware_state_ptr; - autoware_vehicle_msgs::ControlMode::ConstPtr control_mode_ptr; - autoware_control_msgs::GateMode::ConstPtr gate_mode_ptr; - std_msgs::Bool::ConstPtr is_emergency_ptr; - autoware_planning_msgs::StopReasonArray::ConstPtr stop_reason_ptr; - diagnostic_msgs::DiagnosticArray::ConstPtr diagnostic_ptr; - pacmod_msgs::GlobalRpt::ConstPtr global_rpt_ptr; - std_msgs::Bool::ConstPtr lane_change_available_ptr; - std_msgs::Bool::ConstPtr lane_change_ready_ptr; - autoware_planning_msgs::Path::ConstPtr lane_change_candidate_ptr; - std_msgs::Bool::ConstPtr obstacle_avoid_ready_ptr; - autoware_planning_msgs::Trajectory::ConstPtr obstacle_avoid_candidate_ptr; -}; - -template -T getParam(const ros::NodeHandle & nh, const std::string & key, const T & default_value) -{ - T value; - nh.param(key, value, default_value); - return value; -} - -template -T waitForParam(const ros::NodeHandle & nh, const std::string & key) -{ - T value; - ros::Rate rate(1.0); - - while (ros::ok()) { - const auto result = nh.getParam(key, value); - if (result) { - return value; - } - - ROS_WARN("waiting for parameter `%s` ...", key.c_str()); - rate.sleep(); - } - - return {}; -} - -double lowpass_filter(const double current_value, const double prev_value, const double gain); - -} // namespace autoware_api - -#endif // AWAPI_AUTOWARE_UTIL_H diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.h b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.h deleted file mode 100644 index d6d26bbb536b9..0000000000000 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.h +++ /dev/null @@ -1,102 +0,0 @@ -/* - * Copyright 2020 Tier IV, Inc. All rights reserved. - * - * 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. - */ - -#include -#include -#include -#include -#include -#include - -namespace autoware_api -{ -class AutowareIvAdapter -{ -public: - AutowareIvAdapter(); - -private: - // node handle - ros::NodeHandle nh_; - ros::NodeHandle pnh_; - - // subscriber - ros::Subscriber sub_steer_; - ros::Subscriber sub_vehicle_cmd_; - ros::Subscriber sub_turn_signal_cmd_; - ros::Subscriber sub_twist_; - ros::Subscriber sub_gear_; - ros::Subscriber sub_battery_; - ros::Subscriber sub_nav_sat_; - ros::Subscriber sub_autoware_state_; - ros::Subscriber sub_control_mode_; - ros::Subscriber sub_gate_mode_; - ros::Subscriber sub_emergency_; - ros::Subscriber sub_stop_reason_; - ros::Subscriber sub_diagnostics_; - ros::Subscriber sub_global_rpt_; - ros::Subscriber sub_lane_change_available_; - ros::Subscriber sub_lane_change_ready_; - ros::Subscriber sub_lane_change_candidate_; - ros::Subscriber sub_obstacle_avoid_ready_; - ros::Subscriber sub_obstacle_avoid_candidate_; - // timer - ros::Timer timer_; - - // tf - tf2_ros::Buffer tf_buffer_; - tf2_ros::TransformListener tf_listener_; - - // callback function - void callbackSteer(const autoware_vehicle_msgs::Steering::ConstPtr & msg_ptr); - void callbackVehicleCmd(const autoware_vehicle_msgs::VehicleCommand::ConstPtr & msg_ptr); - void callbackTurnSignal(const autoware_vehicle_msgs::TurnSignal::ConstPtr & msg_ptr); - void callbackTwist(const geometry_msgs::TwistStamped::ConstPtr & msg_ptr); - void callbackGear(const autoware_vehicle_msgs::ShiftStamped::ConstPtr & msg_ptr); - void callbackBattery(const std_msgs::Float32::ConstPtr & msg_ptr); - void callbackNavSat(const sensor_msgs::NavSatFix::ConstPtr & msg_ptr); - void callbackAutowareState(const autoware_system_msgs::AutowareState::ConstPtr & msg_ptr); - void callbackControlMode(const autoware_vehicle_msgs::ControlMode::ConstPtr & msg_ptr); - void callbackGateMode(const autoware_control_msgs::GateMode::ConstPtr & msg_ptr); - void callbackIsEmergency(const std_msgs::Bool::ConstPtr & msg_ptr); - void callbackStopReason(const autoware_planning_msgs::StopReasonArray::ConstPtr & msg_ptr); - void callbackDiagnostics(const diagnostic_msgs::DiagnosticArray::ConstPtr & msg_ptr); - void callbackGlobalRpt(const pacmod_msgs::GlobalRpt::ConstPtr & msg_ptr); - void callbackLaneChangeAvailable(const std_msgs::Bool::ConstPtr & msg_ptr); - void callbackLaneChangeReady(const std_msgs::Bool::ConstPtr & msg_ptr); - void callbackLaneChangeCandidatePath(const autoware_planning_msgs::Path::ConstPtr & msg_ptr); - void callbackLaneObstacleAvoidReady(const std_msgs::Bool::ConstPtr & msg_ptr); - void callbackLaneObstacleAvoidCandidatePath( - const autoware_planning_msgs::Trajectory::ConstPtr & msg_ptr); - - // timer function - void timerCallback(const ros::TimerEvent & e); - - void emergencyParamCheck(const bool emergency_handling_param); - void getCurrentPose(); - - // parameter - AutowareInfo aw_info_; - std::unique_ptr vehicle_state_publisher_; - std::unique_ptr autoware_state_publisher_; - std::unique_ptr stop_reason_aggreagator_; - std::unique_ptr lane_change_state_publisher_; - std::unique_ptr obstacle_avoidance_state_publisher_; - double status_pub_hz_; - double stop_reason_timeout_; -}; - -} // namespace autoware_api diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_lane_change_state_publisher.h b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_lane_change_state_publisher.h deleted file mode 100644 index 56c30f4aa30cd..0000000000000 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_lane_change_state_publisher.h +++ /dev/null @@ -1,45 +0,0 @@ -/* - * Copyright 2020 Tier IV, Inc. All rights reserved. - * - * 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. - */ - -#include -#include - -namespace autoware_api -{ -class AutowareIvLaneChangeStatePublisher -{ -public: - AutowareIvLaneChangeStatePublisher(); - void statePublisher(const AutowareInfo & aw_info); - -private: - // node handle - ros::NodeHandle nh_; - ros::NodeHandle pnh_; - - // publisher - ros::Publisher pub_state_; - - void getLaneChangeAvailableInfo( - const std_msgs::Bool::ConstPtr & available_ptr, autoware_api_msgs::LaneChangeStatus * status); - void getLaneChangeReadyInfo( - const std_msgs::Bool::ConstPtr & ready_ptr, autoware_api_msgs::LaneChangeStatus * status); - void getCandidatePathInfo( - const autoware_planning_msgs::Path::ConstPtr & path_ptr, - autoware_api_msgs::LaneChangeStatus * status); -}; - -} // namespace autoware_api diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_obstacle_avoidance_state_publisher.h b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_obstacle_avoidance_state_publisher.h deleted file mode 100644 index f82ddb16dff07..0000000000000 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_obstacle_avoidance_state_publisher.h +++ /dev/null @@ -1,44 +0,0 @@ -/* - * Copyright 2020 Tier IV, Inc. All rights reserved. - * - * 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. - */ - -#include -#include - -namespace autoware_api -{ -class AutowareIvObstacleAvoidanceStatePublisher -{ -public: - AutowareIvObstacleAvoidanceStatePublisher(); - void statePublisher(const AutowareInfo & aw_info); - -private: - // node handle - ros::NodeHandle nh_; - ros::NodeHandle pnh_; - - // publisher - ros::Publisher pub_state_; - - void getObstacleAvoidReadyInfo( - const std_msgs::Bool::ConstPtr & ready_ptr, - autoware_api_msgs::ObstacleAvoidanceStatus * status); - void getCandidatePathInfo( - const autoware_planning_msgs::Trajectory::ConstPtr & path_ptr, - autoware_api_msgs::ObstacleAvoidanceStatus * status); -}; - -} // namespace autoware_api diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_stop_reason_aggregator.h b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_stop_reason_aggregator.h deleted file mode 100644 index 32e1f4abfcebf..0000000000000 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_stop_reason_aggregator.h +++ /dev/null @@ -1,45 +0,0 @@ -/* - * Copyright 2020 Tier IV, Inc. All rights reserved. - * - * 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. - */ - -#pragma once - -#include - -namespace autoware_api -{ -class AutowareIvStopReasonAggregator -{ -public: - AutowareIvStopReasonAggregator(const double timeout); - autoware_planning_msgs::StopReasonArray::ConstPtr updateStopReasonArray( - const autoware_planning_msgs::StopReasonArray::ConstPtr & msg_ptr); - -private: - void applyUpdate(const autoware_planning_msgs::StopReasonArray::ConstPtr & msg_ptr); - bool checkMatchingReason( - const autoware_planning_msgs::StopReasonArray::ConstPtr & msg_stop_reason_array, - const autoware_planning_msgs::StopReasonArray & stop_reason_array); - void applyTimeOut(); - void appendStopReasonToArray( - const autoware_planning_msgs::StopReason & stop_reason, - autoware_planning_msgs::StopReasonArray * stop_reason_array); - autoware_planning_msgs::StopReasonArray::ConstPtr makeStopReasonArray(); - - double timeout_; - std::vector stop_reason_array_vec_; -}; - -} // namespace autoware_api diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_vehicle_state_publisher.h b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_vehicle_state_publisher.h deleted file mode 100644 index e30f39ffcf604..0000000000000 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_vehicle_state_publisher.h +++ /dev/null @@ -1,73 +0,0 @@ -/* - * Copyright 2020 Tier IV, Inc. All rights reserved. - * - * 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. - */ - -#include -#include - -namespace autoware_api -{ -class AutowareIvVehicleStatePublisher -{ -public: - AutowareIvVehicleStatePublisher(); - void statePublisher(const AutowareInfo & aw_info); - -private: - // node handle - ros::NodeHandle nh_; - ros::NodeHandle pnh_; - - // publisher - ros::Publisher pub_state_; - - autoware_api_msgs::AwapiVehicleStatus initVehicleStatus(); - void getPoseInfo( - const std::shared_ptr & pose_ptr, - autoware_api_msgs::AwapiVehicleStatus * status); - void getSteerInfo( - const autoware_vehicle_msgs::Steering::ConstPtr & steer_ptr, - autoware_api_msgs::AwapiVehicleStatus * status); - void getVehicleCmdInfo( - const autoware_vehicle_msgs::VehicleCommand::ConstPtr & vehicle_cmd_ptr, - autoware_api_msgs::AwapiVehicleStatus * status); - void getTurnSignalInfo( - const autoware_vehicle_msgs::TurnSignal::ConstPtr & turn_signal_ptr, - autoware_api_msgs::AwapiVehicleStatus * status); - void getTwistInfo( - const geometry_msgs::TwistStamped::ConstPtr & twist_ptr, - autoware_api_msgs::AwapiVehicleStatus * status); - void getGearInfo( - const autoware_vehicle_msgs::ShiftStamped::ConstPtr & gear_ptr, - autoware_api_msgs::AwapiVehicleStatus * status); - void getBatteryInfo( - const std_msgs::Float32::ConstPtr & battery_ptr, - autoware_api_msgs::AwapiVehicleStatus * status); - void getGpsInfo( - const sensor_msgs::NavSatFix::ConstPtr & nav_sat_ptr, - autoware_api_msgs::AwapiVehicleStatus * status); - - //parameters - geometry_msgs::TwistStamped::ConstPtr previous_twist_ptr_; - autoware_vehicle_msgs::Steering::ConstPtr previous_steer_ptr_; - double prev_accel_; - double prev_steer_vel_; - - // defined value - const double accel_lowpass_gain_ = 0.2; - const double steer_vel_lowpass_gain_ = 0.2; -}; - -} // namespace autoware_api diff --git a/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch b/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch deleted file mode 100644 index 47c9be49b8557..0000000000000 --- a/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch +++ /dev/null @@ -1,111 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/awapi/awapi_awiv_adapter/package.xml b/awapi/awapi_awiv_adapter/package.xml deleted file mode 100644 index 6909569c2b9b5..0000000000000 --- a/awapi/awapi_awiv_adapter/package.xml +++ /dev/null @@ -1,25 +0,0 @@ - - - awapi_awiv_adapter - 0.1.0 - The awapi_awiv_adapter package - Tomoya Kimura - Tomoya Kimura - BSD - - catkin - - roscpp - autoware_api_msgs - autoware_system_msgs - autoware_planning_msgs - autoware_control_msgs - autoware_vehicle_msgs - diagnostic_msgs - geometry_msgs - pacmod_msgs - std_msgs - sensor_msgs - tf2 - tf2_geometry_msgs - diff --git a/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp b/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp deleted file mode 100644 index 35cf84193c772..0000000000000 --- a/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp +++ /dev/null @@ -1,208 +0,0 @@ -/* - * Copyright 2020 Tier IV, Inc. All rights reserved. - * - * 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. - */ - -#include -#include - -namespace autoware_api -{ -AutowareIvAutowareStatePublisher::AutowareIvAutowareStatePublisher() -: nh_(), pnh_("~"), arrived_goal_(false) -{ - // publisher - pub_state_ = pnh_.advertise("output/autoware_status", 1); -} - -void AutowareIvAutowareStatePublisher::statePublisher(const AutowareInfo & aw_info) -{ - autoware_api_msgs::AwapiAutowareStatus status; - - //input header - status.header.frame_id = "base_link"; - status.header.stamp = ros::Time::now(); - - // get all info - getAutowareStateInfo(aw_info.autoware_state_ptr, &status); - getControlModeInfo(aw_info.control_mode_ptr, &status); - getGateModeInfo(aw_info.gate_mode_ptr, &status); - getIsEmergencyInfo(aw_info.is_emergency_ptr, &status); - getStopReasonInfo(aw_info.stop_reason_ptr, &status); - getDiagInfo(aw_info.diagnostic_ptr, &status); - getGlobalRptInfo(aw_info.global_rpt_ptr, &status); - - // publish info - pub_state_.publish(status); -} - -void AutowareIvAutowareStatePublisher::getAutowareStateInfo( - const autoware_system_msgs::AutowareState::ConstPtr & autoware_state_ptr, - autoware_api_msgs::AwapiAutowareStatus * status) -{ - if (!autoware_state_ptr) { - ROS_DEBUG_STREAM_THROTTLE(5.0, "[AutowareIvAutowareStatePublisher] autoware_state is nullptr"); - return; - } - - // get autoware_state - status->autoware_state = autoware_state_ptr->state; - status->arrived_goal = isGoal(autoware_state_ptr); -} - -void AutowareIvAutowareStatePublisher::getControlModeInfo( - const autoware_vehicle_msgs::ControlMode::ConstPtr & control_mode_ptr, - autoware_api_msgs::AwapiAutowareStatus * status) -{ - if (!control_mode_ptr) { - ROS_DEBUG_STREAM_THROTTLE(5.0, "[AutowareIvAutowareStatePublisher] control mode is nullptr"); - return; - } - - // get control mode - status->control_mode = control_mode_ptr->data; -} - -void AutowareIvAutowareStatePublisher::getGateModeInfo( - const autoware_control_msgs::GateMode::ConstPtr & gate_mode_ptr, - autoware_api_msgs::AwapiAutowareStatus * status) -{ - if (!gate_mode_ptr) { - ROS_DEBUG_STREAM_THROTTLE(5.0, "[AutowareIvAutowareStatePublisher] gate mode is nullptr"); - return; - } - - // get control mode - status->gate_mode = gate_mode_ptr->data; -} - -void AutowareIvAutowareStatePublisher::getIsEmergencyInfo( - const std_msgs::Bool::ConstPtr & is_emergency_ptr, - autoware_api_msgs::AwapiAutowareStatus * status) -{ - if (!is_emergency_ptr) { - ROS_DEBUG_STREAM_THROTTLE(5.0, "[AutowareIvAutowareStatePublisher] is_emergency is nullptr"); - return; - } - - // get emergency - status->emergency_stopped = is_emergency_ptr->data; -} - -void AutowareIvAutowareStatePublisher::getStopReasonInfo( - const autoware_planning_msgs::StopReasonArray::ConstPtr & stop_reason_ptr, - autoware_api_msgs::AwapiAutowareStatus * status) -{ - if (!stop_reason_ptr) { - ROS_DEBUG_STREAM_THROTTLE(5.0, "[AutowareIvAutowareStatePublisher] stop reason is nullptr"); - return; - } - - status->stop_reason = *stop_reason_ptr; -} - -void AutowareIvAutowareStatePublisher::getDiagInfo( - const diagnostic_msgs::DiagnosticArray::ConstPtr & diag_ptr, - autoware_api_msgs::AwapiAutowareStatus * status) -{ - if (!diag_ptr) { - ROS_DEBUG_STREAM_THROTTLE(5.0, "[AutowareIvAutowareStatePublisher] diagnostics is nullptr"); - return; - } - - // get diag - status->diagnostics = extractLeafDiag(diag_ptr->status); -} - -void AutowareIvAutowareStatePublisher::getGlobalRptInfo( - const pacmod_msgs::GlobalRpt::ConstPtr & global_rpt_ptr, - autoware_api_msgs::AwapiAutowareStatus * status) -{ - if (!global_rpt_ptr) { - ROS_DEBUG_STREAM_THROTTLE(5.0, "[AutowareIvAutowareStatePublisher] global_rpt is nullptr"); - return; - } - - // get global_rpt - status->autonomous_overriden = global_rpt_ptr->override_active; -} - -bool AutowareIvAutowareStatePublisher::isGoal( - const autoware_system_msgs::AutowareState::ConstPtr & autoware_state) -{ - //rename - const auto & aw_state = autoware_state->state; - - if (aw_state == autoware_system_msgs::AutowareState::ArrivedGoal) { - arrived_goal_ = true; - } else if ( - prev_state_ == autoware_system_msgs::AutowareState::Driving && - aw_state == autoware_system_msgs::AutowareState::WaitingForRoute) { - arrived_goal_ = true; - } - - if ( - aw_state == autoware_system_msgs::AutowareState::WaitingForEngage || - aw_state == autoware_system_msgs::AutowareState::Driving) { - //cancel goal state - arrived_goal_ = false; - } - - prev_state_ = aw_state; - - return arrived_goal_; -} - -std::vector AutowareIvAutowareStatePublisher::extractLeafDiag( - const std::vector & diag_vec) -{ - updateDiagNameSet(diag_vec); - - std::vector leaf_diag_info; - for (const auto diag : diag_vec) { - if (isLeaf(diag)) { - leaf_diag_info.emplace_back(diag); - } - } - return leaf_diag_info; -} - -std::string AutowareIvAutowareStatePublisher::splitStringByLastSlash(const std::string & str) -{ - const auto last_slash = str.find_last_of("/"); - - if (last_slash == std::string::npos) { - // if not find slash - return str; - } - - return str.substr(0, last_slash); -} - -void AutowareIvAutowareStatePublisher::updateDiagNameSet( - const std::vector & diag_vec) -{ - // set diag name to diag_name_set_ - for (const auto & diag : diag_vec) { - diag_name_set_.insert(splitStringByLastSlash(diag.name)); - } -} - -bool AutowareIvAutowareStatePublisher::isLeaf(const diagnostic_msgs::DiagnosticStatus & diag) -{ - //if not find diag.name in diag set, diag is leaf. - return diag_name_set_.find(diag.name) == diag_name_set_.end(); -} - -} // namespace autoware_api diff --git a/awapi/awapi_awiv_adapter/src/awapi_autoware_util.cpp b/awapi/awapi_awiv_adapter/src/awapi_autoware_util.cpp deleted file mode 100644 index b6341ed8a5528..0000000000000 --- a/awapi/awapi_awiv_adapter/src/awapi_autoware_util.cpp +++ /dev/null @@ -1,26 +0,0 @@ -/* - * Copyright 2020 Tier IV, Inc. All rights reserved. - * - * 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. - */ - -#include - -namespace autoware_api -{ -double lowpass_filter(const double current_value, const double prev_value, const double gain) -{ - return gain * prev_value + (1.0 - gain) * current_value; -} - -} // namespace autoware_api \ No newline at end of file diff --git a/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp b/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp deleted file mode 100644 index 1ef8801a99449..0000000000000 --- a/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp +++ /dev/null @@ -1,222 +0,0 @@ -/* - * Copyright 2020 Tier IV, Inc. All rights reserved. - * - * 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. - */ - -#include - -namespace autoware_api -{ -AutowareIvAdapter::AutowareIvAdapter() : nh_(), pnh_("~"), tf_listener_(tf_buffer_) -{ - // get param - pnh_.param("status_pub_hz", status_pub_hz_, 5.0); - pnh_.param("stop_reason_timeout", stop_reason_timeout_, 0.5); - const bool em_handle_param = waitForParam(pnh_, "param/emergency_handling"); - emergencyParamCheck(em_handle_param); - - // setup instance - vehicle_state_publisher_ = std::make_unique(); - autoware_state_publisher_ = std::make_unique(); - stop_reason_aggreagator_ = std::make_unique(stop_reason_timeout_); - lane_change_state_publisher_ = std::make_unique(); - obstacle_avoidance_state_publisher_ = - std::make_unique(); - - // subscriber - sub_steer_ = pnh_.subscribe("input/steer", 1, &AutowareIvAdapter::callbackSteer, this); - sub_vehicle_cmd_ = - pnh_.subscribe("input/vehicle_cmd", 1, &AutowareIvAdapter::callbackVehicleCmd, this); - sub_turn_signal_cmd_ = - pnh_.subscribe("input/turn_signal", 1, &AutowareIvAdapter::callbackTurnSignal, this); - sub_twist_ = pnh_.subscribe("input/twist", 1, &AutowareIvAdapter::callbackTwist, this); - sub_gear_ = pnh_.subscribe("input/gear", 1, &AutowareIvAdapter::callbackGear, this); - sub_battery_ = pnh_.subscribe("input/battery", 1, &AutowareIvAdapter::callbackBattery, this); - sub_nav_sat_ = pnh_.subscribe("input/nav_sat", 1, &AutowareIvAdapter::callbackNavSat, this); - sub_autoware_state_ = - pnh_.subscribe("input/autoware_state", 1, &AutowareIvAdapter::callbackAutowareState, this); - sub_control_mode_ = - pnh_.subscribe("input/control_mode", 1, &AutowareIvAdapter::callbackControlMode, this); - sub_gate_mode_ = pnh_.subscribe("input/gate_mode", 1, &AutowareIvAdapter::callbackGateMode, this); - sub_emergency_ = - pnh_.subscribe("input/is_emergency", 1, &AutowareIvAdapter::callbackIsEmergency, this); - sub_stop_reason_ = - pnh_.subscribe("input/stop_reason", 100, &AutowareIvAdapter::callbackStopReason, this); - sub_diagnostics_ = - pnh_.subscribe("input/diagnostics", 1, &AutowareIvAdapter::callbackDiagnostics, this); - sub_global_rpt_ = - pnh_.subscribe("input/global_rpt", 1, &AutowareIvAdapter::callbackGlobalRpt, this); - sub_lane_change_available_ = pnh_.subscribe( - "input/lane_change_avaiable", 1, &AutowareIvAdapter::callbackLaneChangeAvailable, this); - sub_lane_change_ready_ = - pnh_.subscribe("input/lane_change_ready", 1, &AutowareIvAdapter::callbackLaneChangeReady, this); - sub_lane_change_candidate_ = pnh_.subscribe( - "input/lane_change_candidate_path", 1, &AutowareIvAdapter::callbackLaneChangeCandidatePath, - this); - sub_obstacle_avoid_ready_ = pnh_.subscribe( - "input/obstacle_avoid_ready", 1, &AutowareIvAdapter::callbackLaneObstacleAvoidReady, this); - sub_obstacle_avoid_candidate_ = pnh_.subscribe( - "input/obstacle_avoid_candidate_path", 1, - &AutowareIvAdapter::callbackLaneObstacleAvoidCandidatePath, this); - - // timer - timer_ = - nh_.createTimer(ros::Duration(1.0 / status_pub_hz_), &AutowareIvAdapter::timerCallback, this); -} - -void AutowareIvAdapter::emergencyParamCheck(const bool emergency_handling_param) -{ - if (!emergency_handling_param) { - ROS_WARN_STREAM("parameter[use_emergency_handling] is false."); - ROS_WARN_STREAM("autoware/put/emergency is not valid"); - } -} - -void AutowareIvAdapter::timerCallback(const ros::TimerEvent & e) -{ - // get current pose - getCurrentPose(); - - // publish vehicle state - vehicle_state_publisher_->statePublisher(aw_info_); - - // publish autoware state - autoware_state_publisher_->statePublisher(aw_info_); - - // publish lane change state - lane_change_state_publisher_->statePublisher(aw_info_); - - // publish obstacle_avoidance state - obstacle_avoidance_state_publisher_->statePublisher(aw_info_); -} - -void AutowareIvAdapter::callbackSteer(const autoware_vehicle_msgs::Steering::ConstPtr & msg_ptr) -{ - aw_info_.steer_ptr = msg_ptr; -} - -void AutowareIvAdapter::callbackVehicleCmd( - const autoware_vehicle_msgs::VehicleCommand::ConstPtr & msg_ptr) -{ - aw_info_.vehicle_cmd_ptr = msg_ptr; -} - -void AutowareIvAdapter::callbackTurnSignal( - const autoware_vehicle_msgs::TurnSignal::ConstPtr & msg_ptr) -{ - aw_info_.turn_signal_ptr = msg_ptr; -} - -void AutowareIvAdapter::callbackTwist(const geometry_msgs::TwistStamped::ConstPtr & msg_ptr) -{ - aw_info_.twist_ptr = msg_ptr; -} - -void AutowareIvAdapter::callbackGear(const autoware_vehicle_msgs::ShiftStamped::ConstPtr & msg_ptr) -{ - aw_info_.gear_ptr = msg_ptr; -} - -void AutowareIvAdapter::callbackBattery(const std_msgs::Float32::ConstPtr & msg_ptr) -{ - aw_info_.battery_ptr = msg_ptr; -} - -void AutowareIvAdapter::callbackNavSat(const sensor_msgs::NavSatFix::ConstPtr & msg_ptr) -{ - aw_info_.nav_sat_ptr = msg_ptr; -} - -void AutowareIvAdapter::getCurrentPose() -{ - try { - auto transform = tf_buffer_.lookupTransform("map", "base_link", ros::Time(0)); - geometry_msgs::PoseStamped ps; - ps.header = transform.header; - ps.pose.position.x = transform.transform.translation.x; - ps.pose.position.y = transform.transform.translation.y; - ps.pose.position.z = transform.transform.translation.z; - ps.pose.orientation = transform.transform.rotation; - aw_info_.current_pose_ptr = std::make_shared(ps); - } catch (tf2::TransformException & ex) { - ROS_DEBUG_STREAM_THROTTLE(2.0, "[awapi_autoware_adapter] cannot get self pose"); - } -} - -void AutowareIvAdapter::callbackAutowareState( - const autoware_system_msgs::AutowareState::ConstPtr & msg_ptr) -{ - aw_info_.autoware_state_ptr = msg_ptr; -} -void AutowareIvAdapter::callbackControlMode( - const autoware_vehicle_msgs::ControlMode::ConstPtr & msg_ptr) -{ - aw_info_.control_mode_ptr = msg_ptr; -} - -void AutowareIvAdapter::callbackGateMode(const autoware_control_msgs::GateMode::ConstPtr & msg_ptr) -{ - aw_info_.gate_mode_ptr = msg_ptr; -} - -void AutowareIvAdapter::callbackIsEmergency(const std_msgs::Bool::ConstPtr & msg_ptr) -{ - aw_info_.is_emergency_ptr = msg_ptr; -} - -void AutowareIvAdapter::callbackStopReason( - const autoware_planning_msgs::StopReasonArrayConstPtr & msg_ptr) -{ - aw_info_.stop_reason_ptr = stop_reason_aggreagator_->updateStopReasonArray(msg_ptr); -} - -void AutowareIvAdapter::callbackDiagnostics( - const diagnostic_msgs::DiagnosticArray::ConstPtr & msg_ptr) -{ - aw_info_.diagnostic_ptr = msg_ptr; -} - -void AutowareIvAdapter::callbackGlobalRpt(const pacmod_msgs::GlobalRpt::ConstPtr & msg_ptr) -{ - aw_info_.global_rpt_ptr = msg_ptr; -} - -void AutowareIvAdapter::callbackLaneChangeAvailable(const std_msgs::Bool::ConstPtr & msg_ptr) -{ - aw_info_.lane_change_available_ptr = msg_ptr; -} - -void AutowareIvAdapter::callbackLaneChangeReady(const std_msgs::Bool::ConstPtr & msg_ptr) -{ - aw_info_.lane_change_ready_ptr = msg_ptr; -} - -void AutowareIvAdapter::callbackLaneChangeCandidatePath( - const autoware_planning_msgs::Path::ConstPtr & msg_ptr) -{ - aw_info_.lane_change_candidate_ptr = msg_ptr; -} - -void AutowareIvAdapter::callbackLaneObstacleAvoidReady(const std_msgs::Bool::ConstPtr & msg_ptr) -{ - aw_info_.obstacle_avoid_ready_ptr = msg_ptr; -} - -void AutowareIvAdapter::callbackLaneObstacleAvoidCandidatePath( - const autoware_planning_msgs::Trajectory::ConstPtr & msg_ptr) -{ - aw_info_.obstacle_avoid_candidate_ptr = msg_ptr; -} - -} // namespace autoware_api diff --git a/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_node.cpp b/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_node.cpp deleted file mode 100644 index a66c0a5773d3b..0000000000000 --- a/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_node.cpp +++ /dev/null @@ -1,30 +0,0 @@ -/* - * Copyright 2020 Tier IV, Inc. All rights reserved. - * - * 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. - */ - -#include - -#include - -int main(int argc, char ** argv) -{ - ros::init(argc, argv, "awapi_awiv_adapter_node"); - - autoware_api::AutowareIvAdapter node; - - ros::spin(); - - return 0; -} \ No newline at end of file diff --git a/awapi/awapi_awiv_adapter/src/awapi_lane_change_state_publisher.cpp b/awapi/awapi_awiv_adapter/src/awapi_lane_change_state_publisher.cpp deleted file mode 100644 index 8a3f7f7fd1ca7..0000000000000 --- a/awapi/awapi_awiv_adapter/src/awapi_lane_change_state_publisher.cpp +++ /dev/null @@ -1,85 +0,0 @@ -/* - * Copyright 2020 Tier IV, Inc. All rights reserved. - * - * 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. - */ - -#include - -namespace autoware_api -{ -AutowareIvLaneChangeStatePublisher::AutowareIvLaneChangeStatePublisher() : nh_(), pnh_("~") -{ - // publisher - pub_state_ = pnh_.advertise("output/lane_change_status", 1); -} - -void AutowareIvLaneChangeStatePublisher::statePublisher(const AutowareInfo & aw_info) -{ - autoware_api_msgs::LaneChangeStatus status; - - //input header - status.header.frame_id = "base_link"; - status.header.stamp = ros::Time::now(); - - // get all info - getLaneChangeAvailableInfo(aw_info.lane_change_available_ptr, &status); - getLaneChangeReadyInfo(aw_info.lane_change_ready_ptr, &status); - getCandidatePathInfo(aw_info.lane_change_candidate_ptr, &status); - - // publish info - pub_state_.publish(status); -} - -void AutowareIvLaneChangeStatePublisher::getLaneChangeAvailableInfo( - const std_msgs::Bool::ConstPtr & available_ptr, autoware_api_msgs::LaneChangeStatus * status) -{ - if (!available_ptr) { - ROS_DEBUG_STREAM_THROTTLE( - 5.0, "[AutowareIvLaneChangeStatePublisher] lane change available is nullptr"); - return; - } - - // get lane change available info - status->force_lane_change_available = available_ptr->data; -} - -void AutowareIvLaneChangeStatePublisher::getLaneChangeReadyInfo( - const std_msgs::Bool::ConstPtr & ready_ptr, autoware_api_msgs::LaneChangeStatus * status) -{ - if (!ready_ptr) { - ROS_DEBUG_STREAM_THROTTLE( - 5.0, "[AutowareIvLaneChangeStatePublisher] lane change ready is nullptr"); - return; - } - - // get lane change available info - status->lane_change_ready = ready_ptr->data; -} - -void AutowareIvLaneChangeStatePublisher::getCandidatePathInfo( - const autoware_planning_msgs::Path::ConstPtr & path_ptr, - autoware_api_msgs::LaneChangeStatus * status) -{ - if (!path_ptr) { - ROS_DEBUG_STREAM_THROTTLE( - 5.0, - "[AutowareIvLaneChangeStatePublisher] lane_change_candidate_path is " - "nullptr"); - return; - } - - status->candidate_path = *path_ptr; -} - -} // namespace autoware_api diff --git a/awapi/awapi_awiv_adapter/src/awapi_obstacle_avoidance_state_publisher.cpp b/awapi/awapi_awiv_adapter/src/awapi_obstacle_avoidance_state_publisher.cpp deleted file mode 100644 index 4a00761d72fe6..0000000000000 --- a/awapi/awapi_awiv_adapter/src/awapi_obstacle_avoidance_state_publisher.cpp +++ /dev/null @@ -1,72 +0,0 @@ -/* - * Copyright 2020 Tier IV, Inc. All rights reserved. - * - * 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. - */ - -#include - -namespace autoware_api -{ -AutowareIvObstacleAvoidanceStatePublisher::AutowareIvObstacleAvoidanceStatePublisher() -: nh_(), pnh_("~") -{ - // publisher - pub_state_ = - pnh_.advertise("output/obstacle_avoid_status", 1); -} - -void AutowareIvObstacleAvoidanceStatePublisher::statePublisher(const AutowareInfo & aw_info) -{ - autoware_api_msgs::ObstacleAvoidanceStatus status; - - //input header - status.header.frame_id = "base_link"; - status.header.stamp = ros::Time::now(); - - // get all info - getObstacleAvoidReadyInfo(aw_info.obstacle_avoid_ready_ptr, &status); - getCandidatePathInfo(aw_info.obstacle_avoid_candidate_ptr, &status); - - // publish info - pub_state_.publish(status); -} - -void AutowareIvObstacleAvoidanceStatePublisher::getObstacleAvoidReadyInfo( - const std_msgs::Bool::ConstPtr & ready_ptr, autoware_api_msgs::ObstacleAvoidanceStatus * status) -{ - if (!ready_ptr) { - ROS_DEBUG_STREAM_THROTTLE( - 5.0, "[AutowareIvObstacleAvoidanceStatePublisher] obstacle_avoidance_ready is nullptr"); - return; - } - - status->obstacle_avoidance_ready = ready_ptr->data; -} - -void AutowareIvObstacleAvoidanceStatePublisher::getCandidatePathInfo( - const autoware_planning_msgs::Trajectory::ConstPtr & path_ptr, - autoware_api_msgs::ObstacleAvoidanceStatus * status) -{ - if (!path_ptr) { - ROS_DEBUG_STREAM_THROTTLE( - 5.0, - "[AutowareIvObstacleAvoidanceStatePublisher] obstacle_avoidance_candidate_path is " - "nullptr"); - return; - } - - status->candidate_path = *path_ptr; -} - -} // namespace autoware_api diff --git a/awapi/awapi_awiv_adapter/src/awapi_stop_reason_aggregator.cpp b/awapi/awapi_awiv_adapter/src/awapi_stop_reason_aggregator.cpp deleted file mode 100644 index 07396e51f8c31..0000000000000 --- a/awapi/awapi_awiv_adapter/src/awapi_stop_reason_aggregator.cpp +++ /dev/null @@ -1,134 +0,0 @@ -/* - * Copyright 2020 Tier IV, Inc. All rights reserved. - * - * 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. - */ - -#include - -namespace autoware_api -{ -AutowareIvStopReasonAggregator::AutowareIvStopReasonAggregator(const double timeout) -: timeout_(timeout) -{ -} - -autoware_planning_msgs::StopReasonArray::ConstPtr -AutowareIvStopReasonAggregator::updateStopReasonArray( - const autoware_planning_msgs::StopReasonArray::ConstPtr & msg_ptr) -{ - applyUpdate(msg_ptr); - applyTimeOut(); - return makeStopReasonArray(); -} - -void AutowareIvStopReasonAggregator::applyUpdate( - const autoware_planning_msgs::StopReasonArray::ConstPtr & msg_ptr) -{ - /* remove old stop_reason that matches reason with received msg */ - //make reason-matching msg list - - std::vector remove_idx; - if (!stop_reason_array_vec_.empty()) { - for (int i = stop_reason_array_vec_.size() - 1; i >= 0; i--) { - if (checkMatchingReason(msg_ptr, stop_reason_array_vec_.at(i))) { - remove_idx.emplace_back(i); - } - } - } - - // remove reason matching msg - for (const auto idx : remove_idx) { - stop_reason_array_vec_.erase(stop_reason_array_vec_.begin() + idx); - } - - // add new reason msg - stop_reason_array_vec_.emplace_back(*msg_ptr); -} - -bool AutowareIvStopReasonAggregator::checkMatchingReason( - const autoware_planning_msgs::StopReasonArray::ConstPtr & msg_stop_reason_array, - const autoware_planning_msgs::StopReasonArray & stop_reason_array) -{ - for (const auto msg_stop_reason : msg_stop_reason_array->stop_reasons) { - for (const auto stop_reason : stop_reason_array.stop_reasons) { - if (msg_stop_reason.reason == stop_reason.reason) { - //find matching reason - return true; - } - } - } - // cannot find matching reason - return false; -} - -void AutowareIvStopReasonAggregator::applyTimeOut() -{ - const auto current_time = ros::Time::now(); - - //make timeout-msg list - std::vector remove_idx; - if (!stop_reason_array_vec_.empty()) { - for (int i = stop_reason_array_vec_.size() - 1; i >= 0; i--) { - if ((current_time - stop_reason_array_vec_.at(i).header.stamp).toSec() > timeout_) { - remove_idx.emplace_back(i); - } - } - } - //remove timeout-msg - for (const auto idx : remove_idx) { - stop_reason_array_vec_.erase(stop_reason_array_vec_.begin() + idx); - } -} - -void AutowareIvStopReasonAggregator::appendStopReasonToArray( - const autoware_planning_msgs::StopReason & stop_reason, - autoware_planning_msgs::StopReasonArray * stop_reason_array) -{ - //if stop factors is empty, not append - if (stop_reason.stop_factors.empty()) { - return; - } - - //if already exists same reason msg in stop_reason_array_msg, append stop_factors to there - for (int i = 0; i < stop_reason_array->stop_reasons.size(); i++) { - if (stop_reason_array->stop_reasons.at(i).reason == stop_reason.reason) { - stop_reason_array->stop_reasons.at(i).stop_factors.insert( - stop_reason_array->stop_reasons.at(i).stop_factors.end(), stop_reason.stop_factors.begin(), - stop_reason.stop_factors.end()); - return; - } - } - - //if not exist same reason msg, append new stop reason - stop_reason_array->stop_reasons.emplace_back(stop_reason); -} - -autoware_planning_msgs::StopReasonArray::ConstPtr -AutowareIvStopReasonAggregator::makeStopReasonArray() -{ - autoware_planning_msgs::StopReasonArray stop_reason_array_msg; - // input header - stop_reason_array_msg.header.frame_id = "map"; - stop_reason_array_msg.header.stamp = ros::Time::now(); - - // input stop reason - for (const auto stop_reason_array : stop_reason_array_vec_) { - for (const auto stop_reason : stop_reason_array.stop_reasons) { - appendStopReasonToArray(stop_reason, &stop_reason_array_msg); - } - } - return boost::make_shared(stop_reason_array_msg); -} - -} // namespace autoware_api \ No newline at end of file diff --git a/awapi/awapi_awiv_adapter/src/awapi_vehicle_state_publisher.cpp b/awapi/awapi_awiv_adapter/src/awapi_vehicle_state_publisher.cpp deleted file mode 100644 index 0d6fc747d60b0..0000000000000 --- a/awapi/awapi_awiv_adapter/src/awapi_vehicle_state_publisher.cpp +++ /dev/null @@ -1,206 +0,0 @@ -/* - * Copyright 2020 Tier IV, Inc. All rights reserved. - * - * 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. - */ - -#include - -namespace autoware_api -{ -AutowareIvVehicleStatePublisher::AutowareIvVehicleStatePublisher() -: nh_(), pnh_("~"), prev_accel_(0.0) -{ - // publisher - pub_state_ = pnh_.advertise("output/vehicle_status", 1); -} - -void AutowareIvVehicleStatePublisher::statePublisher(const AutowareInfo & aw_info) -{ - autoware_api_msgs::AwapiVehicleStatus status = initVehicleStatus(); - - //input header - status.header.frame_id = "base_link"; - status.header.stamp = ros::Time::now(); - - // get all info - getPoseInfo(aw_info.current_pose_ptr, &status); - getSteerInfo(aw_info.steer_ptr, &status); - getVehicleCmdInfo(aw_info.vehicle_cmd_ptr, &status); - getTurnSignalInfo(aw_info.turn_signal_ptr, &status); - getTwistInfo(aw_info.twist_ptr, &status); - getGearInfo(aw_info.gear_ptr, &status); - getBatteryInfo(aw_info.battery_ptr, &status); - getGpsInfo(aw_info.nav_sat_ptr, &status); - - // publish info - pub_state_.publish(status); -} - -autoware_api_msgs::AwapiVehicleStatus AutowareIvVehicleStatePublisher::initVehicleStatus() -{ - autoware_api_msgs::AwapiVehicleStatus status; - // set default value - if (std::numeric_limits::has_quiet_NaN) { - status.energy_level = std::numeric_limits::quiet_NaN(); - } - return status; -} - -void AutowareIvVehicleStatePublisher::getPoseInfo( - const std::shared_ptr & pose_ptr, - autoware_api_msgs::AwapiVehicleStatus * status) -{ - if (!pose_ptr) { - ROS_DEBUG_STREAM_THROTTLE(5.0, "[AutowareIvVehicleStatePublisher] current pose is nullptr"); - return; - } - - // get pose - status->pose = pose_ptr->pose; - - // convert quaternion to euler - double yaw, pitch, roll; - tf2::getEulerYPR(pose_ptr->pose.orientation, yaw, pitch, roll); - status->eulerangle.yaw = yaw; - status->eulerangle.pitch = pitch; - status->eulerangle.roll = roll; -} - -void AutowareIvVehicleStatePublisher::getSteerInfo( - const autoware_vehicle_msgs::Steering::ConstPtr & steer_ptr, - autoware_api_msgs::AwapiVehicleStatus * status) -{ - if (!steer_ptr) { - ROS_DEBUG_STREAM_THROTTLE(5.0, "[AutowareIvVehicleStatePublisher] steer is nullptr"); - return; - } - - // get steer - status->steering = steer_ptr->data; - - // get steer vel - if (previous_steer_ptr_) { - //calculate steer vel from steer - const double ds = steer_ptr->data - previous_steer_ptr_->data; - const double dt = - std::max((steer_ptr->header.stamp - previous_steer_ptr_->header.stamp).toSec(), 1e-03); - const double steer_vel = ds / dt; - - //apply lowpass filter - const double lowpass_steer = - lowpass_filter(steer_vel, prev_steer_vel_, steer_vel_lowpass_gain_); - prev_steer_vel_ = lowpass_steer; - status->steering_velocity = lowpass_steer; - } - previous_steer_ptr_ = steer_ptr; -} -void AutowareIvVehicleStatePublisher::getVehicleCmdInfo( - const autoware_vehicle_msgs::VehicleCommand::ConstPtr & vehicle_cmd_ptr, - autoware_api_msgs::AwapiVehicleStatus * status) -{ - if (!vehicle_cmd_ptr) { - ROS_DEBUG_STREAM_THROTTLE(5.0, "[AutowareIvVehicleStatePublisher] vehicle cmd is nullptr"); - return; - } - - // get command - status->target_acceleration = vehicle_cmd_ptr->control.acceleration; - status->target_velocity = vehicle_cmd_ptr->control.velocity; - status->target_steering = vehicle_cmd_ptr->control.steering_angle; - status->target_steering_velocity = vehicle_cmd_ptr->control.steering_angle_velocity; -} - -void AutowareIvVehicleStatePublisher::getTurnSignalInfo( - const autoware_vehicle_msgs::TurnSignal::ConstPtr & turn_signal_ptr, - autoware_api_msgs::AwapiVehicleStatus * status) -{ - if (!turn_signal_ptr) { - ROS_DEBUG_STREAM_THROTTLE(5.0, "[AutowareIvVehicleStatePublisher] turn signal is nullptr"); - return; - } - - //get turn singnal - status->turn_signal = turn_signal_ptr->data; -} - -void AutowareIvVehicleStatePublisher::getTwistInfo( - const geometry_msgs::TwistStamped::ConstPtr & twist_ptr, - autoware_api_msgs::AwapiVehicleStatus * status) -{ - if (!twist_ptr) { - ROS_DEBUG_STREAM_THROTTLE(5.0, "[AutowareIvVehicleStatePublisher] twist is nullptr"); - return; - } - - // get twist - status->velocity = twist_ptr->twist.linear.x; - status->angular_velocity = twist_ptr->twist.angular.z; - - // get accel - if (previous_twist_ptr_) { - //calculate accleration from velocity - const double dv = twist_ptr->twist.linear.x - previous_twist_ptr_->twist.linear.x; - const double dt = - std::max((twist_ptr->header.stamp - previous_twist_ptr_->header.stamp).toSec(), 1e-03); - const double accel = dv / dt; - - // apply lowpass filter - const double lowpass_accel = lowpass_filter(accel, prev_accel_, accel_lowpass_gain_); - prev_accel_ = lowpass_accel; - status->acceleration = lowpass_accel; - } - previous_twist_ptr_ = twist_ptr; -} - -void AutowareIvVehicleStatePublisher::getGearInfo( - const autoware_vehicle_msgs::ShiftStamped::ConstPtr & gear_ptr, - autoware_api_msgs::AwapiVehicleStatus * status) -{ - if (!gear_ptr) { - ROS_DEBUG_STREAM_THROTTLE(5.0, "[AutowareIvVehicleStatePublisher] gear is nullptr"); - return; - } - - // get gear (shift) - status->gear = gear_ptr->shift.data; -} - -void AutowareIvVehicleStatePublisher::getBatteryInfo( - const std_msgs::Float32::ConstPtr & battery_ptr, autoware_api_msgs::AwapiVehicleStatus * status) -{ - if (!battery_ptr) { - ROS_DEBUG_STREAM_THROTTLE(5.0, "[AutowareIvVehicleStatePublisher] battery is nullptr"); - return; - } - - // get battery - status->energy_level = battery_ptr->data; -} - -void AutowareIvVehicleStatePublisher::getGpsInfo( - const sensor_msgs::NavSatFix::ConstPtr & nav_sat_ptr, - autoware_api_msgs::AwapiVehicleStatus * status) -{ - if (!nav_sat_ptr) { - ROS_DEBUG_STREAM_THROTTLE(5.0, "[AutowareIvVehicleStatePublisher] nav_sat(gps) is nullptr"); - return; - } - - // get position (latlon) - status->latlon.lat = nav_sat_ptr->latitude; - status->latlon.lon = nav_sat_ptr->longitude; - status->latlon.alt = nav_sat_ptr->altitude; -} - -} // namespace autoware_api From fbb772783b1539e341a28c4a2409c3c397f9fa11 Mon Sep 17 00:00:00 2001 From: mitsudome-r Date: Thu, 8 Oct 2020 17:24:31 +0900 Subject: [PATCH 14/71] Revert "remove ROS1 packages temporarily" This reverts commit c85c1d6789b1a1e63c6b4e60fa4f3296dfb12e74. Signed-off-by: mitsudome-r Signed-off-by: tanaka3 --- awapi/awapi_awiv_adapter/CMakeLists.txt | 70 ++++++ awapi/awapi_awiv_adapter/Readme.md | 190 +++++++++++++++ .../awapi_autoware_state_publisher.h | 76 ++++++ .../awapi_awiv_adapter/awapi_autoware_util.h | 102 ++++++++ .../awapi_awiv_adapter_core.h | 102 ++++++++ .../awapi_lane_change_state_publisher.h | 45 ++++ ...awapi_obstacle_avoidance_state_publisher.h | 44 ++++ .../awapi_stop_reason_aggregator.h | 45 ++++ .../awapi_vehicle_state_publisher.h | 73 ++++++ .../launch/awapi_awiv_adapter.launch | 111 +++++++++ awapi/awapi_awiv_adapter/package.xml | 25 ++ .../src/awapi_autoware_state_publisher.cpp | 208 ++++++++++++++++ .../src/awapi_autoware_util.cpp | 26 ++ .../src/awapi_awiv_adapter_core.cpp | 222 ++++++++++++++++++ .../src/awapi_awiv_adapter_node.cpp | 30 +++ .../src/awapi_lane_change_state_publisher.cpp | 85 +++++++ ...api_obstacle_avoidance_state_publisher.cpp | 72 ++++++ .../src/awapi_stop_reason_aggregator.cpp | 134 +++++++++++ .../src/awapi_vehicle_state_publisher.cpp | 206 ++++++++++++++++ 19 files changed, 1866 insertions(+) create mode 100644 awapi/awapi_awiv_adapter/CMakeLists.txt create mode 100644 awapi/awapi_awiv_adapter/Readme.md create mode 100644 awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.h create mode 100644 awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.h create mode 100644 awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.h create mode 100644 awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_lane_change_state_publisher.h create mode 100644 awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_obstacle_avoidance_state_publisher.h create mode 100644 awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_stop_reason_aggregator.h create mode 100644 awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_vehicle_state_publisher.h create mode 100644 awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch create mode 100644 awapi/awapi_awiv_adapter/package.xml create mode 100644 awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp create mode 100644 awapi/awapi_awiv_adapter/src/awapi_autoware_util.cpp create mode 100644 awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp create mode 100644 awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_node.cpp create mode 100644 awapi/awapi_awiv_adapter/src/awapi_lane_change_state_publisher.cpp create mode 100644 awapi/awapi_awiv_adapter/src/awapi_obstacle_avoidance_state_publisher.cpp create mode 100644 awapi/awapi_awiv_adapter/src/awapi_stop_reason_aggregator.cpp create mode 100644 awapi/awapi_awiv_adapter/src/awapi_vehicle_state_publisher.cpp diff --git a/awapi/awapi_awiv_adapter/CMakeLists.txt b/awapi/awapi_awiv_adapter/CMakeLists.txt new file mode 100644 index 0000000000000..e5f36e0a12a31 --- /dev/null +++ b/awapi/awapi_awiv_adapter/CMakeLists.txt @@ -0,0 +1,70 @@ +cmake_minimum_required(VERSION 3.0.2) +project(awapi_awiv_adapter) + +add_compile_options(-std=c++14) + +find_package(catkin REQUIRED COMPONENTS + message_generation + message_runtime + roscpp + autoware_api_msgs + autoware_system_msgs + autoware_planning_msgs + autoware_control_msgs + autoware_vehicle_msgs + diagnostic_msgs + geometry_msgs + pacmod_msgs + std_msgs + sensor_msgs + tf2 + tf2_geometry_msgs +) + +catkin_package( + INCLUDE_DIRS include +) + +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + +add_executable(awapi_awiv_adapter + src/awapi_awiv_adapter_node.cpp + src/awapi_awiv_adapter_core.cpp + src/awapi_vehicle_state_publisher.cpp + src/awapi_autoware_state_publisher.cpp + src/awapi_stop_reason_aggregator.cpp + src/awapi_lane_change_state_publisher.cpp + src/awapi_obstacle_avoidance_state_publisher.cpp + src/awapi_autoware_util.cpp +) + +target_link_libraries(awapi_awiv_adapter + ${catkin_LIBRARIES} +) + +add_dependencies(awapi_awiv_adapter + ${catkin_EXPORTED_TARGETS} +) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) + +install( + DIRECTORY + launch + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +install(TARGETS awapi_awiv_adapter + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +if(CATKIN_ENABLE_TESTING) + find_package(rostest REQUIRED) +endif() diff --git a/awapi/awapi_awiv_adapter/Readme.md b/awapi/awapi_awiv_adapter/Readme.md new file mode 100644 index 0000000000000..ad534f2ec4a2c --- /dev/null +++ b/awapi/awapi_awiv_adapter/Readme.md @@ -0,0 +1,190 @@ +# AWAPI_AWIV_ADAPTER + +✓: confirmed, (blank): not confirmed + +## get topic + +### /awapi/vehicle/get/status + +- get vehicle status +- MessageType: awapi_awiv_adapter/AwapiVehicleStatus + +| ✓ | type | name | unit | note | +| --- | :------------------------ | :----------------------- | :-------------------------------------------- | :--------------------------------------- | +| ✓ | std_msgs/Header | header | | | +| ✓ | geometry_msgs/Pose | pose | position:[m] | | +| ✓ | awapi_awiv_adapter/Euler | eulerangle | [rad] | roll/pitch/yaw | +| | awapi_awiv_adapter/Latlon | latlon | | lat/lon/alt | +| ✓ | float64 | velocity | [m/s] | | +| ✓ | float64 | acceleration | [m/ss] | calculate from velocity in awapi_adapter | +| ✓ | float64 | steering | [rad] | | +| ✓ | float64 | steering_velocity | [rad/s] | calculate from steering in awapi_adapter | +| ✓ | float64 | angular_velocity | [rad/s] | | +| | int32 | gear | accroding to autoware_vehicle_msgs/Shift | | +| | float32 | energy_level | | available only for golf-cart | +| ✓ | int32 | turn_signal | accroding to autoware_vehicle_msgs/TurnSignal | | +| ✓ | float64 | target_velocity | [m/s] | | +| ✓ | float64 | target_acceleration | [m/ss] | | +| ✓ | float64 | target_steering | [rad] | | +| ✓ | float64 | target_steering_velocity | [rad/s] | | + +### /awapi/autoware/get/status + +- get autoware status +- MessageType: awapi_awiv_adapter/AwapiVehicleStatus + +| ✓ | type | name | unit | note | +| --- | :------------------------------------- | :---------------- | :--------------------------------------------- | :-------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| ✓ | std_msgs/Header | header | | | +| ✓ | string | autoware_state | | | +| ✓ | int32 | control_mode | accroding to autoware_vehicle_msgs/ControlMode | manual/auto (changed by /awapi/autoware/put/engage) | +| | int32 | gate_mode | autoware_vehicle_msgs/GateMode | auto/remote (it is valid only when control_mode=auto)) | +| ✓ | bool | emergency_stopped | True in emergency mode | | +| ✓ | autoware_planning_msgs/StopReasonArray | stop_reason | | "stop_pose" represents the position of "base_link" (not the head of the car) | +| ✓ | diagnostic_msgs/DiagnosticStatus[] | diagnostics | | output only diag. of leaf node (diag. of parent node are cut) | +| ✓ | bool | arrived_goal | | True if the autoware_state is changed from Driving to ArrivedGoal or WairingForRoute. False if the autoware_state is changed to WaitingForEngage or Driving. Default False. | + +### /awapi/autoware/get/route + +- get route +- MessageType: autoware_planning_msgs/Route + +| ✓ | type | name | unit | note | +| --- | :--------------------------- | :--- | :--- | :--- | +| ✓ | autoware_planning_msgs/Route | | | | + +### /awapi/prediction/get/objects + +- get predicted object +- MessageType: autoware_perception_msgs/DynamicObjectArray + +| ✓ | type | name | unit | note | +| --- | :------------------------------------------ | :--- | :--- | :--- | +| ✓ | autoware_perception_msgs/DynamicObjectArray | | | | + +### /awapi/lane_change/get/status + +- get lane change information +- MessageType: awapi_awiv_adapter/LaneChangeStatus + +| ✓ | type | name | unit | note | +| --- | :-------------------------- | :-------------------------- | :--------------------------------------- | :--------------------------------------------------------------------------------- | +| | std_msgs/Header | header | | | +| | bool | force_lane_change_available | True when lane change is avilable | available: Physically lane changeable state (do not consider other vehicle) | +| | bool | lane_change_ready | True when lane change is ready | ready: State that ego-vehicle can change lane without collision with other vehicle | +| | autoware_planning_msgs/Path | candidate_path | according to autoware_planning_msgs/Path | | + +### /awapi/object_avoidance/get/status + +- get obstacle avoidance information +- MessageType: awapi_awiv_adapter/ObstacleAvoidanceStatus + +| ✓ | type | name | unit | note | +| --- | :-------------------------------- | :----------------------- | :--------------------------------------------- | :---------------------------------------------------- | +| | std_msgs/Header | header | | | +| | bool | obstacle_avoidance_ready | True when obstacle avoidance is ready | | +| | autoware_planning_msgs/Trajectory | candidate_path | according to autoware_planning_msgs/Trajectory | Msg type is different from lane change candidate path | + +### /awapi/traffic_light/get/status + +- get recognition result of traffic light +- MessageType: autoware_perception_msgs/TrafficLightStateArray + +| ✓ | type | name | unit | note | +| --- | :---------------------------------------------- | :--- | :--- | :--- | +| | autoware_perception_msgs/TrafficLightStateArray | | | | + +## put topic + +### /awapi/vehicle/put/velocity + +- set upper velocity +- MessageType: std_msgs/Float32 + +| ✓ | type | name | unit | note | +| --- | :--------------- | :--- | :--- | :----------- | +| ✓ | std_msgs/Float32 | | | max velocity | + +### /awapi/autoware/put/gate_mode + +- send gate mode (auto/remote) +- MessageType: autoware_control_msgs/GateMode + +| ✓ | type | name | unit | note | +| --- | :----------------------------- | :--- | :--- | :--- | +| | autoware_control_msgs/GateMode | | | | + +### /awapi/autoware/put/emergency + +- send emergency signal +- MessageType: std_msgs/Bool +- **To enable this functionality, autoware have to be in the Remote Mode or set _/control/vehicle_cmd_gate/use_emergency_handling_ to true.** + +| ✓ | type | name | unit | note | +| --- | :------------ | :--- | :--- | :--- | +| ✓ | std_msgs/Bool | | | | + +### /awapi/autoware/put/engage + +- send engage signal (both of autoware/engage and vehicle/engage) +- MessageType: std_msgs/Bool + +| ✓ | type | name | unit | note | +| --- | :------------ | :--- | :--- | :--- | +| ✓ | std_msgs/Bool | | | | + +### /awapi/autoware/put/route + +- send goal pose +- MessageType: autoware_planning_msgs/Route + +| ✓ | type | name | unit | note | +| --- | :--------------------------- | :--- | :--- | :--- | +| ✓ | autoware_planning_msgs/Route | | | | + +### /awapi/lane_change/put/approval + +- send lane change approval flag +- send True: start lane change when **lane_change_ready** is true +- MessageType: std_msgs/Bool + +| ✓ | type | name | unit | note | +| --- | :------------ | :--- | :--- | :--- | +| | std_msgs/Bool | | | | + +### /awapi/lane_change/put/force + +- send force lane change flag +- send True: start lane change when **force_lane_change_available** is true +- MessageType: std_msgs/Bool + +| ✓ | type | name | unit | note | +| --- | :------------ | :--- | :--- | :--- | +| | std_msgs/Bool | | | | + +### /awapi/object_avoidance/put/approval + +- send object avoidance approval flag +- MessageType: std_msgs/Bool + +| ✓ | type | name | unit | note | +| --- | :------------ | :--- | :--- | :--- | +| | std_msgs/Bool | | | | + +### /awapi/object_avoidance/put/force + +- send force object avoidance flag +- **not implemented (Autoware does not have corresponded topic)** + +| ✓ | type | name | unit | note | +| --- | :--- | :--- | :--- | :--- | + + +### /awapi/traffic_light/put/traffic_light + +- Overwrite the recognition result of traffic light +- **not implemented (Autoware does not have corresponded topic)** + +| ✓ | type | name | unit | note | +| --- | :--- | :--- | :--- | :--- | + diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.h b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.h new file mode 100644 index 0000000000000..32a95428a4a35 --- /dev/null +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.h @@ -0,0 +1,76 @@ +/* + * Copyright 2020 Tier IV, Inc. All rights reserved. + * + * 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. + */ + +#include +#include + +namespace autoware_api +{ +class AutowareIvAutowareStatePublisher +{ +public: + AutowareIvAutowareStatePublisher(); + void statePublisher(const AutowareInfo & aw_info); + +private: + // node handle + ros::NodeHandle nh_; + ros::NodeHandle pnh_; + + // publisher + ros::Publisher pub_state_; + + // parameter + + /* parameter for judging goal now */ + bool arrived_goal_; + autoware_system_msgs::AutowareState::_state_type prev_state_; + + /* parameter for judging diag leaf */ + std::set diag_name_set_; + + void getAutowareStateInfo( + const autoware_system_msgs::AutowareState::ConstPtr & autoware_state_ptr, + autoware_api_msgs::AwapiAutowareStatus * status); + void getControlModeInfo( + const autoware_vehicle_msgs::ControlMode::ConstPtr & control_mode_ptr, + autoware_api_msgs::AwapiAutowareStatus * status); + void getGateModeInfo( + const autoware_control_msgs::GateMode::ConstPtr & gate_mode_ptr, + autoware_api_msgs::AwapiAutowareStatus * status); + void getIsEmergencyInfo( + const std_msgs::Bool::ConstPtr & is_emergency_ptr, + autoware_api_msgs::AwapiAutowareStatus * status); + void getStopReasonInfo( + const autoware_planning_msgs::StopReasonArray::ConstPtr & stop_reason_ptr, + autoware_api_msgs::AwapiAutowareStatus * status); + void getDiagInfo( + const diagnostic_msgs::DiagnosticArray::ConstPtr & diag_ptr, + autoware_api_msgs::AwapiAutowareStatus * status); + + void getGlobalRptInfo( + const pacmod_msgs::GlobalRpt::ConstPtr & global_rpt_ptr, + autoware_api_msgs::AwapiAutowareStatus * status); + + bool isGoal(const autoware_system_msgs::AutowareState::ConstPtr & autoware_state); + std::vector extractLeafDiag( + const std::vector & diag_vec); + std::string splitStringByLastSlash(const std::string & str); + void updateDiagNameSet(const std::vector & diag_vec); + bool isLeaf(const diagnostic_msgs::DiagnosticStatus & diag); +}; + +} // namespace autoware_api diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.h b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.h new file mode 100644 index 0000000000000..bf33ec2bd9655 --- /dev/null +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.h @@ -0,0 +1,102 @@ +/* + * Copyright 2020 Tier IV, Inc. All rights reserved. + * + * 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. + */ + +#pragma once + +#ifndef AWAPI_AUTOWARE_UTIL_H +#define AWAPI_AUTOWARE_UTIL_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace autoware_api +{ +struct AutowareInfo +{ + std::shared_ptr current_pose_ptr; + autoware_vehicle_msgs::Steering::ConstPtr steer_ptr; + autoware_vehicle_msgs::VehicleCommand::ConstPtr vehicle_cmd_ptr; + autoware_vehicle_msgs::TurnSignal::ConstPtr turn_signal_ptr; + geometry_msgs::TwistStamped::ConstPtr twist_ptr; + autoware_vehicle_msgs::ShiftStamped::ConstPtr gear_ptr; + std_msgs::Float32::ConstPtr battery_ptr; + sensor_msgs::NavSatFix::ConstPtr nav_sat_ptr; + autoware_system_msgs::AutowareState::ConstPtr autoware_state_ptr; + autoware_vehicle_msgs::ControlMode::ConstPtr control_mode_ptr; + autoware_control_msgs::GateMode::ConstPtr gate_mode_ptr; + std_msgs::Bool::ConstPtr is_emergency_ptr; + autoware_planning_msgs::StopReasonArray::ConstPtr stop_reason_ptr; + diagnostic_msgs::DiagnosticArray::ConstPtr diagnostic_ptr; + pacmod_msgs::GlobalRpt::ConstPtr global_rpt_ptr; + std_msgs::Bool::ConstPtr lane_change_available_ptr; + std_msgs::Bool::ConstPtr lane_change_ready_ptr; + autoware_planning_msgs::Path::ConstPtr lane_change_candidate_ptr; + std_msgs::Bool::ConstPtr obstacle_avoid_ready_ptr; + autoware_planning_msgs::Trajectory::ConstPtr obstacle_avoid_candidate_ptr; +}; + +template +T getParam(const ros::NodeHandle & nh, const std::string & key, const T & default_value) +{ + T value; + nh.param(key, value, default_value); + return value; +} + +template +T waitForParam(const ros::NodeHandle & nh, const std::string & key) +{ + T value; + ros::Rate rate(1.0); + + while (ros::ok()) { + const auto result = nh.getParam(key, value); + if (result) { + return value; + } + + ROS_WARN("waiting for parameter `%s` ...", key.c_str()); + rate.sleep(); + } + + return {}; +} + +double lowpass_filter(const double current_value, const double prev_value, const double gain); + +} // namespace autoware_api + +#endif // AWAPI_AUTOWARE_UTIL_H diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.h b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.h new file mode 100644 index 0000000000000..d6d26bbb536b9 --- /dev/null +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.h @@ -0,0 +1,102 @@ +/* + * Copyright 2020 Tier IV, Inc. All rights reserved. + * + * 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. + */ + +#include +#include +#include +#include +#include +#include + +namespace autoware_api +{ +class AutowareIvAdapter +{ +public: + AutowareIvAdapter(); + +private: + // node handle + ros::NodeHandle nh_; + ros::NodeHandle pnh_; + + // subscriber + ros::Subscriber sub_steer_; + ros::Subscriber sub_vehicle_cmd_; + ros::Subscriber sub_turn_signal_cmd_; + ros::Subscriber sub_twist_; + ros::Subscriber sub_gear_; + ros::Subscriber sub_battery_; + ros::Subscriber sub_nav_sat_; + ros::Subscriber sub_autoware_state_; + ros::Subscriber sub_control_mode_; + ros::Subscriber sub_gate_mode_; + ros::Subscriber sub_emergency_; + ros::Subscriber sub_stop_reason_; + ros::Subscriber sub_diagnostics_; + ros::Subscriber sub_global_rpt_; + ros::Subscriber sub_lane_change_available_; + ros::Subscriber sub_lane_change_ready_; + ros::Subscriber sub_lane_change_candidate_; + ros::Subscriber sub_obstacle_avoid_ready_; + ros::Subscriber sub_obstacle_avoid_candidate_; + // timer + ros::Timer timer_; + + // tf + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + + // callback function + void callbackSteer(const autoware_vehicle_msgs::Steering::ConstPtr & msg_ptr); + void callbackVehicleCmd(const autoware_vehicle_msgs::VehicleCommand::ConstPtr & msg_ptr); + void callbackTurnSignal(const autoware_vehicle_msgs::TurnSignal::ConstPtr & msg_ptr); + void callbackTwist(const geometry_msgs::TwistStamped::ConstPtr & msg_ptr); + void callbackGear(const autoware_vehicle_msgs::ShiftStamped::ConstPtr & msg_ptr); + void callbackBattery(const std_msgs::Float32::ConstPtr & msg_ptr); + void callbackNavSat(const sensor_msgs::NavSatFix::ConstPtr & msg_ptr); + void callbackAutowareState(const autoware_system_msgs::AutowareState::ConstPtr & msg_ptr); + void callbackControlMode(const autoware_vehicle_msgs::ControlMode::ConstPtr & msg_ptr); + void callbackGateMode(const autoware_control_msgs::GateMode::ConstPtr & msg_ptr); + void callbackIsEmergency(const std_msgs::Bool::ConstPtr & msg_ptr); + void callbackStopReason(const autoware_planning_msgs::StopReasonArray::ConstPtr & msg_ptr); + void callbackDiagnostics(const diagnostic_msgs::DiagnosticArray::ConstPtr & msg_ptr); + void callbackGlobalRpt(const pacmod_msgs::GlobalRpt::ConstPtr & msg_ptr); + void callbackLaneChangeAvailable(const std_msgs::Bool::ConstPtr & msg_ptr); + void callbackLaneChangeReady(const std_msgs::Bool::ConstPtr & msg_ptr); + void callbackLaneChangeCandidatePath(const autoware_planning_msgs::Path::ConstPtr & msg_ptr); + void callbackLaneObstacleAvoidReady(const std_msgs::Bool::ConstPtr & msg_ptr); + void callbackLaneObstacleAvoidCandidatePath( + const autoware_planning_msgs::Trajectory::ConstPtr & msg_ptr); + + // timer function + void timerCallback(const ros::TimerEvent & e); + + void emergencyParamCheck(const bool emergency_handling_param); + void getCurrentPose(); + + // parameter + AutowareInfo aw_info_; + std::unique_ptr vehicle_state_publisher_; + std::unique_ptr autoware_state_publisher_; + std::unique_ptr stop_reason_aggreagator_; + std::unique_ptr lane_change_state_publisher_; + std::unique_ptr obstacle_avoidance_state_publisher_; + double status_pub_hz_; + double stop_reason_timeout_; +}; + +} // namespace autoware_api diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_lane_change_state_publisher.h b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_lane_change_state_publisher.h new file mode 100644 index 0000000000000..56c30f4aa30cd --- /dev/null +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_lane_change_state_publisher.h @@ -0,0 +1,45 @@ +/* + * Copyright 2020 Tier IV, Inc. All rights reserved. + * + * 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. + */ + +#include +#include + +namespace autoware_api +{ +class AutowareIvLaneChangeStatePublisher +{ +public: + AutowareIvLaneChangeStatePublisher(); + void statePublisher(const AutowareInfo & aw_info); + +private: + // node handle + ros::NodeHandle nh_; + ros::NodeHandle pnh_; + + // publisher + ros::Publisher pub_state_; + + void getLaneChangeAvailableInfo( + const std_msgs::Bool::ConstPtr & available_ptr, autoware_api_msgs::LaneChangeStatus * status); + void getLaneChangeReadyInfo( + const std_msgs::Bool::ConstPtr & ready_ptr, autoware_api_msgs::LaneChangeStatus * status); + void getCandidatePathInfo( + const autoware_planning_msgs::Path::ConstPtr & path_ptr, + autoware_api_msgs::LaneChangeStatus * status); +}; + +} // namespace autoware_api diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_obstacle_avoidance_state_publisher.h b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_obstacle_avoidance_state_publisher.h new file mode 100644 index 0000000000000..f82ddb16dff07 --- /dev/null +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_obstacle_avoidance_state_publisher.h @@ -0,0 +1,44 @@ +/* + * Copyright 2020 Tier IV, Inc. All rights reserved. + * + * 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. + */ + +#include +#include + +namespace autoware_api +{ +class AutowareIvObstacleAvoidanceStatePublisher +{ +public: + AutowareIvObstacleAvoidanceStatePublisher(); + void statePublisher(const AutowareInfo & aw_info); + +private: + // node handle + ros::NodeHandle nh_; + ros::NodeHandle pnh_; + + // publisher + ros::Publisher pub_state_; + + void getObstacleAvoidReadyInfo( + const std_msgs::Bool::ConstPtr & ready_ptr, + autoware_api_msgs::ObstacleAvoidanceStatus * status); + void getCandidatePathInfo( + const autoware_planning_msgs::Trajectory::ConstPtr & path_ptr, + autoware_api_msgs::ObstacleAvoidanceStatus * status); +}; + +} // namespace autoware_api diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_stop_reason_aggregator.h b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_stop_reason_aggregator.h new file mode 100644 index 0000000000000..32e1f4abfcebf --- /dev/null +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_stop_reason_aggregator.h @@ -0,0 +1,45 @@ +/* + * Copyright 2020 Tier IV, Inc. All rights reserved. + * + * 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. + */ + +#pragma once + +#include + +namespace autoware_api +{ +class AutowareIvStopReasonAggregator +{ +public: + AutowareIvStopReasonAggregator(const double timeout); + autoware_planning_msgs::StopReasonArray::ConstPtr updateStopReasonArray( + const autoware_planning_msgs::StopReasonArray::ConstPtr & msg_ptr); + +private: + void applyUpdate(const autoware_planning_msgs::StopReasonArray::ConstPtr & msg_ptr); + bool checkMatchingReason( + const autoware_planning_msgs::StopReasonArray::ConstPtr & msg_stop_reason_array, + const autoware_planning_msgs::StopReasonArray & stop_reason_array); + void applyTimeOut(); + void appendStopReasonToArray( + const autoware_planning_msgs::StopReason & stop_reason, + autoware_planning_msgs::StopReasonArray * stop_reason_array); + autoware_planning_msgs::StopReasonArray::ConstPtr makeStopReasonArray(); + + double timeout_; + std::vector stop_reason_array_vec_; +}; + +} // namespace autoware_api diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_vehicle_state_publisher.h b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_vehicle_state_publisher.h new file mode 100644 index 0000000000000..e30f39ffcf604 --- /dev/null +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_vehicle_state_publisher.h @@ -0,0 +1,73 @@ +/* + * Copyright 2020 Tier IV, Inc. All rights reserved. + * + * 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. + */ + +#include +#include + +namespace autoware_api +{ +class AutowareIvVehicleStatePublisher +{ +public: + AutowareIvVehicleStatePublisher(); + void statePublisher(const AutowareInfo & aw_info); + +private: + // node handle + ros::NodeHandle nh_; + ros::NodeHandle pnh_; + + // publisher + ros::Publisher pub_state_; + + autoware_api_msgs::AwapiVehicleStatus initVehicleStatus(); + void getPoseInfo( + const std::shared_ptr & pose_ptr, + autoware_api_msgs::AwapiVehicleStatus * status); + void getSteerInfo( + const autoware_vehicle_msgs::Steering::ConstPtr & steer_ptr, + autoware_api_msgs::AwapiVehicleStatus * status); + void getVehicleCmdInfo( + const autoware_vehicle_msgs::VehicleCommand::ConstPtr & vehicle_cmd_ptr, + autoware_api_msgs::AwapiVehicleStatus * status); + void getTurnSignalInfo( + const autoware_vehicle_msgs::TurnSignal::ConstPtr & turn_signal_ptr, + autoware_api_msgs::AwapiVehicleStatus * status); + void getTwistInfo( + const geometry_msgs::TwistStamped::ConstPtr & twist_ptr, + autoware_api_msgs::AwapiVehicleStatus * status); + void getGearInfo( + const autoware_vehicle_msgs::ShiftStamped::ConstPtr & gear_ptr, + autoware_api_msgs::AwapiVehicleStatus * status); + void getBatteryInfo( + const std_msgs::Float32::ConstPtr & battery_ptr, + autoware_api_msgs::AwapiVehicleStatus * status); + void getGpsInfo( + const sensor_msgs::NavSatFix::ConstPtr & nav_sat_ptr, + autoware_api_msgs::AwapiVehicleStatus * status); + + //parameters + geometry_msgs::TwistStamped::ConstPtr previous_twist_ptr_; + autoware_vehicle_msgs::Steering::ConstPtr previous_steer_ptr_; + double prev_accel_; + double prev_steer_vel_; + + // defined value + const double accel_lowpass_gain_ = 0.2; + const double steer_vel_lowpass_gain_ = 0.2; +}; + +} // namespace autoware_api diff --git a/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch b/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch new file mode 100644 index 0000000000000..47c9be49b8557 --- /dev/null +++ b/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch @@ -0,0 +1,111 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/awapi/awapi_awiv_adapter/package.xml b/awapi/awapi_awiv_adapter/package.xml new file mode 100644 index 0000000000000..6909569c2b9b5 --- /dev/null +++ b/awapi/awapi_awiv_adapter/package.xml @@ -0,0 +1,25 @@ + + + awapi_awiv_adapter + 0.1.0 + The awapi_awiv_adapter package + Tomoya Kimura + Tomoya Kimura + BSD + + catkin + + roscpp + autoware_api_msgs + autoware_system_msgs + autoware_planning_msgs + autoware_control_msgs + autoware_vehicle_msgs + diagnostic_msgs + geometry_msgs + pacmod_msgs + std_msgs + sensor_msgs + tf2 + tf2_geometry_msgs + diff --git a/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp b/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp new file mode 100644 index 0000000000000..35cf84193c772 --- /dev/null +++ b/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp @@ -0,0 +1,208 @@ +/* + * Copyright 2020 Tier IV, Inc. All rights reserved. + * + * 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. + */ + +#include +#include + +namespace autoware_api +{ +AutowareIvAutowareStatePublisher::AutowareIvAutowareStatePublisher() +: nh_(), pnh_("~"), arrived_goal_(false) +{ + // publisher + pub_state_ = pnh_.advertise("output/autoware_status", 1); +} + +void AutowareIvAutowareStatePublisher::statePublisher(const AutowareInfo & aw_info) +{ + autoware_api_msgs::AwapiAutowareStatus status; + + //input header + status.header.frame_id = "base_link"; + status.header.stamp = ros::Time::now(); + + // get all info + getAutowareStateInfo(aw_info.autoware_state_ptr, &status); + getControlModeInfo(aw_info.control_mode_ptr, &status); + getGateModeInfo(aw_info.gate_mode_ptr, &status); + getIsEmergencyInfo(aw_info.is_emergency_ptr, &status); + getStopReasonInfo(aw_info.stop_reason_ptr, &status); + getDiagInfo(aw_info.diagnostic_ptr, &status); + getGlobalRptInfo(aw_info.global_rpt_ptr, &status); + + // publish info + pub_state_.publish(status); +} + +void AutowareIvAutowareStatePublisher::getAutowareStateInfo( + const autoware_system_msgs::AutowareState::ConstPtr & autoware_state_ptr, + autoware_api_msgs::AwapiAutowareStatus * status) +{ + if (!autoware_state_ptr) { + ROS_DEBUG_STREAM_THROTTLE(5.0, "[AutowareIvAutowareStatePublisher] autoware_state is nullptr"); + return; + } + + // get autoware_state + status->autoware_state = autoware_state_ptr->state; + status->arrived_goal = isGoal(autoware_state_ptr); +} + +void AutowareIvAutowareStatePublisher::getControlModeInfo( + const autoware_vehicle_msgs::ControlMode::ConstPtr & control_mode_ptr, + autoware_api_msgs::AwapiAutowareStatus * status) +{ + if (!control_mode_ptr) { + ROS_DEBUG_STREAM_THROTTLE(5.0, "[AutowareIvAutowareStatePublisher] control mode is nullptr"); + return; + } + + // get control mode + status->control_mode = control_mode_ptr->data; +} + +void AutowareIvAutowareStatePublisher::getGateModeInfo( + const autoware_control_msgs::GateMode::ConstPtr & gate_mode_ptr, + autoware_api_msgs::AwapiAutowareStatus * status) +{ + if (!gate_mode_ptr) { + ROS_DEBUG_STREAM_THROTTLE(5.0, "[AutowareIvAutowareStatePublisher] gate mode is nullptr"); + return; + } + + // get control mode + status->gate_mode = gate_mode_ptr->data; +} + +void AutowareIvAutowareStatePublisher::getIsEmergencyInfo( + const std_msgs::Bool::ConstPtr & is_emergency_ptr, + autoware_api_msgs::AwapiAutowareStatus * status) +{ + if (!is_emergency_ptr) { + ROS_DEBUG_STREAM_THROTTLE(5.0, "[AutowareIvAutowareStatePublisher] is_emergency is nullptr"); + return; + } + + // get emergency + status->emergency_stopped = is_emergency_ptr->data; +} + +void AutowareIvAutowareStatePublisher::getStopReasonInfo( + const autoware_planning_msgs::StopReasonArray::ConstPtr & stop_reason_ptr, + autoware_api_msgs::AwapiAutowareStatus * status) +{ + if (!stop_reason_ptr) { + ROS_DEBUG_STREAM_THROTTLE(5.0, "[AutowareIvAutowareStatePublisher] stop reason is nullptr"); + return; + } + + status->stop_reason = *stop_reason_ptr; +} + +void AutowareIvAutowareStatePublisher::getDiagInfo( + const diagnostic_msgs::DiagnosticArray::ConstPtr & diag_ptr, + autoware_api_msgs::AwapiAutowareStatus * status) +{ + if (!diag_ptr) { + ROS_DEBUG_STREAM_THROTTLE(5.0, "[AutowareIvAutowareStatePublisher] diagnostics is nullptr"); + return; + } + + // get diag + status->diagnostics = extractLeafDiag(diag_ptr->status); +} + +void AutowareIvAutowareStatePublisher::getGlobalRptInfo( + const pacmod_msgs::GlobalRpt::ConstPtr & global_rpt_ptr, + autoware_api_msgs::AwapiAutowareStatus * status) +{ + if (!global_rpt_ptr) { + ROS_DEBUG_STREAM_THROTTLE(5.0, "[AutowareIvAutowareStatePublisher] global_rpt is nullptr"); + return; + } + + // get global_rpt + status->autonomous_overriden = global_rpt_ptr->override_active; +} + +bool AutowareIvAutowareStatePublisher::isGoal( + const autoware_system_msgs::AutowareState::ConstPtr & autoware_state) +{ + //rename + const auto & aw_state = autoware_state->state; + + if (aw_state == autoware_system_msgs::AutowareState::ArrivedGoal) { + arrived_goal_ = true; + } else if ( + prev_state_ == autoware_system_msgs::AutowareState::Driving && + aw_state == autoware_system_msgs::AutowareState::WaitingForRoute) { + arrived_goal_ = true; + } + + if ( + aw_state == autoware_system_msgs::AutowareState::WaitingForEngage || + aw_state == autoware_system_msgs::AutowareState::Driving) { + //cancel goal state + arrived_goal_ = false; + } + + prev_state_ = aw_state; + + return arrived_goal_; +} + +std::vector AutowareIvAutowareStatePublisher::extractLeafDiag( + const std::vector & diag_vec) +{ + updateDiagNameSet(diag_vec); + + std::vector leaf_diag_info; + for (const auto diag : diag_vec) { + if (isLeaf(diag)) { + leaf_diag_info.emplace_back(diag); + } + } + return leaf_diag_info; +} + +std::string AutowareIvAutowareStatePublisher::splitStringByLastSlash(const std::string & str) +{ + const auto last_slash = str.find_last_of("/"); + + if (last_slash == std::string::npos) { + // if not find slash + return str; + } + + return str.substr(0, last_slash); +} + +void AutowareIvAutowareStatePublisher::updateDiagNameSet( + const std::vector & diag_vec) +{ + // set diag name to diag_name_set_ + for (const auto & diag : diag_vec) { + diag_name_set_.insert(splitStringByLastSlash(diag.name)); + } +} + +bool AutowareIvAutowareStatePublisher::isLeaf(const diagnostic_msgs::DiagnosticStatus & diag) +{ + //if not find diag.name in diag set, diag is leaf. + return diag_name_set_.find(diag.name) == diag_name_set_.end(); +} + +} // namespace autoware_api diff --git a/awapi/awapi_awiv_adapter/src/awapi_autoware_util.cpp b/awapi/awapi_awiv_adapter/src/awapi_autoware_util.cpp new file mode 100644 index 0000000000000..b6341ed8a5528 --- /dev/null +++ b/awapi/awapi_awiv_adapter/src/awapi_autoware_util.cpp @@ -0,0 +1,26 @@ +/* + * Copyright 2020 Tier IV, Inc. All rights reserved. + * + * 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. + */ + +#include + +namespace autoware_api +{ +double lowpass_filter(const double current_value, const double prev_value, const double gain) +{ + return gain * prev_value + (1.0 - gain) * current_value; +} + +} // namespace autoware_api \ No newline at end of file diff --git a/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp b/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp new file mode 100644 index 0000000000000..1ef8801a99449 --- /dev/null +++ b/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp @@ -0,0 +1,222 @@ +/* + * Copyright 2020 Tier IV, Inc. All rights reserved. + * + * 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. + */ + +#include + +namespace autoware_api +{ +AutowareIvAdapter::AutowareIvAdapter() : nh_(), pnh_("~"), tf_listener_(tf_buffer_) +{ + // get param + pnh_.param("status_pub_hz", status_pub_hz_, 5.0); + pnh_.param("stop_reason_timeout", stop_reason_timeout_, 0.5); + const bool em_handle_param = waitForParam(pnh_, "param/emergency_handling"); + emergencyParamCheck(em_handle_param); + + // setup instance + vehicle_state_publisher_ = std::make_unique(); + autoware_state_publisher_ = std::make_unique(); + stop_reason_aggreagator_ = std::make_unique(stop_reason_timeout_); + lane_change_state_publisher_ = std::make_unique(); + obstacle_avoidance_state_publisher_ = + std::make_unique(); + + // subscriber + sub_steer_ = pnh_.subscribe("input/steer", 1, &AutowareIvAdapter::callbackSteer, this); + sub_vehicle_cmd_ = + pnh_.subscribe("input/vehicle_cmd", 1, &AutowareIvAdapter::callbackVehicleCmd, this); + sub_turn_signal_cmd_ = + pnh_.subscribe("input/turn_signal", 1, &AutowareIvAdapter::callbackTurnSignal, this); + sub_twist_ = pnh_.subscribe("input/twist", 1, &AutowareIvAdapter::callbackTwist, this); + sub_gear_ = pnh_.subscribe("input/gear", 1, &AutowareIvAdapter::callbackGear, this); + sub_battery_ = pnh_.subscribe("input/battery", 1, &AutowareIvAdapter::callbackBattery, this); + sub_nav_sat_ = pnh_.subscribe("input/nav_sat", 1, &AutowareIvAdapter::callbackNavSat, this); + sub_autoware_state_ = + pnh_.subscribe("input/autoware_state", 1, &AutowareIvAdapter::callbackAutowareState, this); + sub_control_mode_ = + pnh_.subscribe("input/control_mode", 1, &AutowareIvAdapter::callbackControlMode, this); + sub_gate_mode_ = pnh_.subscribe("input/gate_mode", 1, &AutowareIvAdapter::callbackGateMode, this); + sub_emergency_ = + pnh_.subscribe("input/is_emergency", 1, &AutowareIvAdapter::callbackIsEmergency, this); + sub_stop_reason_ = + pnh_.subscribe("input/stop_reason", 100, &AutowareIvAdapter::callbackStopReason, this); + sub_diagnostics_ = + pnh_.subscribe("input/diagnostics", 1, &AutowareIvAdapter::callbackDiagnostics, this); + sub_global_rpt_ = + pnh_.subscribe("input/global_rpt", 1, &AutowareIvAdapter::callbackGlobalRpt, this); + sub_lane_change_available_ = pnh_.subscribe( + "input/lane_change_avaiable", 1, &AutowareIvAdapter::callbackLaneChangeAvailable, this); + sub_lane_change_ready_ = + pnh_.subscribe("input/lane_change_ready", 1, &AutowareIvAdapter::callbackLaneChangeReady, this); + sub_lane_change_candidate_ = pnh_.subscribe( + "input/lane_change_candidate_path", 1, &AutowareIvAdapter::callbackLaneChangeCandidatePath, + this); + sub_obstacle_avoid_ready_ = pnh_.subscribe( + "input/obstacle_avoid_ready", 1, &AutowareIvAdapter::callbackLaneObstacleAvoidReady, this); + sub_obstacle_avoid_candidate_ = pnh_.subscribe( + "input/obstacle_avoid_candidate_path", 1, + &AutowareIvAdapter::callbackLaneObstacleAvoidCandidatePath, this); + + // timer + timer_ = + nh_.createTimer(ros::Duration(1.0 / status_pub_hz_), &AutowareIvAdapter::timerCallback, this); +} + +void AutowareIvAdapter::emergencyParamCheck(const bool emergency_handling_param) +{ + if (!emergency_handling_param) { + ROS_WARN_STREAM("parameter[use_emergency_handling] is false."); + ROS_WARN_STREAM("autoware/put/emergency is not valid"); + } +} + +void AutowareIvAdapter::timerCallback(const ros::TimerEvent & e) +{ + // get current pose + getCurrentPose(); + + // publish vehicle state + vehicle_state_publisher_->statePublisher(aw_info_); + + // publish autoware state + autoware_state_publisher_->statePublisher(aw_info_); + + // publish lane change state + lane_change_state_publisher_->statePublisher(aw_info_); + + // publish obstacle_avoidance state + obstacle_avoidance_state_publisher_->statePublisher(aw_info_); +} + +void AutowareIvAdapter::callbackSteer(const autoware_vehicle_msgs::Steering::ConstPtr & msg_ptr) +{ + aw_info_.steer_ptr = msg_ptr; +} + +void AutowareIvAdapter::callbackVehicleCmd( + const autoware_vehicle_msgs::VehicleCommand::ConstPtr & msg_ptr) +{ + aw_info_.vehicle_cmd_ptr = msg_ptr; +} + +void AutowareIvAdapter::callbackTurnSignal( + const autoware_vehicle_msgs::TurnSignal::ConstPtr & msg_ptr) +{ + aw_info_.turn_signal_ptr = msg_ptr; +} + +void AutowareIvAdapter::callbackTwist(const geometry_msgs::TwistStamped::ConstPtr & msg_ptr) +{ + aw_info_.twist_ptr = msg_ptr; +} + +void AutowareIvAdapter::callbackGear(const autoware_vehicle_msgs::ShiftStamped::ConstPtr & msg_ptr) +{ + aw_info_.gear_ptr = msg_ptr; +} + +void AutowareIvAdapter::callbackBattery(const std_msgs::Float32::ConstPtr & msg_ptr) +{ + aw_info_.battery_ptr = msg_ptr; +} + +void AutowareIvAdapter::callbackNavSat(const sensor_msgs::NavSatFix::ConstPtr & msg_ptr) +{ + aw_info_.nav_sat_ptr = msg_ptr; +} + +void AutowareIvAdapter::getCurrentPose() +{ + try { + auto transform = tf_buffer_.lookupTransform("map", "base_link", ros::Time(0)); + geometry_msgs::PoseStamped ps; + ps.header = transform.header; + ps.pose.position.x = transform.transform.translation.x; + ps.pose.position.y = transform.transform.translation.y; + ps.pose.position.z = transform.transform.translation.z; + ps.pose.orientation = transform.transform.rotation; + aw_info_.current_pose_ptr = std::make_shared(ps); + } catch (tf2::TransformException & ex) { + ROS_DEBUG_STREAM_THROTTLE(2.0, "[awapi_autoware_adapter] cannot get self pose"); + } +} + +void AutowareIvAdapter::callbackAutowareState( + const autoware_system_msgs::AutowareState::ConstPtr & msg_ptr) +{ + aw_info_.autoware_state_ptr = msg_ptr; +} +void AutowareIvAdapter::callbackControlMode( + const autoware_vehicle_msgs::ControlMode::ConstPtr & msg_ptr) +{ + aw_info_.control_mode_ptr = msg_ptr; +} + +void AutowareIvAdapter::callbackGateMode(const autoware_control_msgs::GateMode::ConstPtr & msg_ptr) +{ + aw_info_.gate_mode_ptr = msg_ptr; +} + +void AutowareIvAdapter::callbackIsEmergency(const std_msgs::Bool::ConstPtr & msg_ptr) +{ + aw_info_.is_emergency_ptr = msg_ptr; +} + +void AutowareIvAdapter::callbackStopReason( + const autoware_planning_msgs::StopReasonArrayConstPtr & msg_ptr) +{ + aw_info_.stop_reason_ptr = stop_reason_aggreagator_->updateStopReasonArray(msg_ptr); +} + +void AutowareIvAdapter::callbackDiagnostics( + const diagnostic_msgs::DiagnosticArray::ConstPtr & msg_ptr) +{ + aw_info_.diagnostic_ptr = msg_ptr; +} + +void AutowareIvAdapter::callbackGlobalRpt(const pacmod_msgs::GlobalRpt::ConstPtr & msg_ptr) +{ + aw_info_.global_rpt_ptr = msg_ptr; +} + +void AutowareIvAdapter::callbackLaneChangeAvailable(const std_msgs::Bool::ConstPtr & msg_ptr) +{ + aw_info_.lane_change_available_ptr = msg_ptr; +} + +void AutowareIvAdapter::callbackLaneChangeReady(const std_msgs::Bool::ConstPtr & msg_ptr) +{ + aw_info_.lane_change_ready_ptr = msg_ptr; +} + +void AutowareIvAdapter::callbackLaneChangeCandidatePath( + const autoware_planning_msgs::Path::ConstPtr & msg_ptr) +{ + aw_info_.lane_change_candidate_ptr = msg_ptr; +} + +void AutowareIvAdapter::callbackLaneObstacleAvoidReady(const std_msgs::Bool::ConstPtr & msg_ptr) +{ + aw_info_.obstacle_avoid_ready_ptr = msg_ptr; +} + +void AutowareIvAdapter::callbackLaneObstacleAvoidCandidatePath( + const autoware_planning_msgs::Trajectory::ConstPtr & msg_ptr) +{ + aw_info_.obstacle_avoid_candidate_ptr = msg_ptr; +} + +} // namespace autoware_api diff --git a/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_node.cpp b/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_node.cpp new file mode 100644 index 0000000000000..a66c0a5773d3b --- /dev/null +++ b/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_node.cpp @@ -0,0 +1,30 @@ +/* + * Copyright 2020 Tier IV, Inc. All rights reserved. + * + * 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. + */ + +#include + +#include + +int main(int argc, char ** argv) +{ + ros::init(argc, argv, "awapi_awiv_adapter_node"); + + autoware_api::AutowareIvAdapter node; + + ros::spin(); + + return 0; +} \ No newline at end of file diff --git a/awapi/awapi_awiv_adapter/src/awapi_lane_change_state_publisher.cpp b/awapi/awapi_awiv_adapter/src/awapi_lane_change_state_publisher.cpp new file mode 100644 index 0000000000000..8a3f7f7fd1ca7 --- /dev/null +++ b/awapi/awapi_awiv_adapter/src/awapi_lane_change_state_publisher.cpp @@ -0,0 +1,85 @@ +/* + * Copyright 2020 Tier IV, Inc. All rights reserved. + * + * 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. + */ + +#include + +namespace autoware_api +{ +AutowareIvLaneChangeStatePublisher::AutowareIvLaneChangeStatePublisher() : nh_(), pnh_("~") +{ + // publisher + pub_state_ = pnh_.advertise("output/lane_change_status", 1); +} + +void AutowareIvLaneChangeStatePublisher::statePublisher(const AutowareInfo & aw_info) +{ + autoware_api_msgs::LaneChangeStatus status; + + //input header + status.header.frame_id = "base_link"; + status.header.stamp = ros::Time::now(); + + // get all info + getLaneChangeAvailableInfo(aw_info.lane_change_available_ptr, &status); + getLaneChangeReadyInfo(aw_info.lane_change_ready_ptr, &status); + getCandidatePathInfo(aw_info.lane_change_candidate_ptr, &status); + + // publish info + pub_state_.publish(status); +} + +void AutowareIvLaneChangeStatePublisher::getLaneChangeAvailableInfo( + const std_msgs::Bool::ConstPtr & available_ptr, autoware_api_msgs::LaneChangeStatus * status) +{ + if (!available_ptr) { + ROS_DEBUG_STREAM_THROTTLE( + 5.0, "[AutowareIvLaneChangeStatePublisher] lane change available is nullptr"); + return; + } + + // get lane change available info + status->force_lane_change_available = available_ptr->data; +} + +void AutowareIvLaneChangeStatePublisher::getLaneChangeReadyInfo( + const std_msgs::Bool::ConstPtr & ready_ptr, autoware_api_msgs::LaneChangeStatus * status) +{ + if (!ready_ptr) { + ROS_DEBUG_STREAM_THROTTLE( + 5.0, "[AutowareIvLaneChangeStatePublisher] lane change ready is nullptr"); + return; + } + + // get lane change available info + status->lane_change_ready = ready_ptr->data; +} + +void AutowareIvLaneChangeStatePublisher::getCandidatePathInfo( + const autoware_planning_msgs::Path::ConstPtr & path_ptr, + autoware_api_msgs::LaneChangeStatus * status) +{ + if (!path_ptr) { + ROS_DEBUG_STREAM_THROTTLE( + 5.0, + "[AutowareIvLaneChangeStatePublisher] lane_change_candidate_path is " + "nullptr"); + return; + } + + status->candidate_path = *path_ptr; +} + +} // namespace autoware_api diff --git a/awapi/awapi_awiv_adapter/src/awapi_obstacle_avoidance_state_publisher.cpp b/awapi/awapi_awiv_adapter/src/awapi_obstacle_avoidance_state_publisher.cpp new file mode 100644 index 0000000000000..4a00761d72fe6 --- /dev/null +++ b/awapi/awapi_awiv_adapter/src/awapi_obstacle_avoidance_state_publisher.cpp @@ -0,0 +1,72 @@ +/* + * Copyright 2020 Tier IV, Inc. All rights reserved. + * + * 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. + */ + +#include + +namespace autoware_api +{ +AutowareIvObstacleAvoidanceStatePublisher::AutowareIvObstacleAvoidanceStatePublisher() +: nh_(), pnh_("~") +{ + // publisher + pub_state_ = + pnh_.advertise("output/obstacle_avoid_status", 1); +} + +void AutowareIvObstacleAvoidanceStatePublisher::statePublisher(const AutowareInfo & aw_info) +{ + autoware_api_msgs::ObstacleAvoidanceStatus status; + + //input header + status.header.frame_id = "base_link"; + status.header.stamp = ros::Time::now(); + + // get all info + getObstacleAvoidReadyInfo(aw_info.obstacle_avoid_ready_ptr, &status); + getCandidatePathInfo(aw_info.obstacle_avoid_candidate_ptr, &status); + + // publish info + pub_state_.publish(status); +} + +void AutowareIvObstacleAvoidanceStatePublisher::getObstacleAvoidReadyInfo( + const std_msgs::Bool::ConstPtr & ready_ptr, autoware_api_msgs::ObstacleAvoidanceStatus * status) +{ + if (!ready_ptr) { + ROS_DEBUG_STREAM_THROTTLE( + 5.0, "[AutowareIvObstacleAvoidanceStatePublisher] obstacle_avoidance_ready is nullptr"); + return; + } + + status->obstacle_avoidance_ready = ready_ptr->data; +} + +void AutowareIvObstacleAvoidanceStatePublisher::getCandidatePathInfo( + const autoware_planning_msgs::Trajectory::ConstPtr & path_ptr, + autoware_api_msgs::ObstacleAvoidanceStatus * status) +{ + if (!path_ptr) { + ROS_DEBUG_STREAM_THROTTLE( + 5.0, + "[AutowareIvObstacleAvoidanceStatePublisher] obstacle_avoidance_candidate_path is " + "nullptr"); + return; + } + + status->candidate_path = *path_ptr; +} + +} // namespace autoware_api diff --git a/awapi/awapi_awiv_adapter/src/awapi_stop_reason_aggregator.cpp b/awapi/awapi_awiv_adapter/src/awapi_stop_reason_aggregator.cpp new file mode 100644 index 0000000000000..07396e51f8c31 --- /dev/null +++ b/awapi/awapi_awiv_adapter/src/awapi_stop_reason_aggregator.cpp @@ -0,0 +1,134 @@ +/* + * Copyright 2020 Tier IV, Inc. All rights reserved. + * + * 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. + */ + +#include + +namespace autoware_api +{ +AutowareIvStopReasonAggregator::AutowareIvStopReasonAggregator(const double timeout) +: timeout_(timeout) +{ +} + +autoware_planning_msgs::StopReasonArray::ConstPtr +AutowareIvStopReasonAggregator::updateStopReasonArray( + const autoware_planning_msgs::StopReasonArray::ConstPtr & msg_ptr) +{ + applyUpdate(msg_ptr); + applyTimeOut(); + return makeStopReasonArray(); +} + +void AutowareIvStopReasonAggregator::applyUpdate( + const autoware_planning_msgs::StopReasonArray::ConstPtr & msg_ptr) +{ + /* remove old stop_reason that matches reason with received msg */ + //make reason-matching msg list + + std::vector remove_idx; + if (!stop_reason_array_vec_.empty()) { + for (int i = stop_reason_array_vec_.size() - 1; i >= 0; i--) { + if (checkMatchingReason(msg_ptr, stop_reason_array_vec_.at(i))) { + remove_idx.emplace_back(i); + } + } + } + + // remove reason matching msg + for (const auto idx : remove_idx) { + stop_reason_array_vec_.erase(stop_reason_array_vec_.begin() + idx); + } + + // add new reason msg + stop_reason_array_vec_.emplace_back(*msg_ptr); +} + +bool AutowareIvStopReasonAggregator::checkMatchingReason( + const autoware_planning_msgs::StopReasonArray::ConstPtr & msg_stop_reason_array, + const autoware_planning_msgs::StopReasonArray & stop_reason_array) +{ + for (const auto msg_stop_reason : msg_stop_reason_array->stop_reasons) { + for (const auto stop_reason : stop_reason_array.stop_reasons) { + if (msg_stop_reason.reason == stop_reason.reason) { + //find matching reason + return true; + } + } + } + // cannot find matching reason + return false; +} + +void AutowareIvStopReasonAggregator::applyTimeOut() +{ + const auto current_time = ros::Time::now(); + + //make timeout-msg list + std::vector remove_idx; + if (!stop_reason_array_vec_.empty()) { + for (int i = stop_reason_array_vec_.size() - 1; i >= 0; i--) { + if ((current_time - stop_reason_array_vec_.at(i).header.stamp).toSec() > timeout_) { + remove_idx.emplace_back(i); + } + } + } + //remove timeout-msg + for (const auto idx : remove_idx) { + stop_reason_array_vec_.erase(stop_reason_array_vec_.begin() + idx); + } +} + +void AutowareIvStopReasonAggregator::appendStopReasonToArray( + const autoware_planning_msgs::StopReason & stop_reason, + autoware_planning_msgs::StopReasonArray * stop_reason_array) +{ + //if stop factors is empty, not append + if (stop_reason.stop_factors.empty()) { + return; + } + + //if already exists same reason msg in stop_reason_array_msg, append stop_factors to there + for (int i = 0; i < stop_reason_array->stop_reasons.size(); i++) { + if (stop_reason_array->stop_reasons.at(i).reason == stop_reason.reason) { + stop_reason_array->stop_reasons.at(i).stop_factors.insert( + stop_reason_array->stop_reasons.at(i).stop_factors.end(), stop_reason.stop_factors.begin(), + stop_reason.stop_factors.end()); + return; + } + } + + //if not exist same reason msg, append new stop reason + stop_reason_array->stop_reasons.emplace_back(stop_reason); +} + +autoware_planning_msgs::StopReasonArray::ConstPtr +AutowareIvStopReasonAggregator::makeStopReasonArray() +{ + autoware_planning_msgs::StopReasonArray stop_reason_array_msg; + // input header + stop_reason_array_msg.header.frame_id = "map"; + stop_reason_array_msg.header.stamp = ros::Time::now(); + + // input stop reason + for (const auto stop_reason_array : stop_reason_array_vec_) { + for (const auto stop_reason : stop_reason_array.stop_reasons) { + appendStopReasonToArray(stop_reason, &stop_reason_array_msg); + } + } + return boost::make_shared(stop_reason_array_msg); +} + +} // namespace autoware_api \ No newline at end of file diff --git a/awapi/awapi_awiv_adapter/src/awapi_vehicle_state_publisher.cpp b/awapi/awapi_awiv_adapter/src/awapi_vehicle_state_publisher.cpp new file mode 100644 index 0000000000000..0d6fc747d60b0 --- /dev/null +++ b/awapi/awapi_awiv_adapter/src/awapi_vehicle_state_publisher.cpp @@ -0,0 +1,206 @@ +/* + * Copyright 2020 Tier IV, Inc. All rights reserved. + * + * 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. + */ + +#include + +namespace autoware_api +{ +AutowareIvVehicleStatePublisher::AutowareIvVehicleStatePublisher() +: nh_(), pnh_("~"), prev_accel_(0.0) +{ + // publisher + pub_state_ = pnh_.advertise("output/vehicle_status", 1); +} + +void AutowareIvVehicleStatePublisher::statePublisher(const AutowareInfo & aw_info) +{ + autoware_api_msgs::AwapiVehicleStatus status = initVehicleStatus(); + + //input header + status.header.frame_id = "base_link"; + status.header.stamp = ros::Time::now(); + + // get all info + getPoseInfo(aw_info.current_pose_ptr, &status); + getSteerInfo(aw_info.steer_ptr, &status); + getVehicleCmdInfo(aw_info.vehicle_cmd_ptr, &status); + getTurnSignalInfo(aw_info.turn_signal_ptr, &status); + getTwistInfo(aw_info.twist_ptr, &status); + getGearInfo(aw_info.gear_ptr, &status); + getBatteryInfo(aw_info.battery_ptr, &status); + getGpsInfo(aw_info.nav_sat_ptr, &status); + + // publish info + pub_state_.publish(status); +} + +autoware_api_msgs::AwapiVehicleStatus AutowareIvVehicleStatePublisher::initVehicleStatus() +{ + autoware_api_msgs::AwapiVehicleStatus status; + // set default value + if (std::numeric_limits::has_quiet_NaN) { + status.energy_level = std::numeric_limits::quiet_NaN(); + } + return status; +} + +void AutowareIvVehicleStatePublisher::getPoseInfo( + const std::shared_ptr & pose_ptr, + autoware_api_msgs::AwapiVehicleStatus * status) +{ + if (!pose_ptr) { + ROS_DEBUG_STREAM_THROTTLE(5.0, "[AutowareIvVehicleStatePublisher] current pose is nullptr"); + return; + } + + // get pose + status->pose = pose_ptr->pose; + + // convert quaternion to euler + double yaw, pitch, roll; + tf2::getEulerYPR(pose_ptr->pose.orientation, yaw, pitch, roll); + status->eulerangle.yaw = yaw; + status->eulerangle.pitch = pitch; + status->eulerangle.roll = roll; +} + +void AutowareIvVehicleStatePublisher::getSteerInfo( + const autoware_vehicle_msgs::Steering::ConstPtr & steer_ptr, + autoware_api_msgs::AwapiVehicleStatus * status) +{ + if (!steer_ptr) { + ROS_DEBUG_STREAM_THROTTLE(5.0, "[AutowareIvVehicleStatePublisher] steer is nullptr"); + return; + } + + // get steer + status->steering = steer_ptr->data; + + // get steer vel + if (previous_steer_ptr_) { + //calculate steer vel from steer + const double ds = steer_ptr->data - previous_steer_ptr_->data; + const double dt = + std::max((steer_ptr->header.stamp - previous_steer_ptr_->header.stamp).toSec(), 1e-03); + const double steer_vel = ds / dt; + + //apply lowpass filter + const double lowpass_steer = + lowpass_filter(steer_vel, prev_steer_vel_, steer_vel_lowpass_gain_); + prev_steer_vel_ = lowpass_steer; + status->steering_velocity = lowpass_steer; + } + previous_steer_ptr_ = steer_ptr; +} +void AutowareIvVehicleStatePublisher::getVehicleCmdInfo( + const autoware_vehicle_msgs::VehicleCommand::ConstPtr & vehicle_cmd_ptr, + autoware_api_msgs::AwapiVehicleStatus * status) +{ + if (!vehicle_cmd_ptr) { + ROS_DEBUG_STREAM_THROTTLE(5.0, "[AutowareIvVehicleStatePublisher] vehicle cmd is nullptr"); + return; + } + + // get command + status->target_acceleration = vehicle_cmd_ptr->control.acceleration; + status->target_velocity = vehicle_cmd_ptr->control.velocity; + status->target_steering = vehicle_cmd_ptr->control.steering_angle; + status->target_steering_velocity = vehicle_cmd_ptr->control.steering_angle_velocity; +} + +void AutowareIvVehicleStatePublisher::getTurnSignalInfo( + const autoware_vehicle_msgs::TurnSignal::ConstPtr & turn_signal_ptr, + autoware_api_msgs::AwapiVehicleStatus * status) +{ + if (!turn_signal_ptr) { + ROS_DEBUG_STREAM_THROTTLE(5.0, "[AutowareIvVehicleStatePublisher] turn signal is nullptr"); + return; + } + + //get turn singnal + status->turn_signal = turn_signal_ptr->data; +} + +void AutowareIvVehicleStatePublisher::getTwistInfo( + const geometry_msgs::TwistStamped::ConstPtr & twist_ptr, + autoware_api_msgs::AwapiVehicleStatus * status) +{ + if (!twist_ptr) { + ROS_DEBUG_STREAM_THROTTLE(5.0, "[AutowareIvVehicleStatePublisher] twist is nullptr"); + return; + } + + // get twist + status->velocity = twist_ptr->twist.linear.x; + status->angular_velocity = twist_ptr->twist.angular.z; + + // get accel + if (previous_twist_ptr_) { + //calculate accleration from velocity + const double dv = twist_ptr->twist.linear.x - previous_twist_ptr_->twist.linear.x; + const double dt = + std::max((twist_ptr->header.stamp - previous_twist_ptr_->header.stamp).toSec(), 1e-03); + const double accel = dv / dt; + + // apply lowpass filter + const double lowpass_accel = lowpass_filter(accel, prev_accel_, accel_lowpass_gain_); + prev_accel_ = lowpass_accel; + status->acceleration = lowpass_accel; + } + previous_twist_ptr_ = twist_ptr; +} + +void AutowareIvVehicleStatePublisher::getGearInfo( + const autoware_vehicle_msgs::ShiftStamped::ConstPtr & gear_ptr, + autoware_api_msgs::AwapiVehicleStatus * status) +{ + if (!gear_ptr) { + ROS_DEBUG_STREAM_THROTTLE(5.0, "[AutowareIvVehicleStatePublisher] gear is nullptr"); + return; + } + + // get gear (shift) + status->gear = gear_ptr->shift.data; +} + +void AutowareIvVehicleStatePublisher::getBatteryInfo( + const std_msgs::Float32::ConstPtr & battery_ptr, autoware_api_msgs::AwapiVehicleStatus * status) +{ + if (!battery_ptr) { + ROS_DEBUG_STREAM_THROTTLE(5.0, "[AutowareIvVehicleStatePublisher] battery is nullptr"); + return; + } + + // get battery + status->energy_level = battery_ptr->data; +} + +void AutowareIvVehicleStatePublisher::getGpsInfo( + const sensor_msgs::NavSatFix::ConstPtr & nav_sat_ptr, + autoware_api_msgs::AwapiVehicleStatus * status) +{ + if (!nav_sat_ptr) { + ROS_DEBUG_STREAM_THROTTLE(5.0, "[AutowareIvVehicleStatePublisher] nav_sat(gps) is nullptr"); + return; + } + + // get position (latlon) + status->latlon.lat = nav_sat_ptr->latitude; + status->latlon.lon = nav_sat_ptr->longitude; + status->latlon.alt = nav_sat_ptr->altitude; +} + +} // namespace autoware_api From d7224e8bdfb0893a9109aa5be7a958158c305a10 Mon Sep 17 00:00:00 2001 From: mitsudome-r Date: Thu, 8 Oct 2020 17:27:14 +0900 Subject: [PATCH 15/71] add COLCON_IGNORE to ros1 packages Signed-off-by: mitsudome-r Signed-off-by: tanaka3 --- awapi/awapi_awiv_adapter/COLCON_IGNORE | 0 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 awapi/awapi_awiv_adapter/COLCON_IGNORE diff --git a/awapi/awapi_awiv_adapter/COLCON_IGNORE b/awapi/awapi_awiv_adapter/COLCON_IGNORE new file mode 100644 index 0000000000000..e69de29bb2d1d From d79c4f9c856ae1c9a0cbf951022140aa9507a359 Mon Sep 17 00:00:00 2001 From: Nikolai Morin Date: Thu, 15 Oct 2020 14:28:52 +0200 Subject: [PATCH 16/71] Rename launch files to launch.xml (#28) Signed-off-by: tanaka3 --- .../{awapi_awiv_adapter.launch => awapi_awiv_adapter.launch.xml} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename awapi/awapi_awiv_adapter/launch/{awapi_awiv_adapter.launch => awapi_awiv_adapter.launch.xml} (100%) diff --git a/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch b/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml similarity index 100% rename from awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch rename to awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml From db77d4d2902e080deda6521fa4a38888064f55e6 Mon Sep 17 00:00:00 2001 From: Nikolai Morin Date: Mon, 16 Nov 2020 14:32:28 +0100 Subject: [PATCH 17/71] Port awapi_awiv_adapter to ROS2 (#51) * Port autoware_api_msgs to ROS2 * Integrate topic_tools * Don't inherit from Node, convert to milliseconds * Remove _node suffix from non-node loggers Signed-off-by: tanaka3 --- awapi/awapi_awiv_adapter/CMakeLists.txt | 67 ++----- awapi/awapi_awiv_adapter/COLCON_IGNORE | 0 awapi/awapi_awiv_adapter/Readme.md | 16 +- .../awapi_autoware_state_publisher.h | 55 +++--- .../awapi_awiv_adapter/awapi_autoware_util.h | 104 ++++------- .../awapi_awiv_adapter_core.h | 108 ++++++----- .../awapi_lane_change_state_publisher.h | 23 +-- ...awapi_obstacle_avoidance_state_publisher.h | 21 +-- .../awapi_stop_reason_aggregator.h | 24 +-- .../awapi_vehicle_state_publisher.h | 53 +++--- .../launch/awapi_awiv_adapter.launch.xml | 133 +++++++++----- awapi/awapi_awiv_adapter/package.xml | 12 +- .../src/awapi_autoware_state_publisher.cpp | 83 +++++---- .../src/awapi_awiv_adapter_core.cpp | 167 ++++++++++-------- .../src/awapi_awiv_adapter_node.cpp | 10 +- .../src/awapi_lane_change_state_publisher.cpp | 38 ++-- ...api_obstacle_avoidance_state_publisher.cpp | 33 ++-- .../src/awapi_stop_reason_aggregator.cpp | 36 ++-- .../src/awapi_vehicle_state_publisher.cpp | 78 ++++---- 19 files changed, 561 insertions(+), 500 deletions(-) delete mode 100644 awapi/awapi_awiv_adapter/COLCON_IGNORE diff --git a/awapi/awapi_awiv_adapter/CMakeLists.txt b/awapi/awapi_awiv_adapter/CMakeLists.txt index e5f36e0a12a31..3bdd783e82998 100644 --- a/awapi/awapi_awiv_adapter/CMakeLists.txt +++ b/awapi/awapi_awiv_adapter/CMakeLists.txt @@ -1,36 +1,17 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(awapi_awiv_adapter) -add_compile_options(-std=c++14) - -find_package(catkin REQUIRED COMPONENTS - message_generation - message_runtime - roscpp - autoware_api_msgs - autoware_system_msgs - autoware_planning_msgs - autoware_control_msgs - autoware_vehicle_msgs - diagnostic_msgs - geometry_msgs - pacmod_msgs - std_msgs - sensor_msgs - tf2 - tf2_geometry_msgs -) - -catkin_package( - INCLUDE_DIRS include -) +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wno-unused-parameter -Wall -Wextra -Wpedantic -Werror) +endif() -include_directories( - include - ${catkin_INCLUDE_DIRS} -) +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() -add_executable(awapi_awiv_adapter +ament_auto_add_executable(awapi_awiv_adapter src/awapi_awiv_adapter_node.cpp src/awapi_awiv_adapter_core.cpp src/awapi_vehicle_state_publisher.cpp @@ -41,30 +22,4 @@ add_executable(awapi_awiv_adapter src/awapi_autoware_util.cpp ) -target_link_libraries(awapi_awiv_adapter - ${catkin_LIBRARIES} -) - -add_dependencies(awapi_awiv_adapter - ${catkin_EXPORTED_TARGETS} -) - -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) - -install( - DIRECTORY - launch - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install(TARGETS awapi_awiv_adapter - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -if(CATKIN_ENABLE_TESTING) - find_package(rostest REQUIRED) -endif() +ament_auto_package(INSTALL_TO_SHARE launch) diff --git a/awapi/awapi_awiv_adapter/COLCON_IGNORE b/awapi/awapi_awiv_adapter/COLCON_IGNORE deleted file mode 100644 index e69de29bb2d1d..0000000000000 diff --git a/awapi/awapi_awiv_adapter/Readme.md b/awapi/awapi_awiv_adapter/Readme.md index ad534f2ec4a2c..febbb275a59bc 100644 --- a/awapi/awapi_awiv_adapter/Readme.md +++ b/awapi/awapi_awiv_adapter/Readme.md @@ -117,21 +117,21 @@ ### /awapi/autoware/put/emergency - send emergency signal -- MessageType: std_msgs/Bool +- MessageType: autoware_control_msgs/EmergencyMode - **To enable this functionality, autoware have to be in the Remote Mode or set _/control/vehicle_cmd_gate/use_emergency_handling_ to true.** -| ✓ | type | name | unit | note | -| --- | :------------ | :--- | :--- | :--- | -| ✓ | std_msgs/Bool | | | | +| ✓ | type | name | unit | note | +| --- | :---------------------------------- | :--- | :--- | :--- | +| ✓ | autoware_control_msgs/EmergencyMode | | | | ### /awapi/autoware/put/engage - send engage signal (both of autoware/engage and vehicle/engage) -- MessageType: std_msgs/Bool +- MessageType: autoware_control_msgs/EngageMode -| ✓ | type | name | unit | note | -| --- | :------------ | :--- | :--- | :--- | -| ✓ | std_msgs/Bool | | | | +| ✓ | type | name | unit | note | +| --- | :------------------------------- | :--- | :--- | :--- | +| ✓ | autoware_control_msgs/EngageMode | | | | ### /awapi/autoware/put/route diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.h b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.h index 32a95428a4a35..ad39da59c6a85 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.h +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.h @@ -14,63 +14,62 @@ * limitations under the License. */ -#include +#include + #include +#include namespace autoware_api { class AutowareIvAutowareStatePublisher { public: - AutowareIvAutowareStatePublisher(); + AutowareIvAutowareStatePublisher(rclcpp::Node& node); void statePublisher(const AutowareInfo & aw_info); private: - // node handle - ros::NodeHandle nh_; - ros::NodeHandle pnh_; - + rclcpp::Logger logger_; + rclcpp::Clock::SharedPtr clock_; // publisher - ros::Publisher pub_state_; + rclcpp::Publisher::SharedPtr pub_state_; // parameter /* parameter for judging goal now */ bool arrived_goal_; - autoware_system_msgs::AutowareState::_state_type prev_state_; + autoware_system_msgs::msg::AutowareState::_state_type prev_state_; /* parameter for judging diag leaf */ std::set diag_name_set_; void getAutowareStateInfo( - const autoware_system_msgs::AutowareState::ConstPtr & autoware_state_ptr, - autoware_api_msgs::AwapiAutowareStatus * status); + const autoware_system_msgs::msg::AutowareState::ConstSharedPtr & autoware_state_ptr, + autoware_api_msgs::msg::AwapiAutowareStatus * status); void getControlModeInfo( - const autoware_vehicle_msgs::ControlMode::ConstPtr & control_mode_ptr, - autoware_api_msgs::AwapiAutowareStatus * status); + const autoware_vehicle_msgs::msg::ControlMode::ConstSharedPtr & control_mode_ptr, + autoware_api_msgs::msg::AwapiAutowareStatus * status); void getGateModeInfo( - const autoware_control_msgs::GateMode::ConstPtr & gate_mode_ptr, - autoware_api_msgs::AwapiAutowareStatus * status); + const autoware_control_msgs::msg::GateMode::ConstSharedPtr & gate_mode_ptr, + autoware_api_msgs::msg::AwapiAutowareStatus * status); void getIsEmergencyInfo( - const std_msgs::Bool::ConstPtr & is_emergency_ptr, - autoware_api_msgs::AwapiAutowareStatus * status); + const autoware_control_msgs::msg::EmergencyMode::ConstSharedPtr & is_emergency_ptr, + autoware_api_msgs::msg::AwapiAutowareStatus * status); void getStopReasonInfo( - const autoware_planning_msgs::StopReasonArray::ConstPtr & stop_reason_ptr, - autoware_api_msgs::AwapiAutowareStatus * status); + const autoware_planning_msgs::msg::StopReasonArray::ConstSharedPtr & stop_reason_ptr, + autoware_api_msgs::msg::AwapiAutowareStatus * status); void getDiagInfo( - const diagnostic_msgs::DiagnosticArray::ConstPtr & diag_ptr, - autoware_api_msgs::AwapiAutowareStatus * status); - + const diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr & diag_ptr, + autoware_api_msgs::msg::AwapiAutowareStatus * status); void getGlobalRptInfo( - const pacmod_msgs::GlobalRpt::ConstPtr & global_rpt_ptr, - autoware_api_msgs::AwapiAutowareStatus * status); + const pacmod_msgs::msg::GlobalRpt::ConstSharedPtr & global_rpt_ptr, + autoware_api_msgs::msg::AwapiAutowareStatus * status); - bool isGoal(const autoware_system_msgs::AutowareState::ConstPtr & autoware_state); - std::vector extractLeafDiag( - const std::vector & diag_vec); + bool isGoal(const autoware_system_msgs::msg::AutowareState::ConstSharedPtr & autoware_state); + std::vector extractLeafDiag( + const std::vector & diag_vec); std::string splitStringByLastSlash(const std::string & str); - void updateDiagNameSet(const std::vector & diag_vec); - bool isLeaf(const diagnostic_msgs::DiagnosticStatus & diag); + void updateDiagNameSet(const std::vector & diag_vec); + bool isLeaf(const diagnostic_msgs::msg::DiagnosticStatus & diag); }; } // namespace autoware_api diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.h b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.h index bf33ec2bd9655..8e7c38282b16b 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.h +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.h @@ -19,82 +19,56 @@ #ifndef AWAPI_AUTOWARE_UTIL_H #define AWAPI_AUTOWARE_UTIL_H -#include -#include -#include -#include -#include -#include -#include #include #include #include +#include +#include +#include +#include +#include +#include +#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include namespace autoware_api { struct AutowareInfo { - std::shared_ptr current_pose_ptr; - autoware_vehicle_msgs::Steering::ConstPtr steer_ptr; - autoware_vehicle_msgs::VehicleCommand::ConstPtr vehicle_cmd_ptr; - autoware_vehicle_msgs::TurnSignal::ConstPtr turn_signal_ptr; - geometry_msgs::TwistStamped::ConstPtr twist_ptr; - autoware_vehicle_msgs::ShiftStamped::ConstPtr gear_ptr; - std_msgs::Float32::ConstPtr battery_ptr; - sensor_msgs::NavSatFix::ConstPtr nav_sat_ptr; - autoware_system_msgs::AutowareState::ConstPtr autoware_state_ptr; - autoware_vehicle_msgs::ControlMode::ConstPtr control_mode_ptr; - autoware_control_msgs::GateMode::ConstPtr gate_mode_ptr; - std_msgs::Bool::ConstPtr is_emergency_ptr; - autoware_planning_msgs::StopReasonArray::ConstPtr stop_reason_ptr; - diagnostic_msgs::DiagnosticArray::ConstPtr diagnostic_ptr; - pacmod_msgs::GlobalRpt::ConstPtr global_rpt_ptr; - std_msgs::Bool::ConstPtr lane_change_available_ptr; - std_msgs::Bool::ConstPtr lane_change_ready_ptr; - autoware_planning_msgs::Path::ConstPtr lane_change_candidate_ptr; - std_msgs::Bool::ConstPtr obstacle_avoid_ready_ptr; - autoware_planning_msgs::Trajectory::ConstPtr obstacle_avoid_candidate_ptr; + std::shared_ptr current_pose_ptr; + autoware_vehicle_msgs::msg::Steering::ConstSharedPtr steer_ptr; + autoware_vehicle_msgs::msg::VehicleCommand::ConstSharedPtr vehicle_cmd_ptr; + autoware_vehicle_msgs::msg::TurnSignal::ConstSharedPtr turn_signal_ptr; + geometry_msgs::msg::TwistStamped::ConstSharedPtr twist_ptr; + autoware_vehicle_msgs::msg::ShiftStamped::ConstSharedPtr gear_ptr; + std_msgs::msg::Float32::ConstSharedPtr battery_ptr; + sensor_msgs::msg::NavSatFix::ConstSharedPtr nav_sat_ptr; + autoware_system_msgs::msg::AutowareState::ConstSharedPtr autoware_state_ptr; + autoware_vehicle_msgs::msg::ControlMode::ConstSharedPtr control_mode_ptr; + autoware_control_msgs::msg::GateMode::ConstSharedPtr gate_mode_ptr; + autoware_control_msgs::msg::EmergencyMode::ConstSharedPtr is_emergency_ptr; + autoware_planning_msgs::msg::StopReasonArray::ConstSharedPtr stop_reason_ptr; + diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr diagnostic_ptr; + pacmod_msgs::msg::GlobalRpt::ConstSharedPtr global_rpt_ptr; + std_msgs::msg::Bool::ConstSharedPtr lane_change_available_ptr; + std_msgs::msg::Bool::ConstSharedPtr lane_change_ready_ptr; + autoware_planning_msgs::msg::Path::ConstSharedPtr lane_change_candidate_ptr; + std_msgs::msg::Bool::ConstSharedPtr obstacle_avoid_ready_ptr; + autoware_planning_msgs::msg::Trajectory::ConstSharedPtr obstacle_avoid_candidate_ptr; }; -template -T getParam(const ros::NodeHandle & nh, const std::string & key, const T & default_value) -{ - T value; - nh.param(key, value, default_value); - return value; -} - -template -T waitForParam(const ros::NodeHandle & nh, const std::string & key) -{ - T value; - ros::Rate rate(1.0); - - while (ros::ok()) { - const auto result = nh.getParam(key, value); - if (result) { - return value; - } - - ROS_WARN("waiting for parameter `%s` ...", key.c_str()); - rate.sleep(); - } - - return {}; -} - double lowpass_filter(const double current_value, const double prev_value, const double gain); } // namespace autoware_api diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.h b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.h index d6d26bbb536b9..f94520c3f8e64 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.h +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.h @@ -14,6 +14,24 @@ * limitations under the License. */ +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + #include #include #include @@ -23,67 +41,67 @@ namespace autoware_api { -class AutowareIvAdapter +class AutowareIvAdapter : public rclcpp::Node { public: AutowareIvAdapter(); private: - // node handle - ros::NodeHandle nh_; - ros::NodeHandle pnh_; - // subscriber - ros::Subscriber sub_steer_; - ros::Subscriber sub_vehicle_cmd_; - ros::Subscriber sub_turn_signal_cmd_; - ros::Subscriber sub_twist_; - ros::Subscriber sub_gear_; - ros::Subscriber sub_battery_; - ros::Subscriber sub_nav_sat_; - ros::Subscriber sub_autoware_state_; - ros::Subscriber sub_control_mode_; - ros::Subscriber sub_gate_mode_; - ros::Subscriber sub_emergency_; - ros::Subscriber sub_stop_reason_; - ros::Subscriber sub_diagnostics_; - ros::Subscriber sub_global_rpt_; - ros::Subscriber sub_lane_change_available_; - ros::Subscriber sub_lane_change_ready_; - ros::Subscriber sub_lane_change_candidate_; - ros::Subscriber sub_obstacle_avoid_ready_; - ros::Subscriber sub_obstacle_avoid_candidate_; + rclcpp::Subscription::SharedPtr sub_steer_; + rclcpp::Subscription::SharedPtr sub_vehicle_cmd_; + rclcpp::Subscription::SharedPtr sub_turn_signal_cmd_; + rclcpp::Subscription::SharedPtr sub_twist_; + rclcpp::Subscription::SharedPtr sub_gear_; + rclcpp::Subscription::SharedPtr sub_battery_; + rclcpp::Subscription::SharedPtr sub_nav_sat_; + rclcpp::Subscription::SharedPtr sub_autoware_state_; + rclcpp::Subscription::SharedPtr sub_control_mode_; + rclcpp::Subscription::SharedPtr sub_gate_mode_; + rclcpp::Subscription::SharedPtr sub_emergency_; + rclcpp::Subscription::SharedPtr sub_stop_reason_; + rclcpp::Subscription::SharedPtr sub_diagnostics_; + rclcpp::Subscription::SharedPtr sub_global_rpt_; + rclcpp::Subscription::SharedPtr sub_lane_change_available_; + rclcpp::Subscription::SharedPtr sub_lane_change_ready_; + rclcpp::Subscription::SharedPtr sub_lane_change_candidate_; + rclcpp::Subscription::SharedPtr sub_obstacle_avoid_ready_; + rclcpp::Subscription::SharedPtr + sub_obstacle_avoid_candidate_; // timer - ros::Timer timer_; + rclcpp::TimerBase::SharedPtr timer_; // tf tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; // callback function - void callbackSteer(const autoware_vehicle_msgs::Steering::ConstPtr & msg_ptr); - void callbackVehicleCmd(const autoware_vehicle_msgs::VehicleCommand::ConstPtr & msg_ptr); - void callbackTurnSignal(const autoware_vehicle_msgs::TurnSignal::ConstPtr & msg_ptr); - void callbackTwist(const geometry_msgs::TwistStamped::ConstPtr & msg_ptr); - void callbackGear(const autoware_vehicle_msgs::ShiftStamped::ConstPtr & msg_ptr); - void callbackBattery(const std_msgs::Float32::ConstPtr & msg_ptr); - void callbackNavSat(const sensor_msgs::NavSatFix::ConstPtr & msg_ptr); - void callbackAutowareState(const autoware_system_msgs::AutowareState::ConstPtr & msg_ptr); - void callbackControlMode(const autoware_vehicle_msgs::ControlMode::ConstPtr & msg_ptr); - void callbackGateMode(const autoware_control_msgs::GateMode::ConstPtr & msg_ptr); - void callbackIsEmergency(const std_msgs::Bool::ConstPtr & msg_ptr); - void callbackStopReason(const autoware_planning_msgs::StopReasonArray::ConstPtr & msg_ptr); - void callbackDiagnostics(const diagnostic_msgs::DiagnosticArray::ConstPtr & msg_ptr); - void callbackGlobalRpt(const pacmod_msgs::GlobalRpt::ConstPtr & msg_ptr); - void callbackLaneChangeAvailable(const std_msgs::Bool::ConstPtr & msg_ptr); - void callbackLaneChangeReady(const std_msgs::Bool::ConstPtr & msg_ptr); - void callbackLaneChangeCandidatePath(const autoware_planning_msgs::Path::ConstPtr & msg_ptr); - void callbackLaneObstacleAvoidReady(const std_msgs::Bool::ConstPtr & msg_ptr); + void callbackSteer(const autoware_vehicle_msgs::msg::Steering::ConstSharedPtr msg_ptr); + void callbackVehicleCmd(const autoware_vehicle_msgs::msg::VehicleCommand::ConstSharedPtr msg_ptr); + void callbackTurnSignal(const autoware_vehicle_msgs::msg::TurnSignal::ConstSharedPtr msg_ptr); + void callbackTwist(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg_ptr); + void callbackGear(const autoware_vehicle_msgs::msg::ShiftStamped::ConstSharedPtr msg_ptr); + void callbackBattery(const std_msgs::msg::Float32::ConstSharedPtr msg_ptr); + void callbackNavSat(const sensor_msgs::msg::NavSatFix::ConstSharedPtr msg_ptr); + void callbackAutowareState( + const autoware_system_msgs::msg::AutowareState::ConstSharedPtr msg_ptr); + void callbackControlMode(const autoware_vehicle_msgs::msg::ControlMode::ConstSharedPtr msg_ptr); + void callbackGateMode(const autoware_control_msgs::msg::GateMode::ConstSharedPtr msg_ptr); + void callbackIsEmergency(const autoware_control_msgs::msg::EmergencyMode::ConstSharedPtr msg_ptr); + void callbackStopReason( + const autoware_planning_msgs::msg::StopReasonArray::ConstSharedPtr msg_ptr); + void callbackDiagnostics(const diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr msg_ptr); + void callbackGlobalRpt(const pacmod_msgs::msg::GlobalRpt::ConstSharedPtr msg_ptr); + void callbackLaneChangeAvailable(const std_msgs::msg::Bool::ConstSharedPtr msg_ptr); + void callbackLaneChangeReady(const std_msgs::msg::Bool::ConstSharedPtr msg_ptr); + void callbackLaneChangeCandidatePath( + const autoware_planning_msgs::msg::Path::ConstSharedPtr msg_ptr); + void callbackLaneObstacleAvoidReady(const std_msgs::msg::Bool::ConstSharedPtr msg_ptr); void callbackLaneObstacleAvoidCandidatePath( - const autoware_planning_msgs::Trajectory::ConstPtr & msg_ptr); + const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr msg_ptr); // timer function - void timerCallback(const ros::TimerEvent & e); + void timerCallback(); void emergencyParamCheck(const bool emergency_handling_param); void getCurrentPose(); diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_lane_change_state_publisher.h b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_lane_change_state_publisher.h index 56c30f4aa30cd..498e2ae16291b 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_lane_change_state_publisher.h +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_lane_change_state_publisher.h @@ -14,32 +14,35 @@ * limitations under the License. */ -#include +#include + #include +#include namespace autoware_api { class AutowareIvLaneChangeStatePublisher { public: - AutowareIvLaneChangeStatePublisher(); + AutowareIvLaneChangeStatePublisher(rclcpp::Node& node); void statePublisher(const AutowareInfo & aw_info); private: - // node handle - ros::NodeHandle nh_; - ros::NodeHandle pnh_; + rclcpp::Logger logger_; + rclcpp::Clock::SharedPtr clock_; // publisher - ros::Publisher pub_state_; + rclcpp::Publisher::SharedPtr pub_state_; void getLaneChangeAvailableInfo( - const std_msgs::Bool::ConstPtr & available_ptr, autoware_api_msgs::LaneChangeStatus * status); + const std_msgs::msg::Bool::ConstSharedPtr & available_ptr, + autoware_api_msgs::msg::LaneChangeStatus * status); void getLaneChangeReadyInfo( - const std_msgs::Bool::ConstPtr & ready_ptr, autoware_api_msgs::LaneChangeStatus * status); + const std_msgs::msg::Bool::ConstSharedPtr & ready_ptr, + autoware_api_msgs::msg::LaneChangeStatus * status); void getCandidatePathInfo( - const autoware_planning_msgs::Path::ConstPtr & path_ptr, - autoware_api_msgs::LaneChangeStatus * status); + const autoware_planning_msgs::msg::Path::ConstSharedPtr & path_ptr, + autoware_api_msgs::msg::LaneChangeStatus * status); }; } // namespace autoware_api diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_obstacle_avoidance_state_publisher.h b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_obstacle_avoidance_state_publisher.h index f82ddb16dff07..ce5834bd9a638 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_obstacle_avoidance_state_publisher.h +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_obstacle_avoidance_state_publisher.h @@ -14,31 +14,32 @@ * limitations under the License. */ -#include +#include + #include +#include namespace autoware_api { class AutowareIvObstacleAvoidanceStatePublisher { public: - AutowareIvObstacleAvoidanceStatePublisher(); + AutowareIvObstacleAvoidanceStatePublisher(rclcpp::Node& node); void statePublisher(const AutowareInfo & aw_info); private: - // node handle - ros::NodeHandle nh_; - ros::NodeHandle pnh_; + rclcpp::Logger logger_; + rclcpp::Clock::SharedPtr clock_; // publisher - ros::Publisher pub_state_; + rclcpp::Publisher::SharedPtr pub_state_; void getObstacleAvoidReadyInfo( - const std_msgs::Bool::ConstPtr & ready_ptr, - autoware_api_msgs::ObstacleAvoidanceStatus * status); + const std_msgs::msg::Bool::ConstSharedPtr & ready_ptr, + autoware_api_msgs::msg::ObstacleAvoidanceStatus * status); void getCandidatePathInfo( - const autoware_planning_msgs::Trajectory::ConstPtr & path_ptr, - autoware_api_msgs::ObstacleAvoidanceStatus * status); + const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr & path_ptr, + autoware_api_msgs::msg::ObstacleAvoidanceStatus * status); }; } // namespace autoware_api diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_stop_reason_aggregator.h b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_stop_reason_aggregator.h index 32e1f4abfcebf..2763f408df3dc 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_stop_reason_aggregator.h +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_stop_reason_aggregator.h @@ -16,6 +16,8 @@ #pragma once +#include + #include namespace autoware_api @@ -23,23 +25,25 @@ namespace autoware_api class AutowareIvStopReasonAggregator { public: - AutowareIvStopReasonAggregator(const double timeout); - autoware_planning_msgs::StopReasonArray::ConstPtr updateStopReasonArray( - const autoware_planning_msgs::StopReasonArray::ConstPtr & msg_ptr); + AutowareIvStopReasonAggregator(rclcpp::Node& node, const double timeout); + autoware_planning_msgs::msg::StopReasonArray::ConstSharedPtr updateStopReasonArray( + const autoware_planning_msgs::msg::StopReasonArray::ConstSharedPtr & msg_ptr); private: - void applyUpdate(const autoware_planning_msgs::StopReasonArray::ConstPtr & msg_ptr); + void applyUpdate(const autoware_planning_msgs::msg::StopReasonArray::ConstSharedPtr & msg_ptr); bool checkMatchingReason( - const autoware_planning_msgs::StopReasonArray::ConstPtr & msg_stop_reason_array, - const autoware_planning_msgs::StopReasonArray & stop_reason_array); + const autoware_planning_msgs::msg::StopReasonArray::ConstSharedPtr & msg_stop_reason_array, + const autoware_planning_msgs::msg::StopReasonArray & stop_reason_array); void applyTimeOut(); void appendStopReasonToArray( - const autoware_planning_msgs::StopReason & stop_reason, - autoware_planning_msgs::StopReasonArray * stop_reason_array); - autoware_planning_msgs::StopReasonArray::ConstPtr makeStopReasonArray(); + const autoware_planning_msgs::msg::StopReason & stop_reason, + autoware_planning_msgs::msg::StopReasonArray * stop_reason_array); + autoware_planning_msgs::msg::StopReasonArray::ConstSharedPtr makeStopReasonArray(); + rclcpp::Logger logger_; + rclcpp::Clock::SharedPtr clock_; double timeout_; - std::vector stop_reason_array_vec_; + std::vector stop_reason_array_vec_; }; } // namespace autoware_api diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_vehicle_state_publisher.h b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_vehicle_state_publisher.h index e30f39ffcf604..891f81a4ce7b6 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_vehicle_state_publisher.h +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_vehicle_state_publisher.h @@ -14,54 +14,55 @@ * limitations under the License. */ -#include +#include + #include +#include namespace autoware_api { class AutowareIvVehicleStatePublisher { public: - AutowareIvVehicleStatePublisher(); + AutowareIvVehicleStatePublisher(rclcpp::Node& node); void statePublisher(const AutowareInfo & aw_info); private: - // node handle - ros::NodeHandle nh_; - ros::NodeHandle pnh_; - // publisher - ros::Publisher pub_state_; + rclcpp::Publisher::SharedPtr pub_state_; - autoware_api_msgs::AwapiVehicleStatus initVehicleStatus(); + autoware_api_msgs::msg::AwapiVehicleStatus initVehicleStatus(); void getPoseInfo( - const std::shared_ptr & pose_ptr, - autoware_api_msgs::AwapiVehicleStatus * status); + const std::shared_ptr & pose_ptr, + autoware_api_msgs::msg::AwapiVehicleStatus * status); void getSteerInfo( - const autoware_vehicle_msgs::Steering::ConstPtr & steer_ptr, - autoware_api_msgs::AwapiVehicleStatus * status); + const autoware_vehicle_msgs::msg::Steering::ConstSharedPtr & steer_ptr, + autoware_api_msgs::msg::AwapiVehicleStatus * status); void getVehicleCmdInfo( - const autoware_vehicle_msgs::VehicleCommand::ConstPtr & vehicle_cmd_ptr, - autoware_api_msgs::AwapiVehicleStatus * status); + const autoware_vehicle_msgs::msg::VehicleCommand::ConstSharedPtr & vehicle_cmd_ptr, + autoware_api_msgs::msg::AwapiVehicleStatus * status); void getTurnSignalInfo( - const autoware_vehicle_msgs::TurnSignal::ConstPtr & turn_signal_ptr, - autoware_api_msgs::AwapiVehicleStatus * status); + const autoware_vehicle_msgs::msg::TurnSignal::ConstSharedPtr & turn_signal_ptr, + autoware_api_msgs::msg::AwapiVehicleStatus * status); void getTwistInfo( - const geometry_msgs::TwistStamped::ConstPtr & twist_ptr, - autoware_api_msgs::AwapiVehicleStatus * status); + const geometry_msgs::msg::TwistStamped::ConstSharedPtr & twist_ptr, + autoware_api_msgs::msg::AwapiVehicleStatus * status); void getGearInfo( - const autoware_vehicle_msgs::ShiftStamped::ConstPtr & gear_ptr, - autoware_api_msgs::AwapiVehicleStatus * status); + const autoware_vehicle_msgs::msg::ShiftStamped::ConstSharedPtr & gear_ptr, + autoware_api_msgs::msg::AwapiVehicleStatus * status); void getBatteryInfo( - const std_msgs::Float32::ConstPtr & battery_ptr, - autoware_api_msgs::AwapiVehicleStatus * status); + const std_msgs::msg::Float32::ConstSharedPtr & battery_ptr, + autoware_api_msgs::msg::AwapiVehicleStatus * status); void getGpsInfo( - const sensor_msgs::NavSatFix::ConstPtr & nav_sat_ptr, - autoware_api_msgs::AwapiVehicleStatus * status); + const sensor_msgs::msg::NavSatFix::ConstSharedPtr & nav_sat_ptr, + autoware_api_msgs::msg::AwapiVehicleStatus * status); + + rclcpp::Logger logger_; + rclcpp::Clock::SharedPtr clock_; //parameters - geometry_msgs::TwistStamped::ConstPtr previous_twist_ptr_; - autoware_vehicle_msgs::Steering::ConstPtr previous_steer_ptr_; + geometry_msgs::msg::TwistStamped::ConstSharedPtr previous_twist_ptr_; + autoware_vehicle_msgs::msg::Steering::ConstSharedPtr previous_steer_ptr_; double prev_accel_; double prev_steer_vel_; diff --git a/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml b/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml index 47c9be49b8557..6202ae7aa59b7 100644 --- a/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml +++ b/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml @@ -43,7 +43,7 @@ - + @@ -61,50 +61,103 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + - - + + + + + + + + + + - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/awapi/awapi_awiv_adapter/package.xml b/awapi/awapi_awiv_adapter/package.xml index 6909569c2b9b5..2b1e551974f5c 100644 --- a/awapi/awapi_awiv_adapter/package.xml +++ b/awapi/awapi_awiv_adapter/package.xml @@ -7,9 +7,9 @@ Tomoya Kimura BSD - catkin + ament_cmake_auto - roscpp + rclcpp autoware_api_msgs autoware_system_msgs autoware_planning_msgs @@ -22,4 +22,12 @@ sensor_msgs tf2 tf2_geometry_msgs + + autoware_perception_msgs + topic_tools + + + + ament_cmake + diff --git a/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp b/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp index 35cf84193c772..e5646959532cc 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp @@ -19,20 +19,23 @@ namespace autoware_api { -AutowareIvAutowareStatePublisher::AutowareIvAutowareStatePublisher() -: nh_(), pnh_("~"), arrived_goal_(false) +AutowareIvAutowareStatePublisher::AutowareIvAutowareStatePublisher(rclcpp::Node& node) +: logger_(node.get_logger().get_child("awapi_awiv_autoware_state_publisher")), + clock_(node.get_clock()), + arrived_goal_(false) { // publisher - pub_state_ = pnh_.advertise("output/autoware_status", 1); + pub_state_ = node.create_publisher( + "output/autoware_status", 1); } void AutowareIvAutowareStatePublisher::statePublisher(const AutowareInfo & aw_info) { - autoware_api_msgs::AwapiAutowareStatus status; + autoware_api_msgs::msg::AwapiAutowareStatus status; //input header status.header.frame_id = "base_link"; - status.header.stamp = ros::Time::now(); + status.header.stamp = clock_->now(); // get all info getAutowareStateInfo(aw_info.autoware_state_ptr, &status); @@ -44,15 +47,16 @@ void AutowareIvAutowareStatePublisher::statePublisher(const AutowareInfo & aw_in getGlobalRptInfo(aw_info.global_rpt_ptr, &status); // publish info - pub_state_.publish(status); + pub_state_->publish(status); } void AutowareIvAutowareStatePublisher::getAutowareStateInfo( - const autoware_system_msgs::AutowareState::ConstPtr & autoware_state_ptr, - autoware_api_msgs::AwapiAutowareStatus * status) + const autoware_system_msgs::msg::AutowareState::ConstSharedPtr & autoware_state_ptr, + autoware_api_msgs::msg::AwapiAutowareStatus * status) { if (!autoware_state_ptr) { - ROS_DEBUG_STREAM_THROTTLE(5.0, "[AutowareIvAutowareStatePublisher] autoware_state is nullptr"); + RCLCPP_DEBUG_STREAM_THROTTLE( + logger_, *clock_, 5000 /* ms */, "autoware_state is nullptr"); return; } @@ -62,11 +66,11 @@ void AutowareIvAutowareStatePublisher::getAutowareStateInfo( } void AutowareIvAutowareStatePublisher::getControlModeInfo( - const autoware_vehicle_msgs::ControlMode::ConstPtr & control_mode_ptr, - autoware_api_msgs::AwapiAutowareStatus * status) + const autoware_vehicle_msgs::msg::ControlMode::ConstSharedPtr & control_mode_ptr, + autoware_api_msgs::msg::AwapiAutowareStatus * status) { if (!control_mode_ptr) { - ROS_DEBUG_STREAM_THROTTLE(5.0, "[AutowareIvAutowareStatePublisher] control mode is nullptr"); + RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *clock_, 5000 /* ms */, "control mode is nullptr"); return; } @@ -75,11 +79,11 @@ void AutowareIvAutowareStatePublisher::getControlModeInfo( } void AutowareIvAutowareStatePublisher::getGateModeInfo( - const autoware_control_msgs::GateMode::ConstPtr & gate_mode_ptr, - autoware_api_msgs::AwapiAutowareStatus * status) + const autoware_control_msgs::msg::GateMode::ConstSharedPtr & gate_mode_ptr, + autoware_api_msgs::msg::AwapiAutowareStatus * status) { if (!gate_mode_ptr) { - ROS_DEBUG_STREAM_THROTTLE(5.0, "[AutowareIvAutowareStatePublisher] gate mode is nullptr"); + RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *clock_, 5000 /* ms */, "gate mode is nullptr"); return; } @@ -88,24 +92,24 @@ void AutowareIvAutowareStatePublisher::getGateModeInfo( } void AutowareIvAutowareStatePublisher::getIsEmergencyInfo( - const std_msgs::Bool::ConstPtr & is_emergency_ptr, - autoware_api_msgs::AwapiAutowareStatus * status) + const autoware_control_msgs::msg::EmergencyMode::ConstSharedPtr & is_emergency_ptr, + autoware_api_msgs::msg::AwapiAutowareStatus * status) { if (!is_emergency_ptr) { - ROS_DEBUG_STREAM_THROTTLE(5.0, "[AutowareIvAutowareStatePublisher] is_emergency is nullptr"); + RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *clock_, 5000 /* ms */, "is_emergency is nullptr"); return; } // get emergency - status->emergency_stopped = is_emergency_ptr->data; + status->emergency_stopped = is_emergency_ptr->is_emergency; } void AutowareIvAutowareStatePublisher::getStopReasonInfo( - const autoware_planning_msgs::StopReasonArray::ConstPtr & stop_reason_ptr, - autoware_api_msgs::AwapiAutowareStatus * status) + const autoware_planning_msgs::msg::StopReasonArray::ConstSharedPtr & stop_reason_ptr, + autoware_api_msgs::msg::AwapiAutowareStatus * status) { if (!stop_reason_ptr) { - ROS_DEBUG_STREAM_THROTTLE(5.0, "[AutowareIvAutowareStatePublisher] stop reason is nullptr"); + RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *clock_, 5000 /* ms */, "stop reason is nullptr"); return; } @@ -113,11 +117,11 @@ void AutowareIvAutowareStatePublisher::getStopReasonInfo( } void AutowareIvAutowareStatePublisher::getDiagInfo( - const diagnostic_msgs::DiagnosticArray::ConstPtr & diag_ptr, - autoware_api_msgs::AwapiAutowareStatus * status) + const diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr & diag_ptr, + autoware_api_msgs::msg::AwapiAutowareStatus * status) { if (!diag_ptr) { - ROS_DEBUG_STREAM_THROTTLE(5.0, "[AutowareIvAutowareStatePublisher] diagnostics is nullptr"); + RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *clock_, 5000 /* ms */, "diagnostics is nullptr"); return; } @@ -126,11 +130,11 @@ void AutowareIvAutowareStatePublisher::getDiagInfo( } void AutowareIvAutowareStatePublisher::getGlobalRptInfo( - const pacmod_msgs::GlobalRpt::ConstPtr & global_rpt_ptr, - autoware_api_msgs::AwapiAutowareStatus * status) + const pacmod_msgs::msg::GlobalRpt::ConstSharedPtr & global_rpt_ptr, + autoware_api_msgs::msg::AwapiAutowareStatus * status) { if (!global_rpt_ptr) { - ROS_DEBUG_STREAM_THROTTLE(5.0, "[AutowareIvAutowareStatePublisher] global_rpt is nullptr"); + RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *clock_, 5000 /* ms */, "global_rpt is nullptr"); return; } @@ -139,22 +143,22 @@ void AutowareIvAutowareStatePublisher::getGlobalRptInfo( } bool AutowareIvAutowareStatePublisher::isGoal( - const autoware_system_msgs::AutowareState::ConstPtr & autoware_state) + const autoware_system_msgs::msg::AutowareState::ConstSharedPtr & autoware_state) { //rename const auto & aw_state = autoware_state->state; - if (aw_state == autoware_system_msgs::AutowareState::ArrivedGoal) { + if (aw_state == autoware_system_msgs::msg::AutowareState::ARRIVAL_GOAL) { arrived_goal_ = true; } else if ( - prev_state_ == autoware_system_msgs::AutowareState::Driving && - aw_state == autoware_system_msgs::AutowareState::WaitingForRoute) { + prev_state_ == autoware_system_msgs::msg::AutowareState::DRIVING && + aw_state == autoware_system_msgs::msg::AutowareState::WAITING_FOR_ROUTE) { arrived_goal_ = true; } if ( - aw_state == autoware_system_msgs::AutowareState::WaitingForEngage || - aw_state == autoware_system_msgs::AutowareState::Driving) { + aw_state == autoware_system_msgs::msg::AutowareState::WAITING_FOR_ENGAGE || + aw_state == autoware_system_msgs::msg::AutowareState::DRIVING) { //cancel goal state arrived_goal_ = false; } @@ -164,12 +168,13 @@ bool AutowareIvAutowareStatePublisher::isGoal( return arrived_goal_; } -std::vector AutowareIvAutowareStatePublisher::extractLeafDiag( - const std::vector & diag_vec) +std::vector +AutowareIvAutowareStatePublisher::extractLeafDiag( + const std::vector & diag_vec) { updateDiagNameSet(diag_vec); - std::vector leaf_diag_info; + std::vector leaf_diag_info; for (const auto diag : diag_vec) { if (isLeaf(diag)) { leaf_diag_info.emplace_back(diag); @@ -191,7 +196,7 @@ std::string AutowareIvAutowareStatePublisher::splitStringByLastSlash(const std:: } void AutowareIvAutowareStatePublisher::updateDiagNameSet( - const std::vector & diag_vec) + const std::vector & diag_vec) { // set diag name to diag_name_set_ for (const auto & diag : diag_vec) { @@ -199,7 +204,7 @@ void AutowareIvAutowareStatePublisher::updateDiagNameSet( } } -bool AutowareIvAutowareStatePublisher::isLeaf(const diagnostic_msgs::DiagnosticStatus & diag) +bool AutowareIvAutowareStatePublisher::isLeaf(const diagnostic_msgs::msg::DiagnosticStatus & diag) { //if not find diag.name in diag set, diag is leaf. return diag_name_set_.find(diag.name) == diag_name_set_.end(); diff --git a/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp b/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp index 1ef8801a99449..535e7de1e0a26 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp @@ -15,75 +15,93 @@ */ #include +#include namespace autoware_api { -AutowareIvAdapter::AutowareIvAdapter() : nh_(), pnh_("~"), tf_listener_(tf_buffer_) +using std::placeholders::_1; + +AutowareIvAdapter::AutowareIvAdapter() +: Node("awapi_awiv_adapter_node"), tf_buffer_(this->get_clock()), tf_listener_(tf_buffer_) { // get param - pnh_.param("status_pub_hz", status_pub_hz_, 5.0); - pnh_.param("stop_reason_timeout", stop_reason_timeout_, 0.5); - const bool em_handle_param = waitForParam(pnh_, "param/emergency_handling"); + status_pub_hz_ = this->declare_parameter("status_pub_hz", 5.0); + stop_reason_timeout_ = this->declare_parameter("stop_reason_timeout", 0.5); + const bool em_handle_param = this->declare_parameter("use_emergency_handling").get(); emergencyParamCheck(em_handle_param); // setup instance - vehicle_state_publisher_ = std::make_unique(); - autoware_state_publisher_ = std::make_unique(); - stop_reason_aggreagator_ = std::make_unique(stop_reason_timeout_); - lane_change_state_publisher_ = std::make_unique(); + vehicle_state_publisher_ = std::make_unique(*this); + autoware_state_publisher_ = std::make_unique(*this); + stop_reason_aggreagator_ = std::make_unique(*this, stop_reason_timeout_); + lane_change_state_publisher_ = std::make_unique(*this); obstacle_avoidance_state_publisher_ = - std::make_unique(); + std::make_unique(*this); // subscriber - sub_steer_ = pnh_.subscribe("input/steer", 1, &AutowareIvAdapter::callbackSteer, this); - sub_vehicle_cmd_ = - pnh_.subscribe("input/vehicle_cmd", 1, &AutowareIvAdapter::callbackVehicleCmd, this); - sub_turn_signal_cmd_ = - pnh_.subscribe("input/turn_signal", 1, &AutowareIvAdapter::callbackTurnSignal, this); - sub_twist_ = pnh_.subscribe("input/twist", 1, &AutowareIvAdapter::callbackTwist, this); - sub_gear_ = pnh_.subscribe("input/gear", 1, &AutowareIvAdapter::callbackGear, this); - sub_battery_ = pnh_.subscribe("input/battery", 1, &AutowareIvAdapter::callbackBattery, this); - sub_nav_sat_ = pnh_.subscribe("input/nav_sat", 1, &AutowareIvAdapter::callbackNavSat, this); - sub_autoware_state_ = - pnh_.subscribe("input/autoware_state", 1, &AutowareIvAdapter::callbackAutowareState, this); - sub_control_mode_ = - pnh_.subscribe("input/control_mode", 1, &AutowareIvAdapter::callbackControlMode, this); - sub_gate_mode_ = pnh_.subscribe("input/gate_mode", 1, &AutowareIvAdapter::callbackGateMode, this); - sub_emergency_ = - pnh_.subscribe("input/is_emergency", 1, &AutowareIvAdapter::callbackIsEmergency, this); - sub_stop_reason_ = - pnh_.subscribe("input/stop_reason", 100, &AutowareIvAdapter::callbackStopReason, this); - sub_diagnostics_ = - pnh_.subscribe("input/diagnostics", 1, &AutowareIvAdapter::callbackDiagnostics, this); - sub_global_rpt_ = - pnh_.subscribe("input/global_rpt", 1, &AutowareIvAdapter::callbackGlobalRpt, this); - sub_lane_change_available_ = pnh_.subscribe( - "input/lane_change_avaiable", 1, &AutowareIvAdapter::callbackLaneChangeAvailable, this); - sub_lane_change_ready_ = - pnh_.subscribe("input/lane_change_ready", 1, &AutowareIvAdapter::callbackLaneChangeReady, this); - sub_lane_change_candidate_ = pnh_.subscribe( - "input/lane_change_candidate_path", 1, &AutowareIvAdapter::callbackLaneChangeCandidatePath, - this); - sub_obstacle_avoid_ready_ = pnh_.subscribe( - "input/obstacle_avoid_ready", 1, &AutowareIvAdapter::callbackLaneObstacleAvoidReady, this); - sub_obstacle_avoid_candidate_ = pnh_.subscribe( - "input/obstacle_avoid_candidate_path", 1, - &AutowareIvAdapter::callbackLaneObstacleAvoidCandidatePath, this); + sub_steer_ = this->create_subscription( + "input/steer", 1, std::bind(&AutowareIvAdapter::callbackSteer, this, _1)); + sub_vehicle_cmd_ = this->create_subscription( + "input/vehicle_cmd", 1, std::bind(&AutowareIvAdapter::callbackVehicleCmd, this, _1)); + sub_turn_signal_cmd_ = this->create_subscription( + "input/turn_signal", 1, std::bind(&AutowareIvAdapter::callbackTurnSignal, this, _1)); + sub_twist_ = this->create_subscription( + "input/twist", 1, std::bind(&AutowareIvAdapter::callbackTwist, this, _1)); + sub_gear_ = this->create_subscription( + "input/gear", 1, std::bind(&AutowareIvAdapter::callbackGear, this, _1)); + sub_battery_ = this->create_subscription( + "input/battery", 1, std::bind(&AutowareIvAdapter::callbackBattery, this, _1)); + sub_nav_sat_ = this->create_subscription( + "input/nav_sat", 1, std::bind(&AutowareIvAdapter::callbackNavSat, this, _1)); + sub_autoware_state_ = this->create_subscription( + "input/autoware_state", 1, std::bind(&AutowareIvAdapter::callbackAutowareState, this, _1)); + sub_control_mode_ = this->create_subscription( + "input/control_mode", 1, std::bind(&AutowareIvAdapter::callbackControlMode, this, _1)); + sub_gate_mode_ = this->create_subscription( + "input/gate_mode", 1, std::bind(&AutowareIvAdapter::callbackGateMode, this, _1)); + sub_emergency_ = this->create_subscription( + "input/is_emergency", 1, std::bind(&AutowareIvAdapter::callbackIsEmergency, this, _1)); + sub_stop_reason_ = this->create_subscription( + "input/stop_reason", 100, std::bind(&AutowareIvAdapter::callbackStopReason, this, _1)); + sub_diagnostics_ = this->create_subscription( + "input/diagnostics", 1, std::bind(&AutowareIvAdapter::callbackDiagnostics, this, _1)); + sub_global_rpt_ = this->create_subscription( + "input/global_rpt", 1, std::bind(&AutowareIvAdapter::callbackGlobalRpt, this, _1)); + sub_lane_change_available_ = this->create_subscription( + "input/lane_change_avaiable", 1, + std::bind(&AutowareIvAdapter::callbackLaneChangeAvailable, this, _1)); + sub_lane_change_ready_ = this->create_subscription( + "input/lane_change_ready", 1, std::bind(&AutowareIvAdapter::callbackLaneChangeReady, this, _1)); + sub_lane_change_candidate_ = this->create_subscription( + "input/lane_change_candidate_path", 1, + std::bind(&AutowareIvAdapter::callbackLaneChangeCandidatePath, this, _1)); + sub_obstacle_avoid_ready_ = this->create_subscription( + "input/obstacle_avoid_ready", 1, + std::bind(&AutowareIvAdapter::callbackLaneObstacleAvoidReady, this, _1)); + sub_obstacle_avoid_candidate_ = + this->create_subscription( + "input/obstacle_avoid_candidate_path", 1, + std::bind(&AutowareIvAdapter::callbackLaneObstacleAvoidCandidatePath, this, _1)); // timer - timer_ = - nh_.createTimer(ros::Duration(1.0 / status_pub_hz_), &AutowareIvAdapter::timerCallback, this); + auto timer_callback = std::bind(&AutowareIvAdapter::timerCallback, this); + auto period = std::chrono::duration_cast( + std::chrono::duration(1.0 / status_pub_hz_)); + timer_ = std::make_shared>( + this->get_clock(), period, std::move(timer_callback), + this->get_node_base_interface()->get_context()); + this->get_node_timers_interface()->add_timer(timer_, nullptr); } void AutowareIvAdapter::emergencyParamCheck(const bool emergency_handling_param) { if (!emergency_handling_param) { - ROS_WARN_STREAM("parameter[use_emergency_handling] is false."); - ROS_WARN_STREAM("autoware/put/emergency is not valid"); + RCLCPP_WARN_STREAM(get_logger(), "parameter[use_emergency_handling] is false."); + RCLCPP_WARN_STREAM(get_logger(), "autoware/put/emergency is not valid"); } } -void AutowareIvAdapter::timerCallback(const ros::TimerEvent & e) +void AutowareIvAdapter::timerCallback() { // get current pose getCurrentPose(); @@ -101,39 +119,42 @@ void AutowareIvAdapter::timerCallback(const ros::TimerEvent & e) obstacle_avoidance_state_publisher_->statePublisher(aw_info_); } -void AutowareIvAdapter::callbackSteer(const autoware_vehicle_msgs::Steering::ConstPtr & msg_ptr) +void AutowareIvAdapter::callbackSteer( + const autoware_vehicle_msgs::msg::Steering::ConstSharedPtr msg_ptr) { aw_info_.steer_ptr = msg_ptr; } void AutowareIvAdapter::callbackVehicleCmd( - const autoware_vehicle_msgs::VehicleCommand::ConstPtr & msg_ptr) + const autoware_vehicle_msgs::msg::VehicleCommand::ConstSharedPtr msg_ptr) { aw_info_.vehicle_cmd_ptr = msg_ptr; } void AutowareIvAdapter::callbackTurnSignal( - const autoware_vehicle_msgs::TurnSignal::ConstPtr & msg_ptr) + const autoware_vehicle_msgs::msg::TurnSignal::ConstSharedPtr msg_ptr) { aw_info_.turn_signal_ptr = msg_ptr; } -void AutowareIvAdapter::callbackTwist(const geometry_msgs::TwistStamped::ConstPtr & msg_ptr) +void AutowareIvAdapter::callbackTwist( + const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg_ptr) { aw_info_.twist_ptr = msg_ptr; } -void AutowareIvAdapter::callbackGear(const autoware_vehicle_msgs::ShiftStamped::ConstPtr & msg_ptr) +void AutowareIvAdapter::callbackGear( + const autoware_vehicle_msgs::msg::ShiftStamped::ConstSharedPtr msg_ptr) { aw_info_.gear_ptr = msg_ptr; } -void AutowareIvAdapter::callbackBattery(const std_msgs::Float32::ConstPtr & msg_ptr) +void AutowareIvAdapter::callbackBattery(const std_msgs::msg::Float32::ConstSharedPtr msg_ptr) { aw_info_.battery_ptr = msg_ptr; } -void AutowareIvAdapter::callbackNavSat(const sensor_msgs::NavSatFix::ConstPtr & msg_ptr) +void AutowareIvAdapter::callbackNavSat(const sensor_msgs::msg::NavSatFix::ConstSharedPtr msg_ptr) { aw_info_.nav_sat_ptr = msg_ptr; } @@ -141,80 +162,84 @@ void AutowareIvAdapter::callbackNavSat(const sensor_msgs::NavSatFix::ConstPtr & void AutowareIvAdapter::getCurrentPose() { try { - auto transform = tf_buffer_.lookupTransform("map", "base_link", ros::Time(0)); - geometry_msgs::PoseStamped ps; + auto transform = tf_buffer_.lookupTransform("map", "base_link", rclcpp::Time(0)); + geometry_msgs::msg::PoseStamped ps; ps.header = transform.header; ps.pose.position.x = transform.transform.translation.x; ps.pose.position.y = transform.transform.translation.y; ps.pose.position.z = transform.transform.translation.z; ps.pose.orientation = transform.transform.rotation; - aw_info_.current_pose_ptr = std::make_shared(ps); + aw_info_.current_pose_ptr = std::make_shared(ps); } catch (tf2::TransformException & ex) { - ROS_DEBUG_STREAM_THROTTLE(2.0, "[awapi_autoware_adapter] cannot get self pose"); + RCLCPP_DEBUG_STREAM_THROTTLE(get_logger(), *this->get_clock(), 2000 /* ms */, "cannot get self pose"); } } void AutowareIvAdapter::callbackAutowareState( - const autoware_system_msgs::AutowareState::ConstPtr & msg_ptr) + const autoware_system_msgs::msg::AutowareState::ConstSharedPtr msg_ptr) { aw_info_.autoware_state_ptr = msg_ptr; } void AutowareIvAdapter::callbackControlMode( - const autoware_vehicle_msgs::ControlMode::ConstPtr & msg_ptr) + const autoware_vehicle_msgs::msg::ControlMode::ConstSharedPtr msg_ptr) { aw_info_.control_mode_ptr = msg_ptr; } -void AutowareIvAdapter::callbackGateMode(const autoware_control_msgs::GateMode::ConstPtr & msg_ptr) +void AutowareIvAdapter::callbackGateMode( + const autoware_control_msgs::msg::GateMode::ConstSharedPtr msg_ptr) { aw_info_.gate_mode_ptr = msg_ptr; } -void AutowareIvAdapter::callbackIsEmergency(const std_msgs::Bool::ConstPtr & msg_ptr) +void AutowareIvAdapter::callbackIsEmergency( + const autoware_control_msgs::msg::EmergencyMode::ConstSharedPtr msg_ptr) { aw_info_.is_emergency_ptr = msg_ptr; } void AutowareIvAdapter::callbackStopReason( - const autoware_planning_msgs::StopReasonArrayConstPtr & msg_ptr) + const autoware_planning_msgs::msg::StopReasonArray::ConstSharedPtr msg_ptr) { aw_info_.stop_reason_ptr = stop_reason_aggreagator_->updateStopReasonArray(msg_ptr); } void AutowareIvAdapter::callbackDiagnostics( - const diagnostic_msgs::DiagnosticArray::ConstPtr & msg_ptr) + const diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr msg_ptr) { aw_info_.diagnostic_ptr = msg_ptr; } -void AutowareIvAdapter::callbackGlobalRpt(const pacmod_msgs::GlobalRpt::ConstPtr & msg_ptr) +void AutowareIvAdapter::callbackGlobalRpt(const pacmod_msgs::msg::GlobalRpt::ConstSharedPtr msg_ptr) { aw_info_.global_rpt_ptr = msg_ptr; } -void AutowareIvAdapter::callbackLaneChangeAvailable(const std_msgs::Bool::ConstPtr & msg_ptr) +void AutowareIvAdapter::callbackLaneChangeAvailable( + const std_msgs::msg::Bool::ConstSharedPtr msg_ptr) { aw_info_.lane_change_available_ptr = msg_ptr; } -void AutowareIvAdapter::callbackLaneChangeReady(const std_msgs::Bool::ConstPtr & msg_ptr) +void AutowareIvAdapter::callbackLaneChangeReady(const std_msgs::msg::Bool::ConstSharedPtr msg_ptr) { aw_info_.lane_change_ready_ptr = msg_ptr; } void AutowareIvAdapter::callbackLaneChangeCandidatePath( - const autoware_planning_msgs::Path::ConstPtr & msg_ptr) + const autoware_planning_msgs::msg::Path::ConstSharedPtr msg_ptr) { aw_info_.lane_change_candidate_ptr = msg_ptr; } -void AutowareIvAdapter::callbackLaneObstacleAvoidReady(const std_msgs::Bool::ConstPtr & msg_ptr) +void AutowareIvAdapter::callbackLaneObstacleAvoidReady( + const std_msgs::msg::Bool::ConstSharedPtr msg_ptr) { aw_info_.obstacle_avoid_ready_ptr = msg_ptr; } void AutowareIvAdapter::callbackLaneObstacleAvoidCandidatePath( - const autoware_planning_msgs::Trajectory::ConstPtr & msg_ptr) + const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr msg_ptr) { aw_info_.obstacle_avoid_candidate_ptr = msg_ptr; } diff --git a/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_node.cpp b/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_node.cpp index a66c0a5773d3b..48ccc3d3ec12a 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_node.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_node.cpp @@ -14,17 +14,15 @@ * limitations under the License. */ -#include +#include #include int main(int argc, char ** argv) { - ros::init(argc, argv, "awapi_awiv_adapter_node"); - - autoware_api::AutowareIvAdapter node; - - ros::spin(); + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); return 0; } \ No newline at end of file diff --git a/awapi/awapi_awiv_adapter/src/awapi_lane_change_state_publisher.cpp b/awapi/awapi_awiv_adapter/src/awapi_lane_change_state_publisher.cpp index 8a3f7f7fd1ca7..8cd28dbf1907a 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_lane_change_state_publisher.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_lane_change_state_publisher.cpp @@ -18,19 +18,22 @@ namespace autoware_api { -AutowareIvLaneChangeStatePublisher::AutowareIvLaneChangeStatePublisher() : nh_(), pnh_("~") +AutowareIvLaneChangeStatePublisher::AutowareIvLaneChangeStatePublisher(rclcpp::Node& node) +: logger_(node.get_logger().get_child("awapi_awiv_lane_change_state_publisher")), + clock_(node.get_clock()) { // publisher - pub_state_ = pnh_.advertise("output/lane_change_status", 1); + pub_state_ = node.create_publisher( + "output/lane_change_status", 1); } void AutowareIvLaneChangeStatePublisher::statePublisher(const AutowareInfo & aw_info) { - autoware_api_msgs::LaneChangeStatus status; + autoware_api_msgs::msg::LaneChangeStatus status; //input header status.header.frame_id = "base_link"; - status.header.stamp = ros::Time::now(); + status.header.stamp = clock_->now(); // get all info getLaneChangeAvailableInfo(aw_info.lane_change_available_ptr, &status); @@ -38,15 +41,16 @@ void AutowareIvLaneChangeStatePublisher::statePublisher(const AutowareInfo & aw_ getCandidatePathInfo(aw_info.lane_change_candidate_ptr, &status); // publish info - pub_state_.publish(status); + pub_state_->publish(status); } void AutowareIvLaneChangeStatePublisher::getLaneChangeAvailableInfo( - const std_msgs::Bool::ConstPtr & available_ptr, autoware_api_msgs::LaneChangeStatus * status) + const std_msgs::msg::Bool::ConstSharedPtr & available_ptr, + autoware_api_msgs::msg::LaneChangeStatus * status) { if (!available_ptr) { - ROS_DEBUG_STREAM_THROTTLE( - 5.0, "[AutowareIvLaneChangeStatePublisher] lane change available is nullptr"); + RCLCPP_DEBUG_STREAM_THROTTLE( + logger_, *clock_, 5000 /* ms */, "lane change available is nullptr"); return; } @@ -55,11 +59,12 @@ void AutowareIvLaneChangeStatePublisher::getLaneChangeAvailableInfo( } void AutowareIvLaneChangeStatePublisher::getLaneChangeReadyInfo( - const std_msgs::Bool::ConstPtr & ready_ptr, autoware_api_msgs::LaneChangeStatus * status) + const std_msgs::msg::Bool::ConstSharedPtr & ready_ptr, + autoware_api_msgs::msg::LaneChangeStatus * status) { if (!ready_ptr) { - ROS_DEBUG_STREAM_THROTTLE( - 5.0, "[AutowareIvLaneChangeStatePublisher] lane change ready is nullptr"); + RCLCPP_DEBUG_STREAM_THROTTLE( + logger_, *clock_, 5000 /* ms */, "lane change ready is nullptr"); return; } @@ -68,14 +73,13 @@ void AutowareIvLaneChangeStatePublisher::getLaneChangeReadyInfo( } void AutowareIvLaneChangeStatePublisher::getCandidatePathInfo( - const autoware_planning_msgs::Path::ConstPtr & path_ptr, - autoware_api_msgs::LaneChangeStatus * status) + const autoware_planning_msgs::msg::Path::ConstSharedPtr & path_ptr, + autoware_api_msgs::msg::LaneChangeStatus * status) { if (!path_ptr) { - ROS_DEBUG_STREAM_THROTTLE( - 5.0, - "[AutowareIvLaneChangeStatePublisher] lane_change_candidate_path is " - "nullptr"); + RCLCPP_DEBUG_STREAM_THROTTLE( + logger_, *clock_, 5000 /* ms */, + "lane_change_candidate_path is nullptr"); return; } diff --git a/awapi/awapi_awiv_adapter/src/awapi_obstacle_avoidance_state_publisher.cpp b/awapi/awapi_awiv_adapter/src/awapi_obstacle_avoidance_state_publisher.cpp index 4a00761d72fe6..2c723703bf700 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_obstacle_avoidance_state_publisher.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_obstacle_avoidance_state_publisher.cpp @@ -18,36 +18,38 @@ namespace autoware_api { -AutowareIvObstacleAvoidanceStatePublisher::AutowareIvObstacleAvoidanceStatePublisher() -: nh_(), pnh_("~") +AutowareIvObstacleAvoidanceStatePublisher::AutowareIvObstacleAvoidanceStatePublisher(rclcpp::Node& node) +: logger_(node.get_logger().get_child("awapi_awiv_obstacle_avoidance_state_publisher")), + clock_(node.get_clock()) { // publisher - pub_state_ = - pnh_.advertise("output/obstacle_avoid_status", 1); + pub_state_ = node.create_publisher( + "output/obstacle_avoid_status", 1); } void AutowareIvObstacleAvoidanceStatePublisher::statePublisher(const AutowareInfo & aw_info) { - autoware_api_msgs::ObstacleAvoidanceStatus status; + autoware_api_msgs::msg::ObstacleAvoidanceStatus status; //input header status.header.frame_id = "base_link"; - status.header.stamp = ros::Time::now(); + status.header.stamp = clock_->now(); // get all info getObstacleAvoidReadyInfo(aw_info.obstacle_avoid_ready_ptr, &status); getCandidatePathInfo(aw_info.obstacle_avoid_candidate_ptr, &status); // publish info - pub_state_.publish(status); + pub_state_->publish(status); } void AutowareIvObstacleAvoidanceStatePublisher::getObstacleAvoidReadyInfo( - const std_msgs::Bool::ConstPtr & ready_ptr, autoware_api_msgs::ObstacleAvoidanceStatus * status) + const std_msgs::msg::Bool::ConstSharedPtr & ready_ptr, + autoware_api_msgs::msg::ObstacleAvoidanceStatus * status) { if (!ready_ptr) { - ROS_DEBUG_STREAM_THROTTLE( - 5.0, "[AutowareIvObstacleAvoidanceStatePublisher] obstacle_avoidance_ready is nullptr"); + RCLCPP_DEBUG_STREAM_THROTTLE( + logger_, *clock_, 5000 /* ms */, "obstacle_avoidance_ready is nullptr"); return; } @@ -55,14 +57,13 @@ void AutowareIvObstacleAvoidanceStatePublisher::getObstacleAvoidReadyInfo( } void AutowareIvObstacleAvoidanceStatePublisher::getCandidatePathInfo( - const autoware_planning_msgs::Trajectory::ConstPtr & path_ptr, - autoware_api_msgs::ObstacleAvoidanceStatus * status) + const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr & path_ptr, + autoware_api_msgs::msg::ObstacleAvoidanceStatus * status) { if (!path_ptr) { - ROS_DEBUG_STREAM_THROTTLE( - 5.0, - "[AutowareIvObstacleAvoidanceStatePublisher] obstacle_avoidance_candidate_path is " - "nullptr"); + RCLCPP_DEBUG_STREAM_THROTTLE( + logger_, *clock_, 5000 /* ms */, + "obstacle_avoidance_candidate_path is nullptr"); return; } diff --git a/awapi/awapi_awiv_adapter/src/awapi_stop_reason_aggregator.cpp b/awapi/awapi_awiv_adapter/src/awapi_stop_reason_aggregator.cpp index 07396e51f8c31..21d1bd4a1ae49 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_stop_reason_aggregator.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_stop_reason_aggregator.cpp @@ -18,14 +18,16 @@ namespace autoware_api { -AutowareIvStopReasonAggregator::AutowareIvStopReasonAggregator(const double timeout) -: timeout_(timeout) +AutowareIvStopReasonAggregator::AutowareIvStopReasonAggregator(rclcpp::Node& node, const double timeout) +: logger_(node.get_logger().get_child("awapi_awiv_stop_reason_aggregator")), + clock_(node.get_clock()), + timeout_(timeout) { } -autoware_planning_msgs::StopReasonArray::ConstPtr +autoware_planning_msgs::msg::StopReasonArray::ConstSharedPtr AutowareIvStopReasonAggregator::updateStopReasonArray( - const autoware_planning_msgs::StopReasonArray::ConstPtr & msg_ptr) + const autoware_planning_msgs::msg::StopReasonArray::ConstSharedPtr & msg_ptr) { applyUpdate(msg_ptr); applyTimeOut(); @@ -33,7 +35,7 @@ AutowareIvStopReasonAggregator::updateStopReasonArray( } void AutowareIvStopReasonAggregator::applyUpdate( - const autoware_planning_msgs::StopReasonArray::ConstPtr & msg_ptr) + const autoware_planning_msgs::msg::StopReasonArray::ConstSharedPtr & msg_ptr) { /* remove old stop_reason that matches reason with received msg */ //make reason-matching msg list @@ -57,8 +59,8 @@ void AutowareIvStopReasonAggregator::applyUpdate( } bool AutowareIvStopReasonAggregator::checkMatchingReason( - const autoware_planning_msgs::StopReasonArray::ConstPtr & msg_stop_reason_array, - const autoware_planning_msgs::StopReasonArray & stop_reason_array) + const autoware_planning_msgs::msg::StopReasonArray::ConstSharedPtr & msg_stop_reason_array, + const autoware_planning_msgs::msg::StopReasonArray & stop_reason_array) { for (const auto msg_stop_reason : msg_stop_reason_array->stop_reasons) { for (const auto stop_reason : stop_reason_array.stop_reasons) { @@ -74,13 +76,15 @@ bool AutowareIvStopReasonAggregator::checkMatchingReason( void AutowareIvStopReasonAggregator::applyTimeOut() { - const auto current_time = ros::Time::now(); + const auto current_time = clock_->now(); //make timeout-msg list std::vector remove_idx; if (!stop_reason_array_vec_.empty()) { for (int i = stop_reason_array_vec_.size() - 1; i >= 0; i--) { - if ((current_time - stop_reason_array_vec_.at(i).header.stamp).toSec() > timeout_) { + if ( + (current_time - rclcpp::Time(stop_reason_array_vec_.at(i).header.stamp)).seconds() > + timeout_) { remove_idx.emplace_back(i); } } @@ -92,8 +96,8 @@ void AutowareIvStopReasonAggregator::applyTimeOut() } void AutowareIvStopReasonAggregator::appendStopReasonToArray( - const autoware_planning_msgs::StopReason & stop_reason, - autoware_planning_msgs::StopReasonArray * stop_reason_array) + const autoware_planning_msgs::msg::StopReason & stop_reason, + autoware_planning_msgs::msg::StopReasonArray * stop_reason_array) { //if stop factors is empty, not append if (stop_reason.stop_factors.empty()) { @@ -101,7 +105,7 @@ void AutowareIvStopReasonAggregator::appendStopReasonToArray( } //if already exists same reason msg in stop_reason_array_msg, append stop_factors to there - for (int i = 0; i < stop_reason_array->stop_reasons.size(); i++) { + for (size_t i = 0; i < stop_reason_array->stop_reasons.size(); i++) { if (stop_reason_array->stop_reasons.at(i).reason == stop_reason.reason) { stop_reason_array->stop_reasons.at(i).stop_factors.insert( stop_reason_array->stop_reasons.at(i).stop_factors.end(), stop_reason.stop_factors.begin(), @@ -114,13 +118,13 @@ void AutowareIvStopReasonAggregator::appendStopReasonToArray( stop_reason_array->stop_reasons.emplace_back(stop_reason); } -autoware_planning_msgs::StopReasonArray::ConstPtr +autoware_planning_msgs::msg::StopReasonArray::ConstSharedPtr AutowareIvStopReasonAggregator::makeStopReasonArray() { - autoware_planning_msgs::StopReasonArray stop_reason_array_msg; + autoware_planning_msgs::msg::StopReasonArray stop_reason_array_msg; // input header stop_reason_array_msg.header.frame_id = "map"; - stop_reason_array_msg.header.stamp = ros::Time::now(); + stop_reason_array_msg.header.stamp = clock_->now(); // input stop reason for (const auto stop_reason_array : stop_reason_array_vec_) { @@ -128,7 +132,7 @@ AutowareIvStopReasonAggregator::makeStopReasonArray() appendStopReasonToArray(stop_reason, &stop_reason_array_msg); } } - return boost::make_shared(stop_reason_array_msg); + return std::make_shared(stop_reason_array_msg); } } // namespace autoware_api \ No newline at end of file diff --git a/awapi/awapi_awiv_adapter/src/awapi_vehicle_state_publisher.cpp b/awapi/awapi_awiv_adapter/src/awapi_vehicle_state_publisher.cpp index 0d6fc747d60b0..dc4a6865b21d5 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_vehicle_state_publisher.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_vehicle_state_publisher.cpp @@ -18,20 +18,23 @@ namespace autoware_api { -AutowareIvVehicleStatePublisher::AutowareIvVehicleStatePublisher() -: nh_(), pnh_("~"), prev_accel_(0.0) +AutowareIvVehicleStatePublisher::AutowareIvVehicleStatePublisher(rclcpp::Node& node) +: logger_(node.get_logger().get_child("awapi_awiv_vehicle_state_publisher")), + clock_(node.get_clock()), + prev_accel_(0.0) { // publisher - pub_state_ = pnh_.advertise("output/vehicle_status", 1); + pub_state_ = + node.create_publisher("output/vehicle_status", 1); } void AutowareIvVehicleStatePublisher::statePublisher(const AutowareInfo & aw_info) { - autoware_api_msgs::AwapiVehicleStatus status = initVehicleStatus(); + autoware_api_msgs::msg::AwapiVehicleStatus status = initVehicleStatus(); //input header status.header.frame_id = "base_link"; - status.header.stamp = ros::Time::now(); + status.header.stamp = clock_->now(); // get all info getPoseInfo(aw_info.current_pose_ptr, &status); @@ -44,12 +47,12 @@ void AutowareIvVehicleStatePublisher::statePublisher(const AutowareInfo & aw_inf getGpsInfo(aw_info.nav_sat_ptr, &status); // publish info - pub_state_.publish(status); + pub_state_->publish(status); } -autoware_api_msgs::AwapiVehicleStatus AutowareIvVehicleStatePublisher::initVehicleStatus() +autoware_api_msgs::msg::AwapiVehicleStatus AutowareIvVehicleStatePublisher::initVehicleStatus() { - autoware_api_msgs::AwapiVehicleStatus status; + autoware_api_msgs::msg::AwapiVehicleStatus status; // set default value if (std::numeric_limits::has_quiet_NaN) { status.energy_level = std::numeric_limits::quiet_NaN(); @@ -58,11 +61,11 @@ autoware_api_msgs::AwapiVehicleStatus AutowareIvVehicleStatePublisher::initVehic } void AutowareIvVehicleStatePublisher::getPoseInfo( - const std::shared_ptr & pose_ptr, - autoware_api_msgs::AwapiVehicleStatus * status) + const std::shared_ptr & pose_ptr, + autoware_api_msgs::msg::AwapiVehicleStatus * status) { if (!pose_ptr) { - ROS_DEBUG_STREAM_THROTTLE(5.0, "[AutowareIvVehicleStatePublisher] current pose is nullptr"); + RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *clock_, 5000 /* ms */, "current pose is nullptr"); return; } @@ -78,11 +81,11 @@ void AutowareIvVehicleStatePublisher::getPoseInfo( } void AutowareIvVehicleStatePublisher::getSteerInfo( - const autoware_vehicle_msgs::Steering::ConstPtr & steer_ptr, - autoware_api_msgs::AwapiVehicleStatus * status) + const autoware_vehicle_msgs::msg::Steering::ConstSharedPtr & steer_ptr, + autoware_api_msgs::msg::AwapiVehicleStatus * status) { if (!steer_ptr) { - ROS_DEBUG_STREAM_THROTTLE(5.0, "[AutowareIvVehicleStatePublisher] steer is nullptr"); + RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *clock_, 5000 /* ms */, "steer is nullptr"); return; } @@ -93,8 +96,10 @@ void AutowareIvVehicleStatePublisher::getSteerInfo( if (previous_steer_ptr_) { //calculate steer vel from steer const double ds = steer_ptr->data - previous_steer_ptr_->data; - const double dt = - std::max((steer_ptr->header.stamp - previous_steer_ptr_->header.stamp).toSec(), 1e-03); + const double dt = std::max( + (rclcpp::Time(steer_ptr->header.stamp) - rclcpp::Time(previous_steer_ptr_->header.stamp)) + .seconds(), + 1e-03); const double steer_vel = ds / dt; //apply lowpass filter @@ -106,11 +111,11 @@ void AutowareIvVehicleStatePublisher::getSteerInfo( previous_steer_ptr_ = steer_ptr; } void AutowareIvVehicleStatePublisher::getVehicleCmdInfo( - const autoware_vehicle_msgs::VehicleCommand::ConstPtr & vehicle_cmd_ptr, - autoware_api_msgs::AwapiVehicleStatus * status) + const autoware_vehicle_msgs::msg::VehicleCommand::ConstSharedPtr & vehicle_cmd_ptr, + autoware_api_msgs::msg::AwapiVehicleStatus * status) { if (!vehicle_cmd_ptr) { - ROS_DEBUG_STREAM_THROTTLE(5.0, "[AutowareIvVehicleStatePublisher] vehicle cmd is nullptr"); + RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *clock_, 5000 /* ms */, "vehicle cmd is nullptr"); return; } @@ -122,11 +127,11 @@ void AutowareIvVehicleStatePublisher::getVehicleCmdInfo( } void AutowareIvVehicleStatePublisher::getTurnSignalInfo( - const autoware_vehicle_msgs::TurnSignal::ConstPtr & turn_signal_ptr, - autoware_api_msgs::AwapiVehicleStatus * status) + const autoware_vehicle_msgs::msg::TurnSignal::ConstSharedPtr & turn_signal_ptr, + autoware_api_msgs::msg::AwapiVehicleStatus * status) { if (!turn_signal_ptr) { - ROS_DEBUG_STREAM_THROTTLE(5.0, "[AutowareIvVehicleStatePublisher] turn signal is nullptr"); + RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *clock_, 5000 /* ms */, "turn signal is nullptr"); return; } @@ -135,11 +140,11 @@ void AutowareIvVehicleStatePublisher::getTurnSignalInfo( } void AutowareIvVehicleStatePublisher::getTwistInfo( - const geometry_msgs::TwistStamped::ConstPtr & twist_ptr, - autoware_api_msgs::AwapiVehicleStatus * status) + const geometry_msgs::msg::TwistStamped::ConstSharedPtr & twist_ptr, + autoware_api_msgs::msg::AwapiVehicleStatus * status) { if (!twist_ptr) { - ROS_DEBUG_STREAM_THROTTLE(5.0, "[AutowareIvVehicleStatePublisher] twist is nullptr"); + RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *clock_, 5000 /* ms */, "twist is nullptr"); return; } @@ -151,8 +156,10 @@ void AutowareIvVehicleStatePublisher::getTwistInfo( if (previous_twist_ptr_) { //calculate accleration from velocity const double dv = twist_ptr->twist.linear.x - previous_twist_ptr_->twist.linear.x; - const double dt = - std::max((twist_ptr->header.stamp - previous_twist_ptr_->header.stamp).toSec(), 1e-03); + const double dt = std::max( + (rclcpp::Time(twist_ptr->header.stamp) - rclcpp::Time(previous_twist_ptr_->header.stamp)) + .seconds(), + 1e-03); const double accel = dv / dt; // apply lowpass filter @@ -164,11 +171,11 @@ void AutowareIvVehicleStatePublisher::getTwistInfo( } void AutowareIvVehicleStatePublisher::getGearInfo( - const autoware_vehicle_msgs::ShiftStamped::ConstPtr & gear_ptr, - autoware_api_msgs::AwapiVehicleStatus * status) + const autoware_vehicle_msgs::msg::ShiftStamped::ConstSharedPtr & gear_ptr, + autoware_api_msgs::msg::AwapiVehicleStatus * status) { if (!gear_ptr) { - ROS_DEBUG_STREAM_THROTTLE(5.0, "[AutowareIvVehicleStatePublisher] gear is nullptr"); + RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *clock_, 5000 /* ms */, "gear is nullptr"); return; } @@ -177,10 +184,11 @@ void AutowareIvVehicleStatePublisher::getGearInfo( } void AutowareIvVehicleStatePublisher::getBatteryInfo( - const std_msgs::Float32::ConstPtr & battery_ptr, autoware_api_msgs::AwapiVehicleStatus * status) + const std_msgs::msg::Float32::ConstSharedPtr & battery_ptr, + autoware_api_msgs::msg::AwapiVehicleStatus * status) { if (!battery_ptr) { - ROS_DEBUG_STREAM_THROTTLE(5.0, "[AutowareIvVehicleStatePublisher] battery is nullptr"); + RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *clock_, 5000 /* ms */, "battery is nullptr"); return; } @@ -189,11 +197,11 @@ void AutowareIvVehicleStatePublisher::getBatteryInfo( } void AutowareIvVehicleStatePublisher::getGpsInfo( - const sensor_msgs::NavSatFix::ConstPtr & nav_sat_ptr, - autoware_api_msgs::AwapiVehicleStatus * status) + const sensor_msgs::msg::NavSatFix::ConstSharedPtr & nav_sat_ptr, + autoware_api_msgs::msg::AwapiVehicleStatus * status) { if (!nav_sat_ptr) { - ROS_DEBUG_STREAM_THROTTLE(5.0, "[AutowareIvVehicleStatePublisher] nav_sat(gps) is nullptr"); + RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *clock_, 5000 /* ms */, "nav_sat(gps) is nullptr"); return; } From 6f101d426fe384f2c72aa5c7d5b5293ef277575f Mon Sep 17 00:00:00 2001 From: Nikolai Morin Date: Thu, 3 Dec 2020 14:26:39 +0100 Subject: [PATCH 18/71] Rename h files to hpp (#142) * Change includes * Rename files * Adjustments to make things compile * Other packages Signed-off-by: tanaka3 --- ...ublisher.h => awapi_autoware_state_publisher.hpp} | 2 +- ...awapi_autoware_util.h => awapi_autoware_util.hpp} | 0 ...iv_adapter_core.h => awapi_awiv_adapter_core.hpp} | 12 ++++++------ ...isher.h => awapi_lane_change_state_publisher.hpp} | 2 +- ... => awapi_obstacle_avoidance_state_publisher.hpp} | 2 +- ...aggregator.h => awapi_stop_reason_aggregator.hpp} | 2 +- ...publisher.h => awapi_vehicle_state_publisher.hpp} | 2 +- .../src/awapi_autoware_state_publisher.cpp | 2 +- awapi/awapi_awiv_adapter/src/awapi_autoware_util.cpp | 2 +- .../src/awapi_awiv_adapter_core.cpp | 2 +- .../src/awapi_awiv_adapter_node.cpp | 2 +- .../src/awapi_lane_change_state_publisher.cpp | 2 +- .../src/awapi_obstacle_avoidance_state_publisher.cpp | 2 +- .../src/awapi_stop_reason_aggregator.cpp | 2 +- .../src/awapi_vehicle_state_publisher.cpp | 2 +- 15 files changed, 19 insertions(+), 19 deletions(-) rename awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/{awapi_autoware_state_publisher.h => awapi_autoware_state_publisher.hpp} (98%) rename awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/{awapi_autoware_util.h => awapi_autoware_util.hpp} (100%) rename awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/{awapi_awiv_adapter_core.h => awapi_awiv_adapter_core.hpp} (95%) rename awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/{awapi_lane_change_state_publisher.h => awapi_lane_change_state_publisher.hpp} (96%) rename awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/{awapi_obstacle_avoidance_state_publisher.h => awapi_obstacle_avoidance_state_publisher.hpp} (96%) rename awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/{awapi_stop_reason_aggregator.h => awapi_stop_reason_aggregator.hpp} (97%) rename awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/{awapi_vehicle_state_publisher.h => awapi_vehicle_state_publisher.hpp} (98%) diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.h b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.hpp similarity index 98% rename from awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.h rename to awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.hpp index ad39da59c6a85..851c740958e82 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.h +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.hpp @@ -16,7 +16,7 @@ #include -#include +#include #include namespace autoware_api diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.h b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.hpp similarity index 100% rename from awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.h rename to awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.hpp diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.h b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.hpp similarity index 95% rename from awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.h rename to awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.hpp index f94520c3f8e64..fb4e6e9ecd30c 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.h +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.hpp @@ -32,12 +32,12 @@ #include #include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include namespace autoware_api { diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_lane_change_state_publisher.h b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_lane_change_state_publisher.hpp similarity index 96% rename from awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_lane_change_state_publisher.h rename to awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_lane_change_state_publisher.hpp index 498e2ae16291b..7b4326c483e9a 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_lane_change_state_publisher.h +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_lane_change_state_publisher.hpp @@ -16,7 +16,7 @@ #include -#include +#include #include namespace autoware_api diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_obstacle_avoidance_state_publisher.h b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_obstacle_avoidance_state_publisher.hpp similarity index 96% rename from awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_obstacle_avoidance_state_publisher.h rename to awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_obstacle_avoidance_state_publisher.hpp index ce5834bd9a638..8bbc8fbc02850 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_obstacle_avoidance_state_publisher.h +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_obstacle_avoidance_state_publisher.hpp @@ -16,7 +16,7 @@ #include -#include +#include #include namespace autoware_api diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_stop_reason_aggregator.h b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_stop_reason_aggregator.hpp similarity index 97% rename from awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_stop_reason_aggregator.h rename to awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_stop_reason_aggregator.hpp index 2763f408df3dc..7bc1c91d9fdad 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_stop_reason_aggregator.h +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_stop_reason_aggregator.hpp @@ -18,7 +18,7 @@ #include -#include +#include namespace autoware_api { diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_vehicle_state_publisher.h b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_vehicle_state_publisher.hpp similarity index 98% rename from awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_vehicle_state_publisher.h rename to awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_vehicle_state_publisher.hpp index 891f81a4ce7b6..ea7ec131909f0 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_vehicle_state_publisher.h +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_vehicle_state_publisher.hpp @@ -16,7 +16,7 @@ #include -#include +#include #include namespace autoware_api diff --git a/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp b/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp index e5646959532cc..1bd9a23701f3f 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp @@ -14,7 +14,7 @@ * limitations under the License. */ -#include +#include #include namespace autoware_api diff --git a/awapi/awapi_awiv_adapter/src/awapi_autoware_util.cpp b/awapi/awapi_awiv_adapter/src/awapi_autoware_util.cpp index b6341ed8a5528..a9b3441e57e84 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_autoware_util.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_autoware_util.cpp @@ -14,7 +14,7 @@ * limitations under the License. */ -#include +#include namespace autoware_api { diff --git a/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp b/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp index 535e7de1e0a26..f1a1e3907b75d 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp @@ -14,7 +14,7 @@ * limitations under the License. */ -#include +#include #include namespace autoware_api diff --git a/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_node.cpp b/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_node.cpp index 48ccc3d3ec12a..fa49de99e41a1 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_node.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_node.cpp @@ -16,7 +16,7 @@ #include -#include +#include int main(int argc, char ** argv) { diff --git a/awapi/awapi_awiv_adapter/src/awapi_lane_change_state_publisher.cpp b/awapi/awapi_awiv_adapter/src/awapi_lane_change_state_publisher.cpp index 8cd28dbf1907a..7e589840fd4a3 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_lane_change_state_publisher.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_lane_change_state_publisher.cpp @@ -14,7 +14,7 @@ * limitations under the License. */ -#include +#include namespace autoware_api { diff --git a/awapi/awapi_awiv_adapter/src/awapi_obstacle_avoidance_state_publisher.cpp b/awapi/awapi_awiv_adapter/src/awapi_obstacle_avoidance_state_publisher.cpp index 2c723703bf700..550e887fde695 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_obstacle_avoidance_state_publisher.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_obstacle_avoidance_state_publisher.cpp @@ -14,7 +14,7 @@ * limitations under the License. */ -#include +#include namespace autoware_api { diff --git a/awapi/awapi_awiv_adapter/src/awapi_stop_reason_aggregator.cpp b/awapi/awapi_awiv_adapter/src/awapi_stop_reason_aggregator.cpp index 21d1bd4a1ae49..944b5ce90768e 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_stop_reason_aggregator.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_stop_reason_aggregator.cpp @@ -14,7 +14,7 @@ * limitations under the License. */ -#include +#include namespace autoware_api { diff --git a/awapi/awapi_awiv_adapter/src/awapi_vehicle_state_publisher.cpp b/awapi/awapi_awiv_adapter/src/awapi_vehicle_state_publisher.cpp index dc4a6865b21d5..115692cee2d9e 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_vehicle_state_publisher.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_vehicle_state_publisher.cpp @@ -14,7 +14,7 @@ * limitations under the License. */ -#include +#include namespace autoware_api { From a80598d1cb256a151a22ce9b8ae3448b4445f1f9 Mon Sep 17 00:00:00 2001 From: Nikolai Morin Date: Thu, 3 Dec 2020 17:21:16 +0100 Subject: [PATCH 19/71] Adjust copyright notice on 532 out of 699 source files (#143) Signed-off-by: tanaka3 --- .../awapi_autoware_state_publisher.hpp | 28 +++++++++---------- .../awapi_autoware_util.hpp | 28 +++++++++---------- .../awapi_awiv_adapter_core.hpp | 28 +++++++++---------- .../awapi_lane_change_state_publisher.hpp | 28 +++++++++---------- ...api_obstacle_avoidance_state_publisher.hpp | 28 +++++++++---------- .../awapi_stop_reason_aggregator.hpp | 28 +++++++++---------- .../awapi_vehicle_state_publisher.hpp | 28 +++++++++---------- .../src/awapi_autoware_state_publisher.cpp | 28 +++++++++---------- .../src/awapi_autoware_util.cpp | 28 +++++++++---------- .../src/awapi_awiv_adapter_core.cpp | 28 +++++++++---------- .../src/awapi_awiv_adapter_node.cpp | 28 +++++++++---------- .../src/awapi_lane_change_state_publisher.cpp | 28 +++++++++---------- ...api_obstacle_avoidance_state_publisher.cpp | 28 +++++++++---------- .../src/awapi_stop_reason_aggregator.cpp | 28 +++++++++---------- .../src/awapi_vehicle_state_publisher.cpp | 28 +++++++++---------- 15 files changed, 195 insertions(+), 225 deletions(-) diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.hpp b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.hpp index 851c740958e82..b31bf86eb1cdb 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.hpp +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.hpp @@ -1,18 +1,16 @@ -/* - * Copyright 2020 Tier IV, Inc. All rights reserved. - * - * 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. - */ +// Copyright 2020 Tier IV, Inc. +// +// 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. #include diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.hpp b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.hpp index 8e7c38282b16b..cba3910977089 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.hpp +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.hpp @@ -1,18 +1,16 @@ -/* - * Copyright 2020 Tier IV, Inc. All rights reserved. - * - * 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. - */ +// Copyright 2020 Tier IV, Inc. +// +// 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. #pragma once diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.hpp b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.hpp index fb4e6e9ecd30c..9564cc58959a1 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.hpp +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.hpp @@ -1,18 +1,16 @@ -/* - * Copyright 2020 Tier IV, Inc. All rights reserved. - * - * 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. - */ +// Copyright 2020 Tier IV, Inc. +// +// 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. #include diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_lane_change_state_publisher.hpp b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_lane_change_state_publisher.hpp index 7b4326c483e9a..78ce304f43d9b 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_lane_change_state_publisher.hpp +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_lane_change_state_publisher.hpp @@ -1,18 +1,16 @@ -/* - * Copyright 2020 Tier IV, Inc. All rights reserved. - * - * 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. - */ +// Copyright 2020 Tier IV, Inc. +// +// 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. #include diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_obstacle_avoidance_state_publisher.hpp b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_obstacle_avoidance_state_publisher.hpp index 8bbc8fbc02850..b1645297d4f87 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_obstacle_avoidance_state_publisher.hpp +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_obstacle_avoidance_state_publisher.hpp @@ -1,18 +1,16 @@ -/* - * Copyright 2020 Tier IV, Inc. All rights reserved. - * - * 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. - */ +// Copyright 2020 Tier IV, Inc. +// +// 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. #include diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_stop_reason_aggregator.hpp b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_stop_reason_aggregator.hpp index 7bc1c91d9fdad..dcd4ea266f603 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_stop_reason_aggregator.hpp +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_stop_reason_aggregator.hpp @@ -1,18 +1,16 @@ -/* - * Copyright 2020 Tier IV, Inc. All rights reserved. - * - * 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. - */ +// Copyright 2020 Tier IV, Inc. +// +// 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. #pragma once diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_vehicle_state_publisher.hpp b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_vehicle_state_publisher.hpp index ea7ec131909f0..c306c0371e223 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_vehicle_state_publisher.hpp +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_vehicle_state_publisher.hpp @@ -1,18 +1,16 @@ -/* - * Copyright 2020 Tier IV, Inc. All rights reserved. - * - * 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. - */ +// Copyright 2020 Tier IV, Inc. +// +// 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. #include diff --git a/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp b/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp index 1bd9a23701f3f..676273bcad8f3 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp @@ -1,18 +1,16 @@ -/* - * Copyright 2020 Tier IV, Inc. All rights reserved. - * - * 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. - */ +// Copyright 2020 Tier IV, Inc. +// +// 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. #include #include diff --git a/awapi/awapi_awiv_adapter/src/awapi_autoware_util.cpp b/awapi/awapi_awiv_adapter/src/awapi_autoware_util.cpp index a9b3441e57e84..0374dd8e00811 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_autoware_util.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_autoware_util.cpp @@ -1,18 +1,16 @@ -/* - * Copyright 2020 Tier IV, Inc. All rights reserved. - * - * 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. - */ +// Copyright 2020 Tier IV, Inc. +// +// 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. #include diff --git a/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp b/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp index f1a1e3907b75d..91cd73550a06a 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp @@ -1,18 +1,16 @@ -/* - * Copyright 2020 Tier IV, Inc. All rights reserved. - * - * 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. - */ +// Copyright 2020 Tier IV, Inc. +// +// 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. #include #include diff --git a/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_node.cpp b/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_node.cpp index fa49de99e41a1..b4bcd2f54b3b7 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_node.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_node.cpp @@ -1,18 +1,16 @@ -/* - * Copyright 2020 Tier IV, Inc. All rights reserved. - * - * 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. - */ +// Copyright 2020 Tier IV, Inc. +// +// 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. #include diff --git a/awapi/awapi_awiv_adapter/src/awapi_lane_change_state_publisher.cpp b/awapi/awapi_awiv_adapter/src/awapi_lane_change_state_publisher.cpp index 7e589840fd4a3..e72f0227742cd 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_lane_change_state_publisher.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_lane_change_state_publisher.cpp @@ -1,18 +1,16 @@ -/* - * Copyright 2020 Tier IV, Inc. All rights reserved. - * - * 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. - */ +// Copyright 2020 Tier IV, Inc. +// +// 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. #include diff --git a/awapi/awapi_awiv_adapter/src/awapi_obstacle_avoidance_state_publisher.cpp b/awapi/awapi_awiv_adapter/src/awapi_obstacle_avoidance_state_publisher.cpp index 550e887fde695..3202016ca8efa 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_obstacle_avoidance_state_publisher.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_obstacle_avoidance_state_publisher.cpp @@ -1,18 +1,16 @@ -/* - * Copyright 2020 Tier IV, Inc. All rights reserved. - * - * 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. - */ +// Copyright 2020 Tier IV, Inc. +// +// 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. #include diff --git a/awapi/awapi_awiv_adapter/src/awapi_stop_reason_aggregator.cpp b/awapi/awapi_awiv_adapter/src/awapi_stop_reason_aggregator.cpp index 944b5ce90768e..c613a6faa02fc 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_stop_reason_aggregator.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_stop_reason_aggregator.cpp @@ -1,18 +1,16 @@ -/* - * Copyright 2020 Tier IV, Inc. All rights reserved. - * - * 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. - */ +// Copyright 2020 Tier IV, Inc. +// +// 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. #include diff --git a/awapi/awapi_awiv_adapter/src/awapi_vehicle_state_publisher.cpp b/awapi/awapi_awiv_adapter/src/awapi_vehicle_state_publisher.cpp index 115692cee2d9e..30c7b3ecf4caa 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_vehicle_state_publisher.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_vehicle_state_publisher.cpp @@ -1,18 +1,16 @@ -/* - * Copyright 2020 Tier IV, Inc. All rights reserved. - * - * 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. - */ +// Copyright 2020 Tier IV, Inc. +// +// 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. #include From a04d2c9d0a7b42028aaacd03f2473aa887fd38c4 Mon Sep 17 00:00:00 2001 From: Nikolai Morin Date: Mon, 7 Dec 2020 09:52:36 +0100 Subject: [PATCH 20/71] Use quotes for includes where appropriate (#144) * Use quotes for includes where appropriate * Fix lint tests * Make tests pass hopefully Signed-off-by: tanaka3 --- .../awapi_autoware_state_publisher.hpp | 6 +-- .../awapi_autoware_util.hpp | 44 +++++++++---------- .../awapi_awiv_adapter_core.hpp | 44 +++++++++---------- .../awapi_lane_change_state_publisher.hpp | 6 +-- ...api_obstacle_avoidance_state_publisher.hpp | 6 +-- .../awapi_stop_reason_aggregator.hpp | 4 +- .../awapi_vehicle_state_publisher.hpp | 6 +-- .../src/awapi_autoware_state_publisher.cpp | 2 +- .../src/awapi_autoware_util.cpp | 2 +- .../src/awapi_awiv_adapter_core.cpp | 2 +- .../src/awapi_awiv_adapter_node.cpp | 4 +- .../src/awapi_lane_change_state_publisher.cpp | 2 +- ...api_obstacle_avoidance_state_publisher.cpp | 2 +- .../src/awapi_stop_reason_aggregator.cpp | 2 +- .../src/awapi_vehicle_state_publisher.cpp | 2 +- 15 files changed, 67 insertions(+), 67 deletions(-) diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.hpp b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.hpp index b31bf86eb1cdb..516e33ee2197d 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.hpp +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "rclcpp/rclcpp.hpp" -#include -#include +#include "awapi_awiv_adapter/awapi_autoware_util.hpp" +#include "autoware_api_msgs/msg/awapi_autoware_status.hpp" namespace autoware_api { diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.hpp b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.hpp index cba3910977089..7ce9d11fe8d02 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.hpp +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.hpp @@ -17,29 +17,29 @@ #ifndef AWAPI_AUTOWARE_UTIL_H #define AWAPI_AUTOWARE_UTIL_H -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include "tf2/utils.h" +#include "tf2_ros/transform_broadcaster.h" +#include "tf2_ros/transform_listener.h" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "pacmod_msgs/msg/global_rpt.hpp" +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/nav_sat_fix.hpp" +#include "std_msgs/msg/bool.hpp" +#include "std_msgs/msg/float32.hpp" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include "autoware_control_msgs/msg/emergency_mode.hpp" +#include "autoware_control_msgs/msg/gate_mode.hpp" +#include "autoware_planning_msgs/msg/path.hpp" +#include "autoware_planning_msgs/msg/stop_reason_array.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "autoware_system_msgs/msg/autoware_state.hpp" +#include "autoware_vehicle_msgs/msg/control_mode.hpp" +#include "autoware_vehicle_msgs/msg/shift_stamped.hpp" +#include "autoware_vehicle_msgs/msg/steering.hpp" +#include "autoware_vehicle_msgs/msg/turn_signal.hpp" +#include "autoware_vehicle_msgs/msg/vehicle_command.hpp" +#include "diagnostic_msgs/msg/diagnostic_array.hpp" namespace autoware_api { diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.hpp b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.hpp index 9564cc58959a1..798bb90a01985 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.hpp +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.hpp @@ -12,30 +12,30 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "rclcpp/rclcpp.hpp" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include "autoware_control_msgs/msg/emergency_mode.hpp" +#include "autoware_control_msgs/msg/gate_mode.hpp" +#include "autoware_planning_msgs/msg/path.hpp" +#include "autoware_planning_msgs/msg/stop_reason_array.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "autoware_system_msgs/msg/autoware_state.hpp" +#include "autoware_vehicle_msgs/msg/control_mode.hpp" +#include "autoware_vehicle_msgs/msg/shift_stamped.hpp" +#include "autoware_vehicle_msgs/msg/steering.hpp" +#include "autoware_vehicle_msgs/msg/turn_signal.hpp" +#include "autoware_vehicle_msgs/msg/vehicle_command.hpp" +#include "diagnostic_msgs/msg/diagnostic_array.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "pacmod_msgs/msg/global_rpt.hpp" +#include "sensor_msgs/msg/nav_sat_fix.hpp" -#include -#include -#include -#include -#include -#include +#include "awapi_awiv_adapter/awapi_autoware_state_publisher.hpp" +#include "awapi_awiv_adapter/awapi_autoware_util.hpp" +#include "awapi_awiv_adapter/awapi_lane_change_state_publisher.hpp" +#include "awapi_awiv_adapter/awapi_obstacle_avoidance_state_publisher.hpp" +#include "awapi_awiv_adapter/awapi_stop_reason_aggregator.hpp" +#include "awapi_awiv_adapter/awapi_vehicle_state_publisher.hpp" namespace autoware_api { diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_lane_change_state_publisher.hpp b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_lane_change_state_publisher.hpp index 78ce304f43d9b..40fa113647b27 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_lane_change_state_publisher.hpp +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_lane_change_state_publisher.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "rclcpp/rclcpp.hpp" -#include -#include +#include "awapi_awiv_adapter/awapi_autoware_util.hpp" +#include "autoware_api_msgs/msg/lane_change_status.hpp" namespace autoware_api { diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_obstacle_avoidance_state_publisher.hpp b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_obstacle_avoidance_state_publisher.hpp index b1645297d4f87..293fab27d37e3 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_obstacle_avoidance_state_publisher.hpp +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_obstacle_avoidance_state_publisher.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "rclcpp/rclcpp.hpp" -#include -#include +#include "awapi_awiv_adapter/awapi_autoware_util.hpp" +#include "autoware_api_msgs/msg/obstacle_avoidance_status.hpp" namespace autoware_api { diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_stop_reason_aggregator.hpp b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_stop_reason_aggregator.hpp index dcd4ea266f603..c8e919d04ebd4 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_stop_reason_aggregator.hpp +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_stop_reason_aggregator.hpp @@ -14,9 +14,9 @@ #pragma once -#include +#include "rclcpp/rclcpp.hpp" -#include +#include "awapi_awiv_adapter/awapi_autoware_util.hpp" namespace autoware_api { diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_vehicle_state_publisher.hpp b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_vehicle_state_publisher.hpp index c306c0371e223..52ce230250ef3 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_vehicle_state_publisher.hpp +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_vehicle_state_publisher.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "rclcpp/rclcpp.hpp" -#include -#include +#include "awapi_awiv_adapter/awapi_autoware_util.hpp" +#include "autoware_api_msgs/msg/awapi_vehicle_status.hpp" namespace autoware_api { diff --git a/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp b/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp index 676273bcad8f3..8244d01b6a81a 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "awapi_awiv_adapter/awapi_autoware_state_publisher.hpp" #include namespace autoware_api diff --git a/awapi/awapi_awiv_adapter/src/awapi_autoware_util.cpp b/awapi/awapi_awiv_adapter/src/awapi_autoware_util.cpp index 0374dd8e00811..731be64ca4071 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_autoware_util.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_autoware_util.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "awapi_awiv_adapter/awapi_autoware_util.hpp" namespace autoware_api { diff --git a/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp b/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp index 91cd73550a06a..6b91ebfa0282f 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "awapi_awiv_adapter/awapi_awiv_adapter_core.hpp" #include namespace autoware_api diff --git a/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_node.cpp b/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_node.cpp index b4bcd2f54b3b7..6e395504c2f67 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_node.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_node.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "rclcpp/rclcpp.hpp" -#include +#include "awapi_awiv_adapter/awapi_awiv_adapter_core.hpp" int main(int argc, char ** argv) { diff --git a/awapi/awapi_awiv_adapter/src/awapi_lane_change_state_publisher.cpp b/awapi/awapi_awiv_adapter/src/awapi_lane_change_state_publisher.cpp index e72f0227742cd..42504f669e7c9 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_lane_change_state_publisher.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_lane_change_state_publisher.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "awapi_awiv_adapter/awapi_lane_change_state_publisher.hpp" namespace autoware_api { diff --git a/awapi/awapi_awiv_adapter/src/awapi_obstacle_avoidance_state_publisher.cpp b/awapi/awapi_awiv_adapter/src/awapi_obstacle_avoidance_state_publisher.cpp index 3202016ca8efa..efe5f620eac00 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_obstacle_avoidance_state_publisher.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_obstacle_avoidance_state_publisher.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "awapi_awiv_adapter/awapi_obstacle_avoidance_state_publisher.hpp" namespace autoware_api { diff --git a/awapi/awapi_awiv_adapter/src/awapi_stop_reason_aggregator.cpp b/awapi/awapi_awiv_adapter/src/awapi_stop_reason_aggregator.cpp index c613a6faa02fc..164ce745804ef 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_stop_reason_aggregator.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_stop_reason_aggregator.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "awapi_awiv_adapter/awapi_stop_reason_aggregator.hpp" namespace autoware_api { diff --git a/awapi/awapi_awiv_adapter/src/awapi_vehicle_state_publisher.cpp b/awapi/awapi_awiv_adapter/src/awapi_vehicle_state_publisher.cpp index 30c7b3ecf4caa..8c83bd2cc94f6 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_vehicle_state_publisher.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_vehicle_state_publisher.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "awapi_awiv_adapter/awapi_vehicle_state_publisher.hpp" namespace autoware_api { From e6fdda5596e14fb540d8c77c1e9af0efac4693d7 Mon Sep 17 00:00:00 2001 From: Nikolai Morin Date: Tue, 8 Dec 2020 15:10:45 +0100 Subject: [PATCH 21/71] Run uncrustify on the entire Pilot.Auto codebase (#151) * Run uncrustify on the entire Pilot.Auto codebase * Exclude open PRs Signed-off-by: tanaka3 --- .../awapi_autoware_state_publisher.hpp | 2 +- .../awapi_lane_change_state_publisher.hpp | 2 +- .../awapi_obstacle_avoidance_state_publisher.hpp | 2 +- .../awapi_stop_reason_aggregator.hpp | 2 +- .../awapi_vehicle_state_publisher.hpp | 2 +- .../src/awapi_autoware_state_publisher.cpp | 8 +++++--- awapi/awapi_awiv_adapter/src/awapi_autoware_util.cpp | 2 +- .../src/awapi_awiv_adapter_core.cpp | 12 ++++++++---- .../src/awapi_awiv_adapter_node.cpp | 2 +- .../src/awapi_lane_change_state_publisher.cpp | 2 +- .../src/awapi_obstacle_avoidance_state_publisher.cpp | 3 ++- .../src/awapi_stop_reason_aggregator.cpp | 9 ++++++--- .../src/awapi_vehicle_state_publisher.cpp | 6 +++--- 13 files changed, 32 insertions(+), 22 deletions(-) diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.hpp b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.hpp index 516e33ee2197d..41a5060cd5bc0 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.hpp +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.hpp @@ -22,7 +22,7 @@ namespace autoware_api class AutowareIvAutowareStatePublisher { public: - AutowareIvAutowareStatePublisher(rclcpp::Node& node); + AutowareIvAutowareStatePublisher(rclcpp::Node & node); void statePublisher(const AutowareInfo & aw_info); private: diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_lane_change_state_publisher.hpp b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_lane_change_state_publisher.hpp index 40fa113647b27..030631d6ab106 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_lane_change_state_publisher.hpp +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_lane_change_state_publisher.hpp @@ -22,7 +22,7 @@ namespace autoware_api class AutowareIvLaneChangeStatePublisher { public: - AutowareIvLaneChangeStatePublisher(rclcpp::Node& node); + AutowareIvLaneChangeStatePublisher(rclcpp::Node & node); void statePublisher(const AutowareInfo & aw_info); private: diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_obstacle_avoidance_state_publisher.hpp b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_obstacle_avoidance_state_publisher.hpp index 293fab27d37e3..533ce49e1b76b 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_obstacle_avoidance_state_publisher.hpp +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_obstacle_avoidance_state_publisher.hpp @@ -22,7 +22,7 @@ namespace autoware_api class AutowareIvObstacleAvoidanceStatePublisher { public: - AutowareIvObstacleAvoidanceStatePublisher(rclcpp::Node& node); + AutowareIvObstacleAvoidanceStatePublisher(rclcpp::Node & node); void statePublisher(const AutowareInfo & aw_info); private: diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_stop_reason_aggregator.hpp b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_stop_reason_aggregator.hpp index c8e919d04ebd4..d47baca0ce726 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_stop_reason_aggregator.hpp +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_stop_reason_aggregator.hpp @@ -23,7 +23,7 @@ namespace autoware_api class AutowareIvStopReasonAggregator { public: - AutowareIvStopReasonAggregator(rclcpp::Node& node, const double timeout); + AutowareIvStopReasonAggregator(rclcpp::Node & node, const double timeout); autoware_planning_msgs::msg::StopReasonArray::ConstSharedPtr updateStopReasonArray( const autoware_planning_msgs::msg::StopReasonArray::ConstSharedPtr & msg_ptr); diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_vehicle_state_publisher.hpp b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_vehicle_state_publisher.hpp index 52ce230250ef3..5d6db0a0eea6f 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_vehicle_state_publisher.hpp +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_vehicle_state_publisher.hpp @@ -22,7 +22,7 @@ namespace autoware_api class AutowareIvVehicleStatePublisher { public: - AutowareIvVehicleStatePublisher(rclcpp::Node& node); + AutowareIvVehicleStatePublisher(rclcpp::Node & node); void statePublisher(const AutowareInfo & aw_info); private: diff --git a/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp b/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp index 8244d01b6a81a..7eb9d88816b65 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp @@ -17,7 +17,7 @@ namespace autoware_api { -AutowareIvAutowareStatePublisher::AutowareIvAutowareStatePublisher(rclcpp::Node& node) +AutowareIvAutowareStatePublisher::AutowareIvAutowareStatePublisher(rclcpp::Node & node) : logger_(node.get_logger().get_child("awapi_awiv_autoware_state_publisher")), clock_(node.get_clock()), arrived_goal_(false) @@ -150,13 +150,15 @@ bool AutowareIvAutowareStatePublisher::isGoal( arrived_goal_ = true; } else if ( prev_state_ == autoware_system_msgs::msg::AutowareState::DRIVING && - aw_state == autoware_system_msgs::msg::AutowareState::WAITING_FOR_ROUTE) { + aw_state == autoware_system_msgs::msg::AutowareState::WAITING_FOR_ROUTE) + { arrived_goal_ = true; } if ( aw_state == autoware_system_msgs::msg::AutowareState::WAITING_FOR_ENGAGE || - aw_state == autoware_system_msgs::msg::AutowareState::DRIVING) { + aw_state == autoware_system_msgs::msg::AutowareState::DRIVING) + { //cancel goal state arrived_goal_ = false; } diff --git a/awapi/awapi_awiv_adapter/src/awapi_autoware_util.cpp b/awapi/awapi_awiv_adapter/src/awapi_autoware_util.cpp index 731be64ca4071..c55c2a16d2ba0 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_autoware_util.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_autoware_util.cpp @@ -21,4 +21,4 @@ double lowpass_filter(const double current_value, const double prev_value, const return gain * prev_value + (1.0 - gain) * current_value; } -} // namespace autoware_api \ No newline at end of file +} // namespace autoware_api diff --git a/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp b/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp index 6b91ebfa0282f..d9dc5d4ce3f40 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp @@ -31,7 +31,9 @@ AutowareIvAdapter::AutowareIvAdapter() // setup instance vehicle_state_publisher_ = std::make_unique(*this); autoware_state_publisher_ = std::make_unique(*this); - stop_reason_aggreagator_ = std::make_unique(*this, stop_reason_timeout_); + stop_reason_aggreagator_ = std::make_unique( + *this, + stop_reason_timeout_); lane_change_state_publisher_ = std::make_unique(*this); obstacle_avoidance_state_publisher_ = std::make_unique(*this); @@ -78,8 +80,8 @@ AutowareIvAdapter::AutowareIvAdapter() std::bind(&AutowareIvAdapter::callbackLaneObstacleAvoidReady, this, _1)); sub_obstacle_avoid_candidate_ = this->create_subscription( - "input/obstacle_avoid_candidate_path", 1, - std::bind(&AutowareIvAdapter::callbackLaneObstacleAvoidCandidatePath, this, _1)); + "input/obstacle_avoid_candidate_path", 1, + std::bind(&AutowareIvAdapter::callbackLaneObstacleAvoidCandidatePath, this, _1)); // timer auto timer_callback = std::bind(&AutowareIvAdapter::timerCallback, this); @@ -169,7 +171,9 @@ void AutowareIvAdapter::getCurrentPose() ps.pose.orientation = transform.transform.rotation; aw_info_.current_pose_ptr = std::make_shared(ps); } catch (tf2::TransformException & ex) { - RCLCPP_DEBUG_STREAM_THROTTLE(get_logger(), *this->get_clock(), 2000 /* ms */, "cannot get self pose"); + RCLCPP_DEBUG_STREAM_THROTTLE( + get_logger(), + *this->get_clock(), 2000 /* ms */, "cannot get self pose"); } } diff --git a/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_node.cpp b/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_node.cpp index 6e395504c2f67..8de3904a87630 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_node.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_node.cpp @@ -23,4 +23,4 @@ int main(int argc, char ** argv) rclcpp::shutdown(); return 0; -} \ No newline at end of file +} diff --git a/awapi/awapi_awiv_adapter/src/awapi_lane_change_state_publisher.cpp b/awapi/awapi_awiv_adapter/src/awapi_lane_change_state_publisher.cpp index 42504f669e7c9..17c5deb08a942 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_lane_change_state_publisher.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_lane_change_state_publisher.cpp @@ -16,7 +16,7 @@ namespace autoware_api { -AutowareIvLaneChangeStatePublisher::AutowareIvLaneChangeStatePublisher(rclcpp::Node& node) +AutowareIvLaneChangeStatePublisher::AutowareIvLaneChangeStatePublisher(rclcpp::Node & node) : logger_(node.get_logger().get_child("awapi_awiv_lane_change_state_publisher")), clock_(node.get_clock()) { diff --git a/awapi/awapi_awiv_adapter/src/awapi_obstacle_avoidance_state_publisher.cpp b/awapi/awapi_awiv_adapter/src/awapi_obstacle_avoidance_state_publisher.cpp index efe5f620eac00..77ed7c9334f37 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_obstacle_avoidance_state_publisher.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_obstacle_avoidance_state_publisher.cpp @@ -16,7 +16,8 @@ namespace autoware_api { -AutowareIvObstacleAvoidanceStatePublisher::AutowareIvObstacleAvoidanceStatePublisher(rclcpp::Node& node) +AutowareIvObstacleAvoidanceStatePublisher::AutowareIvObstacleAvoidanceStatePublisher( + rclcpp::Node & node) : logger_(node.get_logger().get_child("awapi_awiv_obstacle_avoidance_state_publisher")), clock_(node.get_clock()) { diff --git a/awapi/awapi_awiv_adapter/src/awapi_stop_reason_aggregator.cpp b/awapi/awapi_awiv_adapter/src/awapi_stop_reason_aggregator.cpp index 164ce745804ef..b324dc9a7c571 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_stop_reason_aggregator.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_stop_reason_aggregator.cpp @@ -16,7 +16,9 @@ namespace autoware_api { -AutowareIvStopReasonAggregator::AutowareIvStopReasonAggregator(rclcpp::Node& node, const double timeout) +AutowareIvStopReasonAggregator::AutowareIvStopReasonAggregator( + rclcpp::Node & node, + const double timeout) : logger_(node.get_logger().get_child("awapi_awiv_stop_reason_aggregator")), clock_(node.get_clock()), timeout_(timeout) @@ -82,7 +84,8 @@ void AutowareIvStopReasonAggregator::applyTimeOut() for (int i = stop_reason_array_vec_.size() - 1; i >= 0; i--) { if ( (current_time - rclcpp::Time(stop_reason_array_vec_.at(i).header.stamp)).seconds() > - timeout_) { + timeout_) + { remove_idx.emplace_back(i); } } @@ -133,4 +136,4 @@ AutowareIvStopReasonAggregator::makeStopReasonArray() return std::make_shared(stop_reason_array_msg); } -} // namespace autoware_api \ No newline at end of file +} // namespace autoware_api diff --git a/awapi/awapi_awiv_adapter/src/awapi_vehicle_state_publisher.cpp b/awapi/awapi_awiv_adapter/src/awapi_vehicle_state_publisher.cpp index 8c83bd2cc94f6..9c9f070961e2c 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_vehicle_state_publisher.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_vehicle_state_publisher.cpp @@ -16,7 +16,7 @@ namespace autoware_api { -AutowareIvVehicleStatePublisher::AutowareIvVehicleStatePublisher(rclcpp::Node& node) +AutowareIvVehicleStatePublisher::AutowareIvVehicleStatePublisher(rclcpp::Node & node) : logger_(node.get_logger().get_child("awapi_awiv_vehicle_state_publisher")), clock_(node.get_clock()), prev_accel_(0.0) @@ -96,7 +96,7 @@ void AutowareIvVehicleStatePublisher::getSteerInfo( const double ds = steer_ptr->data - previous_steer_ptr_->data; const double dt = std::max( (rclcpp::Time(steer_ptr->header.stamp) - rclcpp::Time(previous_steer_ptr_->header.stamp)) - .seconds(), + .seconds(), 1e-03); const double steer_vel = ds / dt; @@ -156,7 +156,7 @@ void AutowareIvVehicleStatePublisher::getTwistInfo( const double dv = twist_ptr->twist.linear.x - previous_twist_ptr_->twist.linear.x; const double dt = std::max( (rclcpp::Time(twist_ptr->header.stamp) - rclcpp::Time(previous_twist_ptr_->header.stamp)) - .seconds(), + .seconds(), 1e-03); const double accel = dv / dt; From 0b3d141ef35255f86386244ea5f69f8a4b3113dd Mon Sep 17 00:00:00 2001 From: Jilada Eccleston Date: Wed, 16 Dec 2020 19:03:55 +0900 Subject: [PATCH 22/71] Add linters (#227) Signed-off-by: tanaka3 --- awapi/awapi_awiv_adapter/CMakeLists.txt | 7 +++++++ awapi/awapi_awiv_adapter/package.xml | 2 ++ 2 files changed, 9 insertions(+) diff --git a/awapi/awapi_awiv_adapter/CMakeLists.txt b/awapi/awapi_awiv_adapter/CMakeLists.txt index 3bdd783e82998..f200bd7dfdeb9 100644 --- a/awapi/awapi_awiv_adapter/CMakeLists.txt +++ b/awapi/awapi_awiv_adapter/CMakeLists.txt @@ -3,6 +3,8 @@ project(awapi_awiv_adapter) if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wno-unused-parameter -Wall -Wextra -Wpedantic -Werror) @@ -22,4 +24,9 @@ ament_auto_add_executable(awapi_awiv_adapter src/awapi_autoware_util.cpp ) +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + ament_auto_package(INSTALL_TO_SHARE launch) diff --git a/awapi/awapi_awiv_adapter/package.xml b/awapi/awapi_awiv_adapter/package.xml index 2b1e551974f5c..ce2fbf373bd59 100644 --- a/awapi/awapi_awiv_adapter/package.xml +++ b/awapi/awapi_awiv_adapter/package.xml @@ -26,6 +26,8 @@ autoware_perception_msgs topic_tools + ament_lint_auto + ament_cmake_cppcheck ament_cmake From dc6a7ca25cb69083d7eda3156cd4e1ff64bee3f4 Mon Sep 17 00:00:00 2001 From: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Date: Thu, 4 Feb 2021 14:22:22 +0900 Subject: [PATCH 23/71] Ros2 v0.8.0 awapi awiv adapter (#282) Signed-off-by: tanaka3 --- awapi/awapi_awiv_adapter/CMakeLists.txt | 2 + awapi/awapi_awiv_adapter/Readme.md | 152 +++++++++++++++--- .../awapi_autoware_state_publisher.hpp | 21 ++- .../awapi_autoware_util.hpp | 79 ++++++++- .../awapi_awiv_adapter_core.hpp | 44 ++++- .../awapi_lane_change_state_publisher.hpp | 7 +- .../awapi_max_velocity_publisher.hpp | 43 +++++ ...api_obstacle_avoidance_state_publisher.hpp | 7 +- .../awapi_awiv_adapter/awapi_pacmod_util.hpp | 39 +++++ .../awapi_stop_reason_aggregator.hpp | 29 +++- .../awapi_vehicle_state_publisher.hpp | 11 +- .../launch/awapi_awiv_adapter.launch.xml | 103 +++++++++--- awapi/awapi_awiv_adapter/package.xml | 2 +- .../src/awapi_autoware_state_publisher.cpp | 127 +++++++++++++-- .../src/awapi_autoware_util.cpp | 81 ++++++++++ .../src/awapi_awiv_adapter_core.cpp | 108 +++++++++++-- .../src/awapi_awiv_adapter_node.cpp | 2 + .../src/awapi_lane_change_state_publisher.cpp | 2 +- .../src/awapi_max_velocity_publisher.cpp | 58 +++++++ ...api_obstacle_avoidance_state_publisher.cpp | 2 +- .../src/awapi_pacmod_util.cpp | 90 +++++++++++ .../src/awapi_stop_reason_aggregator.cpp | 95 ++++++++--- .../src/awapi_vehicle_state_publisher.cpp | 21 +-- 23 files changed, 990 insertions(+), 135 deletions(-) create mode 100644 awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_max_velocity_publisher.hpp create mode 100644 awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_pacmod_util.hpp create mode 100644 awapi/awapi_awiv_adapter/src/awapi_max_velocity_publisher.cpp create mode 100644 awapi/awapi_awiv_adapter/src/awapi_pacmod_util.cpp diff --git a/awapi/awapi_awiv_adapter/CMakeLists.txt b/awapi/awapi_awiv_adapter/CMakeLists.txt index f200bd7dfdeb9..aa06b339e0a34 100644 --- a/awapi/awapi_awiv_adapter/CMakeLists.txt +++ b/awapi/awapi_awiv_adapter/CMakeLists.txt @@ -21,7 +21,9 @@ ament_auto_add_executable(awapi_awiv_adapter src/awapi_stop_reason_aggregator.cpp src/awapi_lane_change_state_publisher.cpp src/awapi_obstacle_avoidance_state_publisher.cpp + src/awapi_max_velocity_publisher.cpp src/awapi_autoware_util.cpp + src/awapi_pacmod_util.cpp ) if(BUILD_TESTING) diff --git a/awapi/awapi_awiv_adapter/Readme.md b/awapi/awapi_awiv_adapter/Readme.md index febbb275a59bc..f7a1741ca833e 100644 --- a/awapi/awapi_awiv_adapter/Readme.md +++ b/awapi/awapi_awiv_adapter/Readme.md @@ -14,15 +14,15 @@ | ✓ | std_msgs/Header | header | | | | ✓ | geometry_msgs/Pose | pose | position:[m] | | | ✓ | awapi_awiv_adapter/Euler | eulerangle | [rad] | roll/pitch/yaw | -| | awapi_awiv_adapter/Latlon | latlon | | lat/lon/alt | +| | geographic_msgs/GeoPoint | geo_point | | lat/lon/alt | | ✓ | float64 | velocity | [m/s] | | | ✓ | float64 | acceleration | [m/ss] | calculate from velocity in awapi_adapter | | ✓ | float64 | steering | [rad] | | | ✓ | float64 | steering_velocity | [rad/s] | calculate from steering in awapi_adapter | | ✓ | float64 | angular_velocity | [rad/s] | | -| | int32 | gear | accroding to autoware_vehicle_msgs/Shift | | +| | int32 | gear | according to autoware_vehicle_msgs/Shift | | | | float32 | energy_level | | available only for golf-cart | -| ✓ | int32 | turn_signal | accroding to autoware_vehicle_msgs/TurnSignal | | +| ✓ | int32 | turn_signal | according to autoware_vehicle_msgs/TurnSignal | | | ✓ | float64 | target_velocity | [m/s] | | | ✓ | float64 | target_acceleration | [m/ss] | | | ✓ | float64 | target_steering | [rad] | | @@ -37,12 +37,19 @@ | --- | :------------------------------------- | :---------------- | :--------------------------------------------- | :-------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | | ✓ | std_msgs/Header | header | | | | ✓ | string | autoware_state | | | -| ✓ | int32 | control_mode | accroding to autoware_vehicle_msgs/ControlMode | manual/auto (changed by /awapi/autoware/put/engage) | +| ✓ | int32 | control_mode | according to autoware_vehicle_msgs/ControlMode | manual/auto (changed by /awapi/autoware/put/engage) | | | int32 | gate_mode | autoware_vehicle_msgs/GateMode | auto/remote (it is valid only when control_mode=auto)) | | ✓ | bool | emergency_stopped | True in emergency mode | | +| | autoware_system_msgs/HazardStatus | hazard_status | | system hazard status | | ✓ | autoware_planning_msgs/StopReasonArray | stop_reason | | "stop_pose" represents the position of "base_link" (not the head of the car) | | ✓ | diagnostic_msgs/DiagnosticStatus[] | diagnostics | | output only diag. of leaf node (diag. of parent node are cut) | -| ✓ | bool | arrived_goal | | True if the autoware_state is changed from Driving to ArrivedGoal or WairingForRoute. False if the autoware_state is changed to WaitingForEngage or Driving. Default False. | +| ✓ | diagnostic_msgs/DiagnosticStatus[] | error_diagnostics | | diagnostics that are the cause of system emergency | +| ✓ | bool | arrived_goal | | True if the autoware_state is changed from Driving to ArrivedGoal or WaitingForRoute. False if the autoware_state is changed to WaitingForEngage or Driving. Default False. | + +- specification of stop_reason + - stop_reason is output only when the following conditions are met. + - stop_point in stop_reason is close to /planning/scenario_planning/trajectory (within 10m). + - The distance between current position and stop_point is less than stop_reason_thresh_dist. ### /awapi/autoware/get/route @@ -53,14 +60,25 @@ | --- | :--------------------------- | :--- | :--- | :--- | | ✓ | autoware_planning_msgs/Route | | | | +### /awapi/autoware/get/stop_speed_exceeded + +- get flag of exceeding stop speed or not + - True: exceed the stop speed ( = "cannot stop before the stop line") + - False: not exceed the stop speed ( = "no stop line in the trajectory" or "possible to stop before the stop line" ) +- MessageType: std_msgs/Bool + +| ✓ | type | name | unit | note | +| --- | :------------ | :--- | :--- | :--- | +| | std_msgs/Bool | | - | | + ### /awapi/prediction/get/objects - get predicted object -- MessageType: autoware_perception_msgs/DynamicObjectArray +- MessageType: autoware_api_msgs/DynamicObjectArray -| ✓ | type | name | unit | note | -| --- | :------------------------------------------ | :--- | :--- | :--- | -| ✓ | autoware_perception_msgs/DynamicObjectArray | | | | +| ✓ | type | name | unit | note | +| --- | :----------------------------------- | :--- | :--- | :--- | +| ✓ | autoware_api_msgs/DynamicObjectArray | | | | ### /awapi/lane_change/get/status @@ -70,7 +88,7 @@ | ✓ | type | name | unit | note | | --- | :-------------------------- | :-------------------------- | :--------------------------------------- | :--------------------------------------------------------------------------------- | | | std_msgs/Header | header | | | -| | bool | force_lane_change_available | True when lane change is avilable | available: Physically lane changeable state (do not consider other vehicle) | +| | bool | force_lane_change_available | True when lane change is available | available: Physically lane changeable state (do not consider other vehicle) | | | bool | lane_change_ready | True when lane change is ready | ready: State that ego-vehicle can change lane without collision with other vehicle | | | autoware_planning_msgs/Path | candidate_path | according to autoware_planning_msgs/Path | | @@ -88,11 +106,34 @@ ### /awapi/traffic_light/get/status - get recognition result of traffic light -- MessageType: autoware_perception_msgs/TrafficLightStateArray +- MessageType: autoware_api_msgs/TrafficLightStateArray -| ✓ | type | name | unit | note | -| --- | :---------------------------------------------- | :--- | :--- | :--- | -| | autoware_perception_msgs/TrafficLightStateArray | | | | +| ✓ | type | name | unit | note | +| --- | :--------------------------------------- | :--- | :--- | :--- | +| | autoware_api_msgs/TrafficLightStateArray | | | | + +### /awapi/traffic_light/get/nearest_traffic_light_status + +- get recognition result of nearest traffic light +- MessageType: autoware_perception_msgs/TrafficLightStateStamped + +| ✓ | type | name | unit | note | +| --- | :------------------------------------------------ | :--- | :--- | :--- | +| | autoware_perception_msgs/TrafficLightStateStamped | | | | + +### /awapi/vehicle/get/door + +- get door status +- MessageType: autoware_api_msgs/DoorStatus.msg + +| ✓ | type | name | unit | note | +| --- | :--------------------------- | :----- | :--------------------------------------------------------------------------------------- | :------------------------------------------ | +| | autoware_api_msgs/DoorStatus | status | 0:UNKNOWN, 1:DOOR_OPENED, 2:DOOR_CLOSED 3:DOOR_OPENING, 4:DOOR_CLOSING, 5:NOT_APPLICABLE | available only for the vehicle using pacmod | + +- Now, available status is following: (0:UNKNOWN, 1:DOOR_OPENED, 2:DOOR_CLOSED, 5:NOT_APPLICABLE ). +- 5 (NOT_APPLICABLE) is published if the pacmod is not used +- Due to the specifications of pacmod, the last door open / close command is published as the status. +- The status is 0 (UNKNOWN) until the door open / close command is published once. ## put topic @@ -105,6 +146,21 @@ | --- | :--------------- | :--- | :--- | :----------- | | ✓ | std_msgs/Float32 | | | max velocity | +### /awapi/vehicle/put/stop + +- set temporary stop signal +- MessageType: std_msgs/bool +- Specification + + - send True: send upper velocity to 0 + - send False: resend last received upper velocity + - (if upper velocity have never received, send _default_max_velocity_ value.) + - _default_max_velocity_ refers to the param: _/planning/scenario_planning/motion_velocity_optimizer/max_velocity_ + + | ✓ | type | name | unit | note | + | --- | :------------ | :--- | :--- | :--- | + | ✓ | std_msgs/Bool | | | | + ### /awapi/autoware/put/gate_mode - send gate mode (auto/remote) @@ -114,11 +170,11 @@ | --- | :----------------------------- | :--- | :--- | :--- | | | autoware_control_msgs/GateMode | | | | -### /awapi/autoware/put/emergency +### /awapi/autoware/put/emergency_stop -- send emergency signal -- MessageType: autoware_control_msgs/EmergencyMode -- **To enable this functionality, autoware have to be in the Remote Mode or set _/control/vehicle_cmd_gate/use_emergency_handling_ to true.** +- send emergency_stop signal +- MessageType: std_msgs/Bool +- **To enable this functionality, autoware have to be in the Remote Mode or set _/control/vehicle_cmd_gate/use_external_emergency_stop_ to true.** | ✓ | type | name | unit | note | | --- | :---------------------------------- | :--- | :--- | :--- | @@ -133,9 +189,18 @@ | --- | :------------------------------- | :--- | :--- | :--- | | ✓ | autoware_control_msgs/EngageMode | | | | -### /awapi/autoware/put/route +### /awapi/autoware/put/goal - send goal pose +- MessageType: geometry_msgs/PoseStamped + +| ✓ | type | name | unit | note | +| --- | :------------------------ | :--- | :--- | :--- | +| | geometry_msgs/PoseStamped | | | | + +### /awapi/autoware/put/route + +- send route - MessageType: autoware_planning_msgs/Route | ✓ | type | name | unit | note | @@ -179,12 +244,53 @@ | ✓ | type | name | unit | note | | --- | :--- | :--- | :--- | :--- | - ### /awapi/traffic_light/put/traffic_light - Overwrite the recognition result of traffic light -- **not implemented (Autoware does not have corresponded topic)** +- MessageType: autoware_perception_msgs/TrafficLightStateArray -| ✓ | type | name | unit | note | -| --- | :--- | :--- | :--- | :--- | +| ✓ | type | name | unit | note | +| --- | :---------------------------------------------- | :--- | :--- | :--- | +| | autoware_perception_msgs/TrafficLightStateArray | | | | + +### /awapi/vehicle/put/door + +- send door command +- MessageType: std_msgs/Bool + - send True: open door + - send False: close door + +| ✓ | type | name | unit | note | +| --- | :------------ | :--- | :--- | :------------------------------------------ | +| | std_msgs/Bool | | | available only for the vehicle using pacmod | + +### /awapi/autoware/put/crosswalk_states + +- send crosswalk status + - forcibly rewrite the internal state of crosswalk module +- MessageType: autoware_api_msgs/CrossWalkStatus + +| ✓ | type | name | unit | note | +| --- | :-------------- | :----- | :----------------------- | :--- | +| | std_msgs/Header | header | | | +| | int32 | status | 0:STOP, 1:GO, 2:SLOWDOWN | | + +### /awapi/autoware/put/intersection_states + +- send intersection status + - forcibly rewrite the internal state of intersection module +- MessageType: autoware_api_msgs/CrosswalkStatus + +| ✓ | type | name | unit | note | +| --- | :-------------- | :----- | :----------- | :--- | +| | std_msgs/Header | header | | | +| | int32 | status | 0:STOP, 1:GO | | + +### /awapi/autoware/put/expand_stop_range + +- send expand range of the polygon used by obstacle stop [m] +- MessageType: std_msgs/Float32 +| ✓ | type | name | unit | note | +| --- | :------ | :--- | :--- | :--- | +| | Float32 | | | | diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.hpp b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.hpp index 41a5060cd5bc0..cb034e56502e7 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.hpp +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.hpp @@ -12,6 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. +#ifndef AWAPI_AWIV_ADAPTER__AWAPI_AUTOWARE_STATE_PUBLISHER_HPP_ +#define AWAPI_AWIV_ADAPTER__AWAPI_AUTOWARE_STATE_PUBLISHER_HPP_ + +#include +#include +#include + #include "rclcpp/rclcpp.hpp" #include "awapi_awiv_adapter/awapi_autoware_util.hpp" @@ -22,7 +29,7 @@ namespace autoware_api class AutowareIvAutowareStatePublisher { public: - AutowareIvAutowareStatePublisher(rclcpp::Node & node); + explicit AutowareIvAutowareStatePublisher(rclcpp::Node & node); void statePublisher(const AutowareInfo & aw_info); private: @@ -50,14 +57,18 @@ class AutowareIvAutowareStatePublisher const autoware_control_msgs::msg::GateMode::ConstSharedPtr & gate_mode_ptr, autoware_api_msgs::msg::AwapiAutowareStatus * status); void getIsEmergencyInfo( - const autoware_control_msgs::msg::EmergencyMode::ConstSharedPtr & is_emergency_ptr, + const std_msgs::msg::Bool::ConstSharedPtr & is_emergency_ptr, + autoware_api_msgs::msg::AwapiAutowareStatus * status); + void getHazardStatusInfo( + const autoware_system_msgs::msg::HazardStatusStamped::ConstSharedPtr & hazard_status_ptr, autoware_api_msgs::msg::AwapiAutowareStatus * status); void getStopReasonInfo( const autoware_planning_msgs::msg::StopReasonArray::ConstSharedPtr & stop_reason_ptr, autoware_api_msgs::msg::AwapiAutowareStatus * status); void getDiagInfo( - const diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr & diag_ptr, - autoware_api_msgs::msg::AwapiAutowareStatus * status); + const AutowareInfo & aw_info, autoware_api_msgs::msg::AwapiAutowareStatus * status); + void getErrorDiagInfo( + const AutowareInfo & aw_info, autoware_api_msgs::msg::AwapiAutowareStatus * status); void getGlobalRptInfo( const pacmod_msgs::msg::GlobalRpt::ConstSharedPtr & global_rpt_ptr, autoware_api_msgs::msg::AwapiAutowareStatus * status); @@ -71,3 +82,5 @@ class AutowareIvAutowareStatePublisher }; } // namespace autoware_api + +#endif // AWAPI_AWIV_ADAPTER__AWAPI_AUTOWARE_STATE_PUBLISHER_HPP_ diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.hpp b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.hpp index 7ce9d11fe8d02..a729910a4735e 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.hpp +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.hpp @@ -12,14 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#pragma once +#ifndef AWAPI_AWIV_ADAPTER__AWAPI_AUTOWARE_UTIL_HPP_ +#define AWAPI_AWIV_ADAPTER__AWAPI_AUTOWARE_UTIL_HPP_ -#ifndef AWAPI_AUTOWARE_UTIL_H -#define AWAPI_AUTOWARE_UTIL_H +#include +#include -#include "tf2/utils.h" -#include "tf2_ros/transform_broadcaster.h" -#include "tf2_ros/transform_listener.h" #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" #include "pacmod_msgs/msg/global_rpt.hpp" @@ -27,6 +25,9 @@ #include "sensor_msgs/msg/nav_sat_fix.hpp" #include "std_msgs/msg/bool.hpp" #include "std_msgs/msg/float32.hpp" +#include "tf2/utils.h" +#include "tf2_ros/transform_broadcaster.h" +#include "tf2_ros/transform_listener.h" #include "autoware_control_msgs/msg/emergency_mode.hpp" #include "autoware_control_msgs/msg/gate_mode.hpp" @@ -34,12 +35,14 @@ #include "autoware_planning_msgs/msg/stop_reason_array.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" #include "autoware_system_msgs/msg/autoware_state.hpp" +#include "autoware_system_msgs/msg/hazard_status_stamped.hpp" #include "autoware_vehicle_msgs/msg/control_mode.hpp" #include "autoware_vehicle_msgs/msg/shift_stamped.hpp" #include "autoware_vehicle_msgs/msg/steering.hpp" #include "autoware_vehicle_msgs/msg/turn_signal.hpp" #include "autoware_vehicle_msgs/msg/vehicle_command.hpp" #include "diagnostic_msgs/msg/diagnostic_array.hpp" +#include "pacmod_msgs/msg/system_rpt_int.hpp" namespace autoware_api { @@ -56,7 +59,8 @@ struct AutowareInfo autoware_system_msgs::msg::AutowareState::ConstSharedPtr autoware_state_ptr; autoware_vehicle_msgs::msg::ControlMode::ConstSharedPtr control_mode_ptr; autoware_control_msgs::msg::GateMode::ConstSharedPtr gate_mode_ptr; - autoware_control_msgs::msg::EmergencyMode::ConstSharedPtr is_emergency_ptr; + std_msgs::msg::Bool::ConstSharedPtr is_emergency_ptr; + autoware_system_msgs::msg::HazardStatusStamped::ConstSharedPtr hazard_status_ptr; autoware_planning_msgs::msg::StopReasonArray::ConstSharedPtr stop_reason_ptr; diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr diagnostic_ptr; pacmod_msgs::msg::GlobalRpt::ConstSharedPtr global_rpt_ptr; @@ -65,10 +69,69 @@ struct AutowareInfo autoware_planning_msgs::msg::Path::ConstSharedPtr lane_change_candidate_ptr; std_msgs::msg::Bool::ConstSharedPtr obstacle_avoid_ready_ptr; autoware_planning_msgs::msg::Trajectory::ConstSharedPtr obstacle_avoid_candidate_ptr; + std_msgs::msg::Float32::ConstSharedPtr max_velocity_ptr; + std_msgs::msg::Bool::ConstSharedPtr temporary_stop_ptr; + autoware_planning_msgs::msg::Trajectory::ConstSharedPtr autoware_planning_traj_ptr; + pacmod_msgs::msg::SystemRptInt::ConstSharedPtr door_state_ptr; }; +template +T waitForParam( + rclcpp::Node * node, const std::string & remote_node_name, const std::string & param_name) +{ + std::chrono::seconds sec(1); + + auto param_client = std::make_shared(node, remote_node_name); + + while (!param_client->wait_for_service(sec)) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(node->get_logger(), "Interrupted while waiting for the service."); + return {}; + } + RCLCPP_INFO_THROTTLE( + node->get_logger(), *node->get_clock(), 1000 /* ms */, + "waiting for node: %s, param: %s\n", + remote_node_name.c_str(), param_name.c_str()); + } + + if (param_client->has_parameter(param_name)) { + return param_client->get_parameter(param_name); + } + + return {}; +} + double lowpass_filter(const double current_value, const double prev_value, const double gain); +namespace planning_util +{ +bool calcClosestIndex( + const autoware_planning_msgs::msg::Trajectory & traj, const geometry_msgs::msg::Pose & pose, + size_t & output_closest_idx, const double dist_thr = 10.0, const double angle_thr = M_PI / 2.0); + +inline geometry_msgs::msg::Pose getPose( + const autoware_planning_msgs::msg::Trajectory & traj, const int idx) +{ + return traj.points.at(idx).pose; +} + +inline double calcDist2d(const geometry_msgs::msg::Point & a, const geometry_msgs::msg::Point & b) +{ + return std::hypot((a.x - b.x), (a.y - b.y)); +} + +double normalizeEulerAngle(double euler); + +double calcArcLengthFromWayPoint( + const autoware_planning_msgs::msg::Trajectory & input_path, const size_t src_idx, + const size_t dst_idx); + +double calcDistanceAlongTrajectory( + const autoware_planning_msgs::msg::Trajectory & trajectory, + const geometry_msgs::msg::Pose & current_pose, const geometry_msgs::msg::Pose & target_pose); + +} // namespace planning_util + } // namespace autoware_api -#endif // AWAPI_AUTOWARE_UTIL_H +#endif // AWAPI_AWIV_ADAPTER__AWAPI_AUTOWARE_UTIL_HPP_ diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.hpp b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.hpp index 798bb90a01985..d491c252d1333 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.hpp +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.hpp @@ -12,6 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. +#ifndef AWAPI_AWIV_ADAPTER__AWAPI_AWIV_ADAPTER_CORE_HPP_ +#define AWAPI_AWIV_ADAPTER__AWAPI_AWIV_ADAPTER_CORE_HPP_ + +#include + #include "rclcpp/rclcpp.hpp" #include "autoware_control_msgs/msg/emergency_mode.hpp" @@ -33,7 +38,9 @@ #include "awapi_awiv_adapter/awapi_autoware_state_publisher.hpp" #include "awapi_awiv_adapter/awapi_autoware_util.hpp" #include "awapi_awiv_adapter/awapi_lane_change_state_publisher.hpp" +#include "awapi_awiv_adapter/awapi_max_velocity_publisher.hpp" #include "awapi_awiv_adapter/awapi_obstacle_avoidance_state_publisher.hpp" +#include "awapi_awiv_adapter/awapi_pacmod_util.hpp" #include "awapi_awiv_adapter/awapi_stop_reason_aggregator.hpp" #include "awapi_awiv_adapter/awapi_vehicle_state_publisher.hpp" @@ -56,7 +63,9 @@ class AutowareIvAdapter : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_autoware_state_; rclcpp::Subscription::SharedPtr sub_control_mode_; rclcpp::Subscription::SharedPtr sub_gate_mode_; - rclcpp::Subscription::SharedPtr sub_emergency_; + rclcpp::Subscription::SharedPtr sub_emergency_; + rclcpp::Subscription::SharedPtr + sub_hazard_status_; rclcpp::Subscription::SharedPtr sub_stop_reason_; rclcpp::Subscription::SharedPtr sub_diagnostics_; rclcpp::Subscription::SharedPtr sub_global_rpt_; @@ -66,6 +75,17 @@ class AutowareIvAdapter : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_obstacle_avoid_ready_; rclcpp::Subscription::SharedPtr sub_obstacle_avoid_candidate_; + rclcpp::Subscription::SharedPtr sub_max_velocity_; + rclcpp::Subscription::SharedPtr sub_temporary_stop_; + rclcpp::Subscription::SharedPtr + sub_autoware_traj_; + rclcpp::Subscription::SharedPtr sub_door_control_; + rclcpp::Subscription::SharedPtr sub_door_status_; + + // publisher + rclcpp::Publisher::SharedPtr pub_door_control_; + rclcpp::Publisher::SharedPtr pub_door_status_; + // timer rclcpp::TimerBase::SharedPtr timer_; @@ -85,7 +105,9 @@ class AutowareIvAdapter : public rclcpp::Node const autoware_system_msgs::msg::AutowareState::ConstSharedPtr msg_ptr); void callbackControlMode(const autoware_vehicle_msgs::msg::ControlMode::ConstSharedPtr msg_ptr); void callbackGateMode(const autoware_control_msgs::msg::GateMode::ConstSharedPtr msg_ptr); - void callbackIsEmergency(const autoware_control_msgs::msg::EmergencyMode::ConstSharedPtr msg_ptr); + void callbackIsEmergency(const std_msgs::msg::Bool::ConstSharedPtr msg_ptr); + void callbackHazardStatus( + const autoware_system_msgs::msg::HazardStatusStamped::ConstSharedPtr msg_ptr); void callbackStopReason( const autoware_planning_msgs::msg::StopReasonArray::ConstSharedPtr msg_ptr); void callbackDiagnostics(const diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr msg_ptr); @@ -97,22 +119,34 @@ class AutowareIvAdapter : public rclcpp::Node void callbackLaneObstacleAvoidReady(const std_msgs::msg::Bool::ConstSharedPtr msg_ptr); void callbackLaneObstacleAvoidCandidatePath( const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr msg_ptr); + void callbackMaxVelocity(const std_msgs::msg::Float32::ConstSharedPtr msg_ptr); + void callbackTemporaryStop(const std_msgs::msg::Bool::ConstSharedPtr msg_ptr); + void callbackAutowareTrajectory( + const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr msg_ptr); + void callbackDoorControl(const std_msgs::msg::Bool::ConstSharedPtr msg_ptr); + void callbackDoorStatus(const pacmod_msgs::msg::SystemRptInt::ConstSharedPtr msg_ptr); // timer function void timerCallback(); - void emergencyParamCheck(const bool emergency_handling_param); + void emergencyParamCheck(const bool emergency_stop_param); void getCurrentPose(); // parameter AutowareInfo aw_info_; std::unique_ptr vehicle_state_publisher_; std::unique_ptr autoware_state_publisher_; - std::unique_ptr stop_reason_aggreagator_; + std::unique_ptr stop_reason_aggregator_; std::unique_ptr lane_change_state_publisher_; - std::unique_ptr obstacle_avoidance_state_publisher_; + std::unique_ptr + obstacle_avoidance_state_publisher_; + std::unique_ptr max_velocity_publisher_; double status_pub_hz_; double stop_reason_timeout_; + double default_max_velocity; + double stop_reason_thresh_dist_; }; } // namespace autoware_api + +#endif // AWAPI_AWIV_ADAPTER__AWAPI_AWIV_ADAPTER_CORE_HPP_ diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_lane_change_state_publisher.hpp b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_lane_change_state_publisher.hpp index 030631d6ab106..0c9227fabc7fd 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_lane_change_state_publisher.hpp +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_lane_change_state_publisher.hpp @@ -12,6 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. +#ifndef AWAPI_AWIV_ADAPTER__AWAPI_LANE_CHANGE_STATE_PUBLISHER_HPP_ +#define AWAPI_AWIV_ADAPTER__AWAPI_LANE_CHANGE_STATE_PUBLISHER_HPP_ + #include "rclcpp/rclcpp.hpp" #include "awapi_awiv_adapter/awapi_autoware_util.hpp" @@ -22,7 +25,7 @@ namespace autoware_api class AutowareIvLaneChangeStatePublisher { public: - AutowareIvLaneChangeStatePublisher(rclcpp::Node & node); + explicit AutowareIvLaneChangeStatePublisher(rclcpp::Node & node); void statePublisher(const AutowareInfo & aw_info); private: @@ -44,3 +47,5 @@ class AutowareIvLaneChangeStatePublisher }; } // namespace autoware_api + +#endif // AWAPI_AWIV_ADAPTER__AWAPI_LANE_CHANGE_STATE_PUBLISHER_HPP_ diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_max_velocity_publisher.hpp b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_max_velocity_publisher.hpp new file mode 100644 index 0000000000000..a7d6ffd68c0ee --- /dev/null +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_max_velocity_publisher.hpp @@ -0,0 +1,43 @@ +// Copyright 2020 Tier IV, Inc. +// +// 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. + +#ifndef AWAPI_AWIV_ADAPTER__AWAPI_MAX_VELOCITY_PUBLISHER_HPP_ +#define AWAPI_AWIV_ADAPTER__AWAPI_MAX_VELOCITY_PUBLISHER_HPP_ + +#include "rclcpp/rclcpp.hpp" + +#include "awapi_awiv_adapter/awapi_autoware_util.hpp" + +namespace autoware_api +{ +class AutowareIvMaxVelocityPublisher +{ +public: + AutowareIvMaxVelocityPublisher(rclcpp::Node & node, const double default_max_velocity); + void statePublisher(const AutowareInfo & aw_info); + +private: + // publisher + rclcpp::Publisher::SharedPtr pub_state_; + + bool calcMaxVelocity( + const std_msgs::msg::Float32::ConstSharedPtr & max_velocity_ptr, + const std_msgs::msg::Bool::ConstSharedPtr & temporary_stop_ptr, float * max_velocity); + + double default_max_velocity_; +}; + +} // namespace autoware_api + +#endif // AWAPI_AWIV_ADAPTER__AWAPI_MAX_VELOCITY_PUBLISHER_HPP_ diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_obstacle_avoidance_state_publisher.hpp b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_obstacle_avoidance_state_publisher.hpp index 533ce49e1b76b..a401b5d123999 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_obstacle_avoidance_state_publisher.hpp +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_obstacle_avoidance_state_publisher.hpp @@ -12,6 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. +#ifndef AWAPI_AWIV_ADAPTER__AWAPI_OBSTACLE_AVOIDANCE_STATE_PUBLISHER_HPP_ +#define AWAPI_AWIV_ADAPTER__AWAPI_OBSTACLE_AVOIDANCE_STATE_PUBLISHER_HPP_ + #include "rclcpp/rclcpp.hpp" #include "awapi_awiv_adapter/awapi_autoware_util.hpp" @@ -22,7 +25,7 @@ namespace autoware_api class AutowareIvObstacleAvoidanceStatePublisher { public: - AutowareIvObstacleAvoidanceStatePublisher(rclcpp::Node & node); + explicit AutowareIvObstacleAvoidanceStatePublisher(rclcpp::Node & node); void statePublisher(const AutowareInfo & aw_info); private: @@ -41,3 +44,5 @@ class AutowareIvObstacleAvoidanceStatePublisher }; } // namespace autoware_api + +#endif // AWAPI_AWIV_ADAPTER__AWAPI_OBSTACLE_AVOIDANCE_STATE_PUBLISHER_HPP_ diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_pacmod_util.hpp b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_pacmod_util.hpp new file mode 100644 index 0000000000000..209364b79d3b7 --- /dev/null +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_pacmod_util.hpp @@ -0,0 +1,39 @@ +// Copyright 2020 Tier IV, Inc. +// +// 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. + +#ifndef AWAPI_AWIV_ADAPTER__AWAPI_PACMOD_UTIL_HPP_ +#define AWAPI_AWIV_ADAPTER__AWAPI_PACMOD_UTIL_HPP_ + +#include "rclcpp/rclcpp.hpp" + +#include "autoware_api_msgs/msg/door_status.hpp" +#include "pacmod_msgs/msg/system_cmd_int.hpp" +#include "pacmod_msgs/msg/system_rpt_int.hpp" +#include "std_msgs/msg/bool.hpp" + +namespace autoware_api +{ +namespace pacmod_util +{ +autoware_api_msgs::msg::DoorStatus getDoorStatusMsg( + const pacmod_msgs::msg::SystemRptInt::ConstSharedPtr & msg_ptr); +pacmod_msgs::msg::SystemCmdInt createClearOverrideDoorCommand( + const rclcpp::Clock::SharedPtr & clock); +pacmod_msgs::msg::SystemCmdInt createDoorCommand( + const rclcpp::Clock::SharedPtr & clock, const std_msgs::msg::Bool::ConstSharedPtr & msg_ptr); +} // namespace pacmod_util + +} // namespace autoware_api + +#endif // AWAPI_AWIV_ADAPTER__AWAPI_PACMOD_UTIL_HPP_ diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_stop_reason_aggregator.hpp b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_stop_reason_aggregator.hpp index d47baca0ce726..60ae64c87bd1b 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_stop_reason_aggregator.hpp +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_stop_reason_aggregator.hpp @@ -12,7 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#pragma once +#ifndef AWAPI_AWIV_ADAPTER__AWAPI_STOP_REASON_AGGREGATOR_HPP_ +#define AWAPI_AWIV_ADAPTER__AWAPI_STOP_REASON_AGGREGATOR_HPP_ + +#include #include "rclcpp/rclcpp.hpp" @@ -23,25 +26,39 @@ namespace autoware_api class AutowareIvStopReasonAggregator { public: - AutowareIvStopReasonAggregator(rclcpp::Node & node, const double timeout); + AutowareIvStopReasonAggregator( + rclcpp::Node & node, const double timeout, const double thresh_dist_to_stop_pose); autoware_planning_msgs::msg::StopReasonArray::ConstSharedPtr updateStopReasonArray( - const autoware_planning_msgs::msg::StopReasonArray::ConstSharedPtr & msg_ptr); + const autoware_planning_msgs::msg::StopReasonArray::ConstSharedPtr & msg_ptr, + const AutowareInfo & aw_info); private: - void applyUpdate(const autoware_planning_msgs::msg::StopReasonArray::ConstSharedPtr & msg_ptr); + void applyUpdate( + const autoware_planning_msgs::msg::StopReasonArray::ConstSharedPtr & msg_ptr, + const AutowareInfo & aw_info); bool checkMatchingReason( const autoware_planning_msgs::msg::StopReasonArray::ConstSharedPtr & msg_stop_reason_array, const autoware_planning_msgs::msg::StopReasonArray & stop_reason_array); void applyTimeOut(); void appendStopReasonToArray( const autoware_planning_msgs::msg::StopReason & stop_reason, - autoware_planning_msgs::msg::StopReasonArray * stop_reason_array); - autoware_planning_msgs::msg::StopReasonArray::ConstSharedPtr makeStopReasonArray(); + autoware_planning_msgs::msg::StopReasonArray * stop_reason_array, const AutowareInfo & aw_info); + autoware_planning_msgs::msg::StopReasonArray::ConstSharedPtr makeStopReasonArray( + const AutowareInfo & aw_info); + autoware_planning_msgs::msg::StopReason inputStopDistToStopReason( + const autoware_planning_msgs::msg::StopReason & stop_reason, const AutowareInfo & aw_info); + double calcStopDistToStopFactor( + const autoware_planning_msgs::msg::StopFactor & stop_factor, const AutowareInfo & aw_info); + autoware_planning_msgs::msg::StopReason getNearStopReason( + const autoware_planning_msgs::msg::StopReason & stop_reason, const AutowareInfo & aw_info); rclcpp::Logger logger_; rclcpp::Clock::SharedPtr clock_; double timeout_; + double thresh_dist_to_stop_pose_; std::vector stop_reason_array_vec_; }; } // namespace autoware_api + +#endif // AWAPI_AWIV_ADAPTER__AWAPI_STOP_REASON_AGGREGATOR_HPP_ diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_vehicle_state_publisher.hpp b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_vehicle_state_publisher.hpp index 5d6db0a0eea6f..1060699392ebb 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_vehicle_state_publisher.hpp +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_vehicle_state_publisher.hpp @@ -12,6 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. +#ifndef AWAPI_AWIV_ADAPTER__AWAPI_VEHICLE_STATE_PUBLISHER_HPP_ +#define AWAPI_AWIV_ADAPTER__AWAPI_VEHICLE_STATE_PUBLISHER_HPP_ + +#include + #include "rclcpp/rclcpp.hpp" #include "awapi_awiv_adapter/awapi_autoware_util.hpp" @@ -22,7 +27,7 @@ namespace autoware_api class AutowareIvVehicleStatePublisher { public: - AutowareIvVehicleStatePublisher(rclcpp::Node & node); + explicit AutowareIvVehicleStatePublisher(rclcpp::Node & node); void statePublisher(const AutowareInfo & aw_info); private: @@ -58,7 +63,7 @@ class AutowareIvVehicleStatePublisher rclcpp::Logger logger_; rclcpp::Clock::SharedPtr clock_; - //parameters + // parameters geometry_msgs::msg::TwistStamped::ConstSharedPtr previous_twist_ptr_; autoware_vehicle_msgs::msg::Steering::ConstSharedPtr previous_steer_ptr_; double prev_accel_; @@ -70,3 +75,5 @@ class AutowareIvVehicleStatePublisher }; } // namespace autoware_api + +#endif // AWAPI_AWIV_ADAPTER__AWAPI_VEHICLE_STATE_PUBLISHER_HPP_ diff --git a/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml b/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml index 6202ae7aa59b7..b90f56f5daeae 100644 --- a/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml +++ b/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml @@ -3,6 +3,7 @@ + @@ -15,6 +16,7 @@ + @@ -26,6 +28,13 @@ + + + + + + + @@ -33,32 +42,46 @@ - + + + - + - + + + + - + + - + + - + + + + + - + + + + @@ -73,6 +96,7 @@ + @@ -81,16 +105,26 @@ + + - + + + + + + + + + - + @@ -100,21 +134,21 @@ + + + + + - - - - - - - - + + + @@ -132,6 +166,11 @@ + + + + + @@ -152,12 +191,32 @@ - - - - - --> + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/awapi/awapi_awiv_adapter/package.xml b/awapi/awapi_awiv_adapter/package.xml index ce2fbf373bd59..c44329cbe488b 100644 --- a/awapi/awapi_awiv_adapter/package.xml +++ b/awapi/awapi_awiv_adapter/package.xml @@ -27,7 +27,7 @@ topic_tools ament_lint_auto - ament_cmake_cppcheck + ament_lint_common ament_cmake diff --git a/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp b/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp index 7eb9d88816b65..7c2c89b301913 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp @@ -12,8 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "awapi_awiv_adapter/awapi_autoware_state_publisher.hpp" #include +#include +#include + +#include "awapi_awiv_adapter/awapi_autoware_state_publisher.hpp" namespace autoware_api { @@ -31,7 +34,7 @@ void AutowareIvAutowareStatePublisher::statePublisher(const AutowareInfo & aw_in { autoware_api_msgs::msg::AwapiAutowareStatus status; - //input header + // input header status.header.frame_id = "base_link"; status.header.stamp = clock_->now(); @@ -40,8 +43,10 @@ void AutowareIvAutowareStatePublisher::statePublisher(const AutowareInfo & aw_in getControlModeInfo(aw_info.control_mode_ptr, &status); getGateModeInfo(aw_info.gate_mode_ptr, &status); getIsEmergencyInfo(aw_info.is_emergency_ptr, &status); + getHazardStatusInfo(aw_info.hazard_status_ptr, &status); getStopReasonInfo(aw_info.stop_reason_ptr, &status); - getDiagInfo(aw_info.diagnostic_ptr, &status); + getDiagInfo(aw_info, &status); + getErrorDiagInfo(aw_info, &status); getGlobalRptInfo(aw_info.global_rpt_ptr, &status); // publish info @@ -90,7 +95,7 @@ void AutowareIvAutowareStatePublisher::getGateModeInfo( } void AutowareIvAutowareStatePublisher::getIsEmergencyInfo( - const autoware_control_msgs::msg::EmergencyMode::ConstSharedPtr & is_emergency_ptr, + const std_msgs::msg::Bool::ConstSharedPtr & is_emergency_ptr, autoware_api_msgs::msg::AwapiAutowareStatus * status) { if (!is_emergency_ptr) { @@ -99,7 +104,22 @@ void AutowareIvAutowareStatePublisher::getIsEmergencyInfo( } // get emergency - status->emergency_stopped = is_emergency_ptr->is_emergency; + status->emergency_stopped = is_emergency_ptr->data; +} + +void AutowareIvAutowareStatePublisher::getHazardStatusInfo( + const autoware_system_msgs::msg::HazardStatusStamped::ConstSharedPtr & hazard_status_ptr, + autoware_api_msgs::msg::AwapiAutowareStatus * status) +{ + if (!hazard_status_ptr) { + RCLCPP_DEBUG_STREAM_THROTTLE( + logger_, *clock_, 5000 /* ms */, + "[AutowareIvAutowareStatePublisher] hazard_status is nullptr"); + return; + } + + // get emergency + status->hazard_status = *hazard_status_ptr; } void AutowareIvAutowareStatePublisher::getStopReasonInfo( @@ -115,16 +135,95 @@ void AutowareIvAutowareStatePublisher::getStopReasonInfo( } void AutowareIvAutowareStatePublisher::getDiagInfo( - const diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr & diag_ptr, - autoware_api_msgs::msg::AwapiAutowareStatus * status) + const AutowareInfo & aw_info, autoware_api_msgs::msg::AwapiAutowareStatus * status) { - if (!diag_ptr) { - RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *clock_, 5000 /* ms */, "diagnostics is nullptr"); + if (!aw_info.diagnostic_ptr) { + RCLCPP_DEBUG_STREAM_THROTTLE( + logger_, *clock_, 5000 /* ms */, + "[AutowareIvAutowareStatePublisher] diagnostics is nullptr"); + return; + } + + // get diag + status->diagnostics = extractLeafDiag(aw_info.diagnostic_ptr->status); +} + +// This function is tentative and should be replaced with getHazardStatusInfo. +// TODO(Kenji Miyake): Make getErrorDiagInfo users to use getHazardStatusInfo. +void AutowareIvAutowareStatePublisher::getErrorDiagInfo( + const AutowareInfo & aw_info, autoware_api_msgs::msg::AwapiAutowareStatus * status) +{ + using autoware_system_msgs::msg::AutowareState; + using autoware_vehicle_msgs::msg::ControlMode; + + if (!aw_info.autoware_state_ptr) { + RCLCPP_DEBUG_STREAM_THROTTLE( + logger_, *clock_, 5000 /* ms */, + "[AutowareIvAutowareStatePublisher] autoware_state is nullptr"); + return; + } + + if (!aw_info.control_mode_ptr) { + RCLCPP_DEBUG_STREAM_THROTTLE( + logger_, *clock_, 5000 /* ms */, + "[AutowareIvAutowareStatePublisher] control mode is nullptr"); + return; + } + + if (!aw_info.diagnostic_ptr) { + RCLCPP_DEBUG_STREAM_THROTTLE( + logger_, *clock_, 5000 /* ms */, + "[AutowareIvAutowareStatePublisher] diagnostics is nullptr"); + return; + } + + if (!aw_info.hazard_status_ptr) { + RCLCPP_DEBUG_STREAM_THROTTLE( + logger_, *clock_, 5000 /* ms */, + "[AutowareIvAutowareStatePublisher] hazard_status is nullptr"); + return; + } + + // filter by state + if (aw_info.autoware_state_ptr->state != AutowareState::EMERGENCY) { + status->error_diagnostics = {}; + return; + } + + // filter by control_mode + if (aw_info.control_mode_ptr->data == ControlMode::MANUAL) { + status->error_diagnostics = {}; return; } // get diag - status->diagnostics = extractLeafDiag(diag_ptr->status); + using diagnostic_msgs::msg::DiagnosticStatus; + const auto & hazard_status = aw_info.hazard_status_ptr->status; + std::vector error_diagnostics; + + for (const auto & hazard_diag : hazard_status.diagnostics_spf) { + auto diag = hazard_diag; + diag.level = DiagnosticStatus::ERROR; + error_diagnostics.push_back(hazard_diag); + } + for (const auto & hazard_diag : hazard_status.diagnostics_lf) { + auto diag = hazard_diag; + diag.level = DiagnosticStatus::ERROR; + error_diagnostics.push_back(hazard_diag); + } + for (const auto & hazard_diag : hazard_status.diagnostics_sf) { + auto diag = hazard_diag; + diag.level = DiagnosticStatus::WARN; + error_diagnostics.push_back(hazard_diag); + } + for (const auto & hazard_diag : hazard_status.diagnostics_nf) { + auto diag = hazard_diag; + diag.level = DiagnosticStatus::OK; + error_diagnostics.push_back(hazard_diag); + } + + // filter leaf diag + status->error_diagnostics = extractLeafDiag(error_diagnostics); } void AutowareIvAutowareStatePublisher::getGlobalRptInfo( @@ -143,12 +242,12 @@ void AutowareIvAutowareStatePublisher::getGlobalRptInfo( bool AutowareIvAutowareStatePublisher::isGoal( const autoware_system_msgs::msg::AutowareState::ConstSharedPtr & autoware_state) { - //rename + // rename const auto & aw_state = autoware_state->state; if (aw_state == autoware_system_msgs::msg::AutowareState::ARRIVAL_GOAL) { arrived_goal_ = true; - } else if ( + } else if ( // NOLINT prev_state_ == autoware_system_msgs::msg::AutowareState::DRIVING && aw_state == autoware_system_msgs::msg::AutowareState::WAITING_FOR_ROUTE) { @@ -159,7 +258,7 @@ bool AutowareIvAutowareStatePublisher::isGoal( aw_state == autoware_system_msgs::msg::AutowareState::WAITING_FOR_ENGAGE || aw_state == autoware_system_msgs::msg::AutowareState::DRIVING) { - //cancel goal state + // cancel goal state arrived_goal_ = false; } @@ -206,7 +305,7 @@ void AutowareIvAutowareStatePublisher::updateDiagNameSet( bool AutowareIvAutowareStatePublisher::isLeaf(const diagnostic_msgs::msg::DiagnosticStatus & diag) { - //if not find diag.name in diag set, diag is leaf. + // if not find diag.name in diag set, diag is leaf. return diag_name_set_.find(diag.name) == diag_name_set_.end(); } diff --git a/awapi/awapi_awiv_adapter/src/awapi_autoware_util.cpp b/awapi/awapi_awiv_adapter/src/awapi_autoware_util.cpp index c55c2a16d2ba0..4710651ca06d7 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_autoware_util.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_autoware_util.cpp @@ -12,6 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include + #include "awapi_awiv_adapter/awapi_autoware_util.hpp" namespace autoware_api @@ -21,4 +23,83 @@ double lowpass_filter(const double current_value, const double prev_value, const return gain * prev_value + (1.0 - gain) * current_value; } +namespace planning_util +{ +bool calcClosestIndex( + const autoware_planning_msgs::msg::Trajectory & traj, const geometry_msgs::msg::Pose & pose, + size_t & output_closest_idx, const double dist_thr, const double angle_thr) +{ + double dist_min = std::numeric_limits::max(); + const double yaw_pose = tf2::getYaw(pose.orientation); + int closest_idx = -1; + + for (int i = 0; i < static_cast(traj.points.size()); ++i) { + const double dist = calcDist2d(getPose(traj, i).position, pose.position); + + /* check distance threshold */ + if (dist > dist_thr) {continue;} + + /* check angle threshold */ + double yaw_i = tf2::getYaw(getPose(traj, i).orientation); + double yaw_diff = normalizeEulerAngle(yaw_pose - yaw_i); + + if (std::fabs(yaw_diff) > angle_thr) {continue;} + + if (dist < dist_min) { + dist_min = dist; + closest_idx = i; + } + } + + output_closest_idx = static_cast(closest_idx); + + return closest_idx != -1; +} + +double normalizeEulerAngle(double euler) +{ + double res = euler; + while (res > M_PI) { + res -= (2.0 * M_PI); + } + while (res < -M_PI) { + res += 2.0 * M_PI; + } + + return res; +} + +double calcArcLengthFromWayPoint( + const autoware_planning_msgs::msg::Trajectory & input_path, const size_t src_idx, + const size_t dst_idx) +{ + double length = 0; + for (size_t i = src_idx; i < dst_idx; ++i) { + const double dx_wp = + input_path.points.at(i + 1).pose.position.x - input_path.points.at(i).pose.position.x; + const double dy_wp = + input_path.points.at(i + 1).pose.position.y - input_path.points.at(i).pose.position.y; + length += std::hypot(dx_wp, dy_wp); + } + return length; +} + +double calcDistanceAlongTrajectory( + const autoware_planning_msgs::msg::Trajectory & trajectory, + const geometry_msgs::msg::Pose & current_pose, const geometry_msgs::msg::Pose & target_pose) +{ + size_t self_idx; + size_t stop_idx; + if ( + !calcClosestIndex(trajectory, current_pose, self_idx) || + !calcClosestIndex(trajectory, target_pose, stop_idx)) + { + return std::numeric_limits::max(); + } + const double dist_to_stop_pose = calcArcLengthFromWayPoint(trajectory, self_idx, stop_idx); + return dist_to_stop_pose; +} + +} // namespace planning_util + } // namespace autoware_api diff --git a/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp b/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp index d9dc5d4ce3f40..13b69f22322e5 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp @@ -12,8 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "awapi_awiv_adapter/awapi_awiv_adapter_core.hpp" #include +#include +#include + +#include "awapi_awiv_adapter/awapi_awiv_adapter_core.hpp" namespace autoware_api { @@ -25,18 +28,33 @@ AutowareIvAdapter::AutowareIvAdapter() // get param status_pub_hz_ = this->declare_parameter("status_pub_hz", 5.0); stop_reason_timeout_ = this->declare_parameter("stop_reason_timeout", 0.5); - const bool em_handle_param = this->declare_parameter("use_emergency_handling").get(); - emergencyParamCheck(em_handle_param); + stop_reason_thresh_dist_ = this->declare_parameter("stop_reason_thresh_dist", 100.0); + const double default_max_velocity = waitForParam( + this, declare_parameter("node/max_velocity", ""), + declare_parameter("param/max_velocity", "")); + const bool em_stop_param = waitForParam( + this, declare_parameter("node/emergency_stop", ""), + declare_parameter("param/emergency_stop", "")); + emergencyParamCheck(em_stop_param); // setup instance vehicle_state_publisher_ = std::make_unique(*this); autoware_state_publisher_ = std::make_unique(*this); - stop_reason_aggreagator_ = std::make_unique( - *this, - stop_reason_timeout_); + stop_reason_aggregator_ = + std::make_unique( + *this, stop_reason_timeout_, + stop_reason_thresh_dist_); lane_change_state_publisher_ = std::make_unique(*this); obstacle_avoidance_state_publisher_ = std::make_unique(*this); + max_velocity_publisher_ = + std::make_unique(*this, default_max_velocity); + + // publisher + pub_door_control_ = + this->create_publisher("output/door_control", 1); + pub_door_status_ = + this->create_publisher("output/door_status", 1); // subscriber sub_steer_ = this->create_subscription( @@ -59,8 +77,11 @@ AutowareIvAdapter::AutowareIvAdapter() "input/control_mode", 1, std::bind(&AutowareIvAdapter::callbackControlMode, this, _1)); sub_gate_mode_ = this->create_subscription( "input/gate_mode", 1, std::bind(&AutowareIvAdapter::callbackGateMode, this, _1)); - sub_emergency_ = this->create_subscription( + sub_emergency_ = this->create_subscription( "input/is_emergency", 1, std::bind(&AutowareIvAdapter::callbackIsEmergency, this, _1)); + sub_hazard_status_ = + this->create_subscription( + "input/hazard_status", 1, std::bind(&AutowareIvAdapter::callbackHazardStatus, this, _1)); sub_stop_reason_ = this->create_subscription( "input/stop_reason", 100, std::bind(&AutowareIvAdapter::callbackStopReason, this, _1)); sub_diagnostics_ = this->create_subscription( @@ -82,6 +103,18 @@ AutowareIvAdapter::AutowareIvAdapter() this->create_subscription( "input/obstacle_avoid_candidate_path", 1, std::bind(&AutowareIvAdapter::callbackLaneObstacleAvoidCandidatePath, this, _1)); + sub_max_velocity_ = this->create_subscription( + "input/max_velocity", 1, std::bind(&AutowareIvAdapter::callbackMaxVelocity, this, _1)); + sub_temporary_stop_ = this->create_subscription( + "input/temporary_stop", 1, std::bind(&AutowareIvAdapter::callbackTemporaryStop, this, _1)); + sub_autoware_traj_ = + this->create_subscription( + "input/autoware_trajectory", 1, + std::bind(&AutowareIvAdapter::callbackAutowareTrajectory, this, _1)); + sub_door_control_ = this->create_subscription( + "input/door_control", 1, std::bind(&AutowareIvAdapter::callbackDoorControl, this, _1)); + sub_door_status_ = this->create_subscription( + "input/door_status", 1, std::bind(&AutowareIvAdapter::callbackDoorStatus, this, _1)); // timer auto timer_callback = std::bind(&AutowareIvAdapter::timerCallback, this); @@ -93,10 +126,10 @@ AutowareIvAdapter::AutowareIvAdapter() this->get_node_timers_interface()->add_timer(timer_, nullptr); } -void AutowareIvAdapter::emergencyParamCheck(const bool emergency_handling_param) +void AutowareIvAdapter::emergencyParamCheck(const bool emergency_stop_param) { - if (!emergency_handling_param) { - RCLCPP_WARN_STREAM(get_logger(), "parameter[use_emergency_handling] is false."); + if (!emergency_stop_param) { + RCLCPP_WARN_STREAM(get_logger(), "parameter[use_external_emergency_stop] is false."); RCLCPP_WARN_STREAM(get_logger(), "autoware/put/emergency is not valid"); } } @@ -117,6 +150,9 @@ void AutowareIvAdapter::timerCallback() // publish obstacle_avoidance state obstacle_avoidance_state_publisher_->statePublisher(aw_info_); + + // publish pacmod door status + pub_door_status_->publish(pacmod_util::getDoorStatusMsg(aw_info_.door_state_ptr)); } void AutowareIvAdapter::callbackSteer( @@ -172,8 +208,7 @@ void AutowareIvAdapter::getCurrentPose() aw_info_.current_pose_ptr = std::make_shared(ps); } catch (tf2::TransformException & ex) { RCLCPP_DEBUG_STREAM_THROTTLE( - get_logger(), - *this->get_clock(), 2000 /* ms */, "cannot get self pose"); + get_logger(), *this->get_clock(), 2000 /* ms */, "cannot get self pose"); } } @@ -195,15 +230,21 @@ void AutowareIvAdapter::callbackGateMode( } void AutowareIvAdapter::callbackIsEmergency( - const autoware_control_msgs::msg::EmergencyMode::ConstSharedPtr msg_ptr) + const std_msgs::msg::Bool::ConstSharedPtr msg_ptr) { aw_info_.is_emergency_ptr = msg_ptr; } +void AutowareIvAdapter::callbackHazardStatus( + const autoware_system_msgs::msg::HazardStatusStamped::ConstSharedPtr msg_ptr) +{ + aw_info_.hazard_status_ptr = msg_ptr; +} + void AutowareIvAdapter::callbackStopReason( const autoware_planning_msgs::msg::StopReasonArray::ConstSharedPtr msg_ptr) { - aw_info_.stop_reason_ptr = stop_reason_aggreagator_->updateStopReasonArray(msg_ptr); + aw_info_.stop_reason_ptr = stop_reason_aggregator_->updateStopReasonArray(msg_ptr, aw_info_); } void AutowareIvAdapter::callbackDiagnostics( @@ -246,4 +287,43 @@ void AutowareIvAdapter::callbackLaneObstacleAvoidCandidatePath( aw_info_.obstacle_avoid_candidate_ptr = msg_ptr; } +void AutowareIvAdapter::callbackMaxVelocity(const std_msgs::msg::Float32::ConstSharedPtr msg_ptr) +{ + aw_info_.max_velocity_ptr = msg_ptr; + max_velocity_publisher_->statePublisher(aw_info_); +} + +void AutowareIvAdapter::callbackTemporaryStop(const std_msgs::msg::Bool::ConstSharedPtr msg_ptr) +{ + if (aw_info_.temporary_stop_ptr) { + if (aw_info_.temporary_stop_ptr->data == msg_ptr->data) { + // if same value as last time is sent, ignore msg. + return; + } + } + + aw_info_.temporary_stop_ptr = msg_ptr; + max_velocity_publisher_->statePublisher(aw_info_); +} + +void AutowareIvAdapter::callbackAutowareTrajectory( + const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr msg_ptr) +{ + aw_info_.autoware_planning_traj_ptr = msg_ptr; +} + +void AutowareIvAdapter::callbackDoorControl( + const std_msgs::msg::Bool::ConstSharedPtr msg_ptr) +{ + pub_door_control_->publish(pacmod_util::createClearOverrideDoorCommand(this->get_clock())); + rclcpp::Rate(10.0).sleep(); // avoid message loss + pub_door_control_->publish(pacmod_util::createDoorCommand(this->get_clock(), msg_ptr)); +} + +void AutowareIvAdapter::callbackDoorStatus( + const pacmod_msgs::msg::SystemRptInt::ConstSharedPtr msg_ptr) +{ + aw_info_.door_state_ptr = msg_ptr; +} + } // namespace autoware_api diff --git a/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_node.cpp b/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_node.cpp index 8de3904a87630..7ea57331dfeaa 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_node.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_node.cpp @@ -12,6 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include + #include "rclcpp/rclcpp.hpp" #include "awapi_awiv_adapter/awapi_awiv_adapter_core.hpp" diff --git a/awapi/awapi_awiv_adapter/src/awapi_lane_change_state_publisher.cpp b/awapi/awapi_awiv_adapter/src/awapi_lane_change_state_publisher.cpp index 17c5deb08a942..8c1840a37dbba 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_lane_change_state_publisher.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_lane_change_state_publisher.cpp @@ -29,7 +29,7 @@ void AutowareIvLaneChangeStatePublisher::statePublisher(const AutowareInfo & aw_ { autoware_api_msgs::msg::LaneChangeStatus status; - //input header + // input header status.header.frame_id = "base_link"; status.header.stamp = clock_->now(); diff --git a/awapi/awapi_awiv_adapter/src/awapi_max_velocity_publisher.cpp b/awapi/awapi_awiv_adapter/src/awapi_max_velocity_publisher.cpp new file mode 100644 index 0000000000000..9dedc3d667078 --- /dev/null +++ b/awapi/awapi_awiv_adapter/src/awapi_max_velocity_publisher.cpp @@ -0,0 +1,58 @@ +// Copyright 2020 Tier IV, Inc. +// +// 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. + +#include "awapi_awiv_adapter/awapi_max_velocity_publisher.hpp" + +namespace autoware_api +{ +AutowareIvMaxVelocityPublisher::AutowareIvMaxVelocityPublisher( + rclcpp::Node & node, const double default_max_velocity) +: default_max_velocity_(default_max_velocity) +{ + // publisher + pub_state_ = node.create_publisher("output/max_velocity", 1); +} + +void AutowareIvMaxVelocityPublisher::statePublisher(const AutowareInfo & aw_info) +{ + std_msgs::msg::Float32 max_velocity; + if (calcMaxVelocity( + aw_info.max_velocity_ptr, aw_info.temporary_stop_ptr, &max_velocity.data)) // publish info + { + pub_state_->publish(max_velocity); + } +} + +bool AutowareIvMaxVelocityPublisher::calcMaxVelocity( + const std_msgs::msg::Float32::ConstSharedPtr & max_velocity_ptr, + const std_msgs::msg::Bool::ConstSharedPtr & temporary_stop_ptr, float * max_velocity) +{ + if (!max_velocity_ptr && !temporary_stop_ptr) { + // receive no max velocity information + return false; + } + + // input max velocity + *max_velocity = + static_cast(max_velocity_ptr ? max_velocity_ptr->data : default_max_velocity_); + + if (temporary_stop_ptr && temporary_stop_ptr->data) { + // if temporary_stop is true, max velocity is 0 + *max_velocity = static_cast(0.0); + } + + return true; +} + +} // namespace autoware_api diff --git a/awapi/awapi_awiv_adapter/src/awapi_obstacle_avoidance_state_publisher.cpp b/awapi/awapi_awiv_adapter/src/awapi_obstacle_avoidance_state_publisher.cpp index 77ed7c9334f37..4b0f956daed5b 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_obstacle_avoidance_state_publisher.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_obstacle_avoidance_state_publisher.cpp @@ -30,7 +30,7 @@ void AutowareIvObstacleAvoidanceStatePublisher::statePublisher(const AutowareInf { autoware_api_msgs::msg::ObstacleAvoidanceStatus status; - //input header + // input header status.header.frame_id = "base_link"; status.header.stamp = clock_->now(); diff --git a/awapi/awapi_awiv_adapter/src/awapi_pacmod_util.cpp b/awapi/awapi_awiv_adapter/src/awapi_pacmod_util.cpp new file mode 100644 index 0000000000000..3f4d782ed1bf1 --- /dev/null +++ b/awapi/awapi_awiv_adapter/src/awapi_pacmod_util.cpp @@ -0,0 +1,90 @@ +// Copyright 2020 Tier IV, Inc. +// +// 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. + +#include "awapi_awiv_adapter/awapi_pacmod_util.hpp" + +namespace autoware_api +{ +namespace pacmod_util +{ +autoware_api_msgs::msg::DoorStatus getDoorStatusMsg( + const pacmod_msgs::msg::SystemRptInt::ConstSharedPtr & msg_ptr) +{ + using autoware_api_msgs::msg::DoorStatus; + using pacmod_msgs::msg::SystemRptInt; + DoorStatus door_status; + + if (!msg_ptr) { + door_status.status = DoorStatus::NOT_APPLICABLE; + return door_status; + } + + door_status.status = DoorStatus::UNKNOWN; + + if ( + msg_ptr->command == SystemRptInt::DOOR_CLOSE && + msg_ptr->output == SystemRptInt::DOOR_OPEN) + { + // do not used (command & output are always the same value) + door_status.status = DoorStatus::DOOR_CLOSING; + } else if ( // NOLINT + msg_ptr->command == SystemRptInt::DOOR_OPEN && + msg_ptr->output == SystemRptInt::DOOR_CLOSE) + { + // do not used (command & output are always the same value) + door_status.status = DoorStatus::DOOR_OPENING; + } else if (msg_ptr->output == SystemRptInt::DOOR_CLOSE) { + door_status.status = DoorStatus::DOOR_CLOSED; + } else if (msg_ptr->output == SystemRptInt::DOOR_OPEN) { + door_status.status = DoorStatus::DOOR_OPENED; + } + + return door_status; +} + +pacmod_msgs::msg::SystemCmdInt createClearOverrideDoorCommand( + const rclcpp::Clock::SharedPtr & clock) +{ + pacmod_msgs::msg::SystemCmdInt door_cmd; + door_cmd.header.frame_id = "base_link"; + door_cmd.header.stamp = clock->now(); + door_cmd.clear_override = true; + return door_cmd; +} + +pacmod_msgs::msg::SystemCmdInt createDoorCommand( + const rclcpp::Clock::SharedPtr & clock, + const std_msgs::msg::Bool::ConstSharedPtr & msg_ptr) +{ + using pacmod_msgs::msg::SystemCmdInt; + + SystemCmdInt door_cmd; + door_cmd.header.frame_id = "base_link"; + door_cmd.header.stamp = clock->now(); + door_cmd.enable = true; + door_cmd.command = SystemCmdInt::DOOR_NEUTRAL; + + if (!msg_ptr) {return door_cmd;} + + if (msg_ptr->data) { + door_cmd.command = SystemCmdInt::DOOR_OPEN; + } else { + door_cmd.command = SystemCmdInt::DOOR_CLOSE; + } + return door_cmd; +} + +} // namespace pacmod_util + +} // namespace autoware_api diff --git a/awapi/awapi_awiv_adapter/src/awapi_stop_reason_aggregator.cpp b/awapi/awapi_awiv_adapter/src/awapi_stop_reason_aggregator.cpp index b324dc9a7c571..d5871190785e6 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_stop_reason_aggregator.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_stop_reason_aggregator.cpp @@ -12,33 +12,39 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include +#include + #include "awapi_awiv_adapter/awapi_stop_reason_aggregator.hpp" namespace autoware_api { AutowareIvStopReasonAggregator::AutowareIvStopReasonAggregator( rclcpp::Node & node, - const double timeout) + const double timeout, const double thresh_dist_to_stop_pose) : logger_(node.get_logger().get_child("awapi_awiv_stop_reason_aggregator")), clock_(node.get_clock()), - timeout_(timeout) + timeout_(timeout), + thresh_dist_to_stop_pose_(thresh_dist_to_stop_pose) { } autoware_planning_msgs::msg::StopReasonArray::ConstSharedPtr AutowareIvStopReasonAggregator::updateStopReasonArray( - const autoware_planning_msgs::msg::StopReasonArray::ConstSharedPtr & msg_ptr) + const autoware_planning_msgs::msg::StopReasonArray::ConstSharedPtr & msg_ptr, + const AutowareInfo & aw_info) { - applyUpdate(msg_ptr); + applyUpdate(msg_ptr, aw_info); applyTimeOut(); - return makeStopReasonArray(); + return makeStopReasonArray(aw_info); } void AutowareIvStopReasonAggregator::applyUpdate( - const autoware_planning_msgs::msg::StopReasonArray::ConstSharedPtr & msg_ptr) + const autoware_planning_msgs::msg::StopReasonArray::ConstSharedPtr & msg_ptr, + const AutowareInfo & aw_info) { /* remove old stop_reason that matches reason with received msg */ - //make reason-matching msg list + // make reason-matching msg list std::vector remove_idx; if (!stop_reason_array_vec_.empty()) { @@ -65,7 +71,7 @@ bool AutowareIvStopReasonAggregator::checkMatchingReason( for (const auto msg_stop_reason : msg_stop_reason_array->stop_reasons) { for (const auto stop_reason : stop_reason_array.stop_reasons) { if (msg_stop_reason.reason == stop_reason.reason) { - //find matching reason + // find matching reason return true; } } @@ -78,7 +84,7 @@ void AutowareIvStopReasonAggregator::applyTimeOut() { const auto current_time = clock_->now(); - //make timeout-msg list + // make timeout-msg list std::vector remove_idx; if (!stop_reason_array_vec_.empty()) { for (int i = stop_reason_array_vec_.size() - 1; i >= 0; i--) { @@ -90,7 +96,7 @@ void AutowareIvStopReasonAggregator::applyTimeOut() } } } - //remove timeout-msg + // remove timeout-msg for (const auto idx : remove_idx) { stop_reason_array_vec_.erase(stop_reason_array_vec_.begin() + idx); } @@ -98,29 +104,35 @@ void AutowareIvStopReasonAggregator::applyTimeOut() void AutowareIvStopReasonAggregator::appendStopReasonToArray( const autoware_planning_msgs::msg::StopReason & stop_reason, - autoware_planning_msgs::msg::StopReasonArray * stop_reason_array) + autoware_planning_msgs::msg::StopReasonArray * stop_reason_array, const AutowareInfo & aw_info) { - //if stop factors is empty, not append - if (stop_reason.stop_factors.empty()) { + // calculate dist_to_stop_pose + const auto stop_reason_with_dist = inputStopDistToStopReason(stop_reason, aw_info); + + // cut stop reason + const auto near_stop_reason = getNearStopReason(stop_reason_with_dist, aw_info); + + // if stop factors is empty, not append + if (near_stop_reason.stop_factors.empty()) { return; } - //if already exists same reason msg in stop_reason_array_msg, append stop_factors to there - for (size_t i = 0; i < stop_reason_array->stop_reasons.size(); i++) { - if (stop_reason_array->stop_reasons.at(i).reason == stop_reason.reason) { - stop_reason_array->stop_reasons.at(i).stop_factors.insert( - stop_reason_array->stop_reasons.at(i).stop_factors.end(), stop_reason.stop_factors.begin(), - stop_reason.stop_factors.end()); + // if already exists same reason msg in stop_reason_array_msg, append stop_factors to there + for (auto & base_stop_reasons : stop_reason_array->stop_reasons) { + if (base_stop_reasons.reason == near_stop_reason.reason) { + base_stop_reasons.stop_factors.insert( + base_stop_reasons.stop_factors.end(), near_stop_reason.stop_factors.begin(), + near_stop_reason.stop_factors.end()); return; } } - //if not exist same reason msg, append new stop reason - stop_reason_array->stop_reasons.emplace_back(stop_reason); + // if not exist same reason msg, append new stop reason + stop_reason_array->stop_reasons.emplace_back(near_stop_reason); } autoware_planning_msgs::msg::StopReasonArray::ConstSharedPtr -AutowareIvStopReasonAggregator::makeStopReasonArray() +AutowareIvStopReasonAggregator::makeStopReasonArray(const AutowareInfo & aw_info) { autoware_planning_msgs::msg::StopReasonArray stop_reason_array_msg; // input header @@ -130,10 +142,47 @@ AutowareIvStopReasonAggregator::makeStopReasonArray() // input stop reason for (const auto stop_reason_array : stop_reason_array_vec_) { for (const auto stop_reason : stop_reason_array.stop_reasons) { - appendStopReasonToArray(stop_reason, &stop_reason_array_msg); + appendStopReasonToArray(stop_reason, &stop_reason_array_msg, aw_info); } } return std::make_shared(stop_reason_array_msg); } +autoware_planning_msgs::msg::StopReason AutowareIvStopReasonAggregator::inputStopDistToStopReason( + const autoware_planning_msgs::msg::StopReason & stop_reason, const AutowareInfo & aw_info) +{ + if (!aw_info.autoware_planning_traj_ptr || !aw_info.current_pose_ptr) { + // pass through all stop reason + return stop_reason; + } + + auto stop_reason_with_dist = stop_reason; + for (auto & stop_factor : stop_reason_with_dist.stop_factors) { + const auto & trajectory = *aw_info.autoware_planning_traj_ptr; + const auto & current_pose = aw_info.current_pose_ptr->pose; + stop_factor.dist_to_stop_pose = + planning_util::calcDistanceAlongTrajectory(trajectory, current_pose, stop_factor.stop_pose); + } + return stop_reason_with_dist; +} + +autoware_planning_msgs::msg::StopReason AutowareIvStopReasonAggregator::getNearStopReason( + const autoware_planning_msgs::msg::StopReason & stop_reason, const AutowareInfo & aw_info) +{ + if (!aw_info.autoware_planning_traj_ptr || !aw_info.current_pose_ptr) { + // pass through all stop reason + return stop_reason; + } + + autoware_planning_msgs::msg::StopReason near_stop_reason; + near_stop_reason.reason = stop_reason.reason; + for (const auto stop_factor : stop_reason.stop_factors) { + if (stop_factor.dist_to_stop_pose < thresh_dist_to_stop_pose_) { + // append only near stop factor + near_stop_reason.stop_factors.emplace_back(stop_factor); + } + } + return near_stop_reason; +} + } // namespace autoware_api diff --git a/awapi/awapi_awiv_adapter/src/awapi_vehicle_state_publisher.cpp b/awapi/awapi_awiv_adapter/src/awapi_vehicle_state_publisher.cpp index 9c9f070961e2c..8b2f67b339c55 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_vehicle_state_publisher.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_vehicle_state_publisher.cpp @@ -12,6 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include +#include + #include "awapi_awiv_adapter/awapi_vehicle_state_publisher.hpp" namespace autoware_api @@ -30,7 +33,7 @@ void AutowareIvVehicleStatePublisher::statePublisher(const AutowareInfo & aw_inf { autoware_api_msgs::msg::AwapiVehicleStatus status = initVehicleStatus(); - //input header + // input header status.header.frame_id = "base_link"; status.header.stamp = clock_->now(); @@ -92,7 +95,7 @@ void AutowareIvVehicleStatePublisher::getSteerInfo( // get steer vel if (previous_steer_ptr_) { - //calculate steer vel from steer + // calculate steer vel from steer const double ds = steer_ptr->data - previous_steer_ptr_->data; const double dt = std::max( (rclcpp::Time(steer_ptr->header.stamp) - rclcpp::Time(previous_steer_ptr_->header.stamp)) @@ -100,7 +103,7 @@ void AutowareIvVehicleStatePublisher::getSteerInfo( 1e-03); const double steer_vel = ds / dt; - //apply lowpass filter + // apply lowpass filter const double lowpass_steer = lowpass_filter(steer_vel, prev_steer_vel_, steer_vel_lowpass_gain_); prev_steer_vel_ = lowpass_steer; @@ -133,7 +136,7 @@ void AutowareIvVehicleStatePublisher::getTurnSignalInfo( return; } - //get turn singnal + // get turn signal status->turn_signal = turn_signal_ptr->data; } @@ -152,7 +155,7 @@ void AutowareIvVehicleStatePublisher::getTwistInfo( // get accel if (previous_twist_ptr_) { - //calculate accleration from velocity + // calculate acceleration from velocity const double dv = twist_ptr->twist.linear.x - previous_twist_ptr_->twist.linear.x; const double dt = std::max( (rclcpp::Time(twist_ptr->header.stamp) - rclcpp::Time(previous_twist_ptr_->header.stamp)) @@ -203,10 +206,10 @@ void AutowareIvVehicleStatePublisher::getGpsInfo( return; } - // get position (latlon) - status->latlon.lat = nav_sat_ptr->latitude; - status->latlon.lon = nav_sat_ptr->longitude; - status->latlon.alt = nav_sat_ptr->altitude; + // get geo_point + status->geo_point.latitude = nav_sat_ptr->latitude; + status->geo_point.longitude = nav_sat_ptr->longitude; + status->geo_point.altitude = nav_sat_ptr->altitude; } } // namespace autoware_api From 4d5070426d08e2fca6e7b6c71045615168a9fa55 Mon Sep 17 00:00:00 2001 From: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Date: Wed, 17 Feb 2021 11:38:54 +0900 Subject: [PATCH 24/71] Ros2 v0.8.0 engage (#342) * [autoware_vehicle_msgs]: Add engage message Signed-off-by: wep21 * [as]: Update message Signed-off-by: wep21 * [awapi_awiv_adapter]: Update message Signed-off-by: wep21 * [web_controller]: Update message Signed-off-by: wep21 * [vehicle_cmd_gate]: Update message Signed-off-by: wep21 * [autoware_state_monitor]: Update message Signed-off-by: wep21 * [autoware_control_msgs]: Remove EngageMode message Signed-off-by: wep21 * [simple_planning_simulator]: Update message Signed-off-by: wep21 Signed-off-by: tanaka3 --- awapi/awapi_awiv_adapter/Readme.md | 4 ++-- awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/awapi/awapi_awiv_adapter/Readme.md b/awapi/awapi_awiv_adapter/Readme.md index f7a1741ca833e..a2df14d422f25 100644 --- a/awapi/awapi_awiv_adapter/Readme.md +++ b/awapi/awapi_awiv_adapter/Readme.md @@ -183,11 +183,11 @@ ### /awapi/autoware/put/engage - send engage signal (both of autoware/engage and vehicle/engage) -- MessageType: autoware_control_msgs/EngageMode +- MessageType: autoware_vehicle_msgs/Engage | ✓ | type | name | unit | note | | --- | :------------------------------- | :--- | :--- | :--- | -| ✓ | autoware_control_msgs/EngageMode | | | | +| ✓ | autoware_vehicle_msgs/Engage | | | | ### /awapi/autoware/put/goal diff --git a/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml b/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml index b90f56f5daeae..089d766a239e8 100644 --- a/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml +++ b/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml @@ -154,12 +154,12 @@ - + - + From b77b9ce0c494bb6d9f6a2ac4cc78e39bd8ff99a5 Mon Sep 17 00:00:00 2001 From: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> Date: Mon, 22 Feb 2021 14:22:20 +0900 Subject: [PATCH 25/71] Ros2 v0.8.0 remove std msgs awapi (#348) * [autoware_vehicle_msgs] add BatteryStatus msg Signed-off-by: mitsudome-r * [autoware_planning_msgs] add ExpandStopRange and StopSpeedExceeded messages Signed-off-by: mitsudome-r * [autoware_api_msgs] add DoorControlCommand, StopCommand, and VelocityLimit messages Signed-off-by: mitsudome-r * remove std_msgs related to autoware_awaiv_adapter node Signed-off-by: mitsudome-r * apply ament_uncrustify Signed-off-by: mitsudome-r * fix build failure Signed-off-by: mitsudome-r * fix test failures Signed-off-by: mitsudome-r * address review commends Signed-off-by: mitsudome-r Signed-off-by: tanaka3 --- awapi/awapi_awiv_adapter/Readme.md | 34 ++++++++-------- .../awapi_autoware_state_publisher.hpp | 2 +- .../awapi_autoware_util.hpp | 21 +++++----- .../awapi_awiv_adapter_core.hpp | 39 ++++++++++-------- .../awapi_lane_change_state_publisher.hpp | 4 +- .../awapi_max_velocity_publisher.hpp | 8 ++-- ...api_obstacle_avoidance_state_publisher.hpp | 2 +- .../awapi_awiv_adapter/awapi_pacmod_util.hpp | 5 ++- .../awapi_vehicle_state_publisher.hpp | 2 +- .../launch/awapi_awiv_adapter.launch.xml | 16 +++++--- .../src/awapi_autoware_state_publisher.cpp | 4 +- .../src/awapi_awiv_adapter_core.cpp | 40 +++++++++++-------- .../src/awapi_lane_change_state_publisher.cpp | 8 ++-- .../src/awapi_max_velocity_publisher.cpp | 18 +++++---- ...api_obstacle_avoidance_state_publisher.cpp | 4 +- .../src/awapi_pacmod_util.cpp | 4 +- .../src/awapi_vehicle_state_publisher.cpp | 4 +- 17 files changed, 121 insertions(+), 94 deletions(-) diff --git a/awapi/awapi_awiv_adapter/Readme.md b/awapi/awapi_awiv_adapter/Readme.md index a2df14d422f25..b7932db47b517 100644 --- a/awapi/awapi_awiv_adapter/Readme.md +++ b/awapi/awapi_awiv_adapter/Readme.md @@ -65,11 +65,11 @@ - get flag of exceeding stop speed or not - True: exceed the stop speed ( = "cannot stop before the stop line") - False: not exceed the stop speed ( = "no stop line in the trajectory" or "possible to stop before the stop line" ) -- MessageType: std_msgs/Bool +- MessageType: autoware_planning_msgs/StopSpeedExceedStatus | ✓ | type | name | unit | note | | --- | :------------ | :--- | :--- | :--- | -| | std_msgs/Bool | | - | | +| | autoware_planning_msgs/StopSpeedExceedStatus | | - | | ### /awapi/prediction/get/objects @@ -140,16 +140,16 @@ ### /awapi/vehicle/put/velocity - set upper velocity -- MessageType: std_msgs/Float32 +- MessageType: autoware_api_msgs/VelocityLimit | ✓ | type | name | unit | note | | --- | :--------------- | :--- | :--- | :----------- | -| ✓ | std_msgs/Float32 | | | max velocity | +| ✓ | autoware_api_msgs/VelocityLimit | | | max velocity | ### /awapi/vehicle/put/stop - set temporary stop signal -- MessageType: std_msgs/bool +- MessageType: autoware_api_msgs/StopCommand - Specification - send True: send upper velocity to 0 @@ -159,7 +159,7 @@ | ✓ | type | name | unit | note | | --- | :------------ | :--- | :--- | :--- | - | ✓ | std_msgs/Bool | | | | + | ✓ | autoware_api_msgs/StopCommand | | | | ### /awapi/autoware/put/gate_mode @@ -173,7 +173,7 @@ ### /awapi/autoware/put/emergency_stop - send emergency_stop signal -- MessageType: std_msgs/Bool +- MessageType: autoware_control_msgs/EmergencyMode - **To enable this functionality, autoware have to be in the Remote Mode or set _/control/vehicle_cmd_gate/use_external_emergency_stop_ to true.** | ✓ | type | name | unit | note | @@ -211,30 +211,30 @@ - send lane change approval flag - send True: start lane change when **lane_change_ready** is true -- MessageType: std_msgs/Bool +- MessageType: autoware_planning_msgs/LaneChangeCommand | ✓ | type | name | unit | note | | --- | :------------ | :--- | :--- | :--- | -| | std_msgs/Bool | | | | +| | autoware_planning_msgs/msg/LaneChangeCommand | | | | ### /awapi/lane_change/put/force - send force lane change flag - send True: start lane change when **force_lane_change_available** is true -- MessageType: std_msgs/Bool +- MessageType: autoware_planning_msgs/LaneChangeCommand | ✓ | type | name | unit | note | | --- | :------------ | :--- | :--- | :--- | -| | std_msgs/Bool | | | | +| | autoware_planning_msgs/LaneChangeCommand | | | | ### /awapi/object_avoidance/put/approval - send object avoidance approval flag -- MessageType: std_msgs/Bool +- MessageType: autoware_planning_msgs/EnableAvoidance | ✓ | type | name | unit | note | | --- | :------------ | :--- | :--- | :--- | -| | std_msgs/Bool | | | | +| | autoware_planning_msgs/EnableAvoidance | | | | ### /awapi/object_avoidance/put/force @@ -256,13 +256,13 @@ ### /awapi/vehicle/put/door - send door command -- MessageType: std_msgs/Bool +- MessageType: autoware_api_msgs/DoorCommand - send True: open door - send False: close door | ✓ | type | name | unit | note | | --- | :------------ | :--- | :--- | :------------------------------------------ | -| | std_msgs/Bool | | | available only for the vehicle using pacmod | +| | autoware_api_msgs/DoorCommand | | | available only for the vehicle using pacmod | ### /awapi/autoware/put/crosswalk_states @@ -289,8 +289,8 @@ ### /awapi/autoware/put/expand_stop_range - send expand range of the polygon used by obstacle stop [m] -- MessageType: std_msgs/Float32 +- MessageType: autoware_planning_msgs/ExpandStopRange | ✓ | type | name | unit | note | | --- | :------ | :--- | :--- | :--- | -| | Float32 | | | | +| | autoware_planning_msgs/ExpandStopRange | | | | diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.hpp b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.hpp index cb034e56502e7..3d857dd3cf70d 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.hpp +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.hpp @@ -57,7 +57,7 @@ class AutowareIvAutowareStatePublisher const autoware_control_msgs::msg::GateMode::ConstSharedPtr & gate_mode_ptr, autoware_api_msgs::msg::AwapiAutowareStatus * status); void getIsEmergencyInfo( - const std_msgs::msg::Bool::ConstSharedPtr & is_emergency_ptr, + const autoware_control_msgs::msg::EmergencyMode::ConstSharedPtr & is_emergency_ptr, autoware_api_msgs::msg::AwapiAutowareStatus * status); void getHazardStatusInfo( const autoware_system_msgs::msg::HazardStatusStamped::ConstSharedPtr & hazard_status_ptr, diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.hpp b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.hpp index a729910a4735e..b27bf05897fd5 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.hpp +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.hpp @@ -23,19 +23,22 @@ #include "pacmod_msgs/msg/global_rpt.hpp" #include "rclcpp/rclcpp.hpp" #include "sensor_msgs/msg/nav_sat_fix.hpp" -#include "std_msgs/msg/bool.hpp" -#include "std_msgs/msg/float32.hpp" #include "tf2/utils.h" #include "tf2_ros/transform_broadcaster.h" #include "tf2_ros/transform_listener.h" +#include "autoware_api_msgs/msg/velocity_limit.hpp" +#include "autoware_api_msgs/msg/stop_command.hpp" #include "autoware_control_msgs/msg/emergency_mode.hpp" #include "autoware_control_msgs/msg/gate_mode.hpp" +#include "autoware_planning_msgs/msg/is_avoidance_possible.hpp" +#include "autoware_planning_msgs/msg/lane_change_status.hpp" #include "autoware_planning_msgs/msg/path.hpp" #include "autoware_planning_msgs/msg/stop_reason_array.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" #include "autoware_system_msgs/msg/autoware_state.hpp" #include "autoware_system_msgs/msg/hazard_status_stamped.hpp" +#include "autoware_vehicle_msgs/msg/battery_status.hpp" #include "autoware_vehicle_msgs/msg/control_mode.hpp" #include "autoware_vehicle_msgs/msg/shift_stamped.hpp" #include "autoware_vehicle_msgs/msg/steering.hpp" @@ -54,23 +57,23 @@ struct AutowareInfo autoware_vehicle_msgs::msg::TurnSignal::ConstSharedPtr turn_signal_ptr; geometry_msgs::msg::TwistStamped::ConstSharedPtr twist_ptr; autoware_vehicle_msgs::msg::ShiftStamped::ConstSharedPtr gear_ptr; - std_msgs::msg::Float32::ConstSharedPtr battery_ptr; + autoware_vehicle_msgs::msg::BatteryStatus::ConstSharedPtr battery_ptr; sensor_msgs::msg::NavSatFix::ConstSharedPtr nav_sat_ptr; autoware_system_msgs::msg::AutowareState::ConstSharedPtr autoware_state_ptr; autoware_vehicle_msgs::msg::ControlMode::ConstSharedPtr control_mode_ptr; autoware_control_msgs::msg::GateMode::ConstSharedPtr gate_mode_ptr; - std_msgs::msg::Bool::ConstSharedPtr is_emergency_ptr; + autoware_control_msgs::msg::EmergencyMode::ConstSharedPtr is_emergency_ptr; autoware_system_msgs::msg::HazardStatusStamped::ConstSharedPtr hazard_status_ptr; autoware_planning_msgs::msg::StopReasonArray::ConstSharedPtr stop_reason_ptr; diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr diagnostic_ptr; pacmod_msgs::msg::GlobalRpt::ConstSharedPtr global_rpt_ptr; - std_msgs::msg::Bool::ConstSharedPtr lane_change_available_ptr; - std_msgs::msg::Bool::ConstSharedPtr lane_change_ready_ptr; + autoware_planning_msgs::msg::LaneChangeStatus::ConstSharedPtr lane_change_available_ptr; + autoware_planning_msgs::msg::LaneChangeStatus::ConstSharedPtr lane_change_ready_ptr; autoware_planning_msgs::msg::Path::ConstSharedPtr lane_change_candidate_ptr; - std_msgs::msg::Bool::ConstSharedPtr obstacle_avoid_ready_ptr; + autoware_planning_msgs::msg::IsAvoidancePossible::ConstSharedPtr obstacle_avoid_ready_ptr; autoware_planning_msgs::msg::Trajectory::ConstSharedPtr obstacle_avoid_candidate_ptr; - std_msgs::msg::Float32::ConstSharedPtr max_velocity_ptr; - std_msgs::msg::Bool::ConstSharedPtr temporary_stop_ptr; + autoware_api_msgs::msg::VelocityLimit::ConstSharedPtr max_velocity_ptr; + autoware_api_msgs::msg::StopCommand::ConstSharedPtr temporary_stop_ptr; autoware_planning_msgs::msg::Trajectory::ConstSharedPtr autoware_planning_traj_ptr; pacmod_msgs::msg::SystemRptInt::ConstSharedPtr door_state_ptr; }; diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.hpp b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.hpp index d491c252d1333..a52349fed38e2 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.hpp +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.hpp @@ -58,28 +58,31 @@ class AutowareIvAdapter : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_turn_signal_cmd_; rclcpp::Subscription::SharedPtr sub_twist_; rclcpp::Subscription::SharedPtr sub_gear_; - rclcpp::Subscription::SharedPtr sub_battery_; + rclcpp::Subscription::SharedPtr sub_battery_; rclcpp::Subscription::SharedPtr sub_nav_sat_; rclcpp::Subscription::SharedPtr sub_autoware_state_; rclcpp::Subscription::SharedPtr sub_control_mode_; rclcpp::Subscription::SharedPtr sub_gate_mode_; - rclcpp::Subscription::SharedPtr sub_emergency_; + rclcpp::Subscription::SharedPtr sub_emergency_; rclcpp::Subscription::SharedPtr sub_hazard_status_; rclcpp::Subscription::SharedPtr sub_stop_reason_; rclcpp::Subscription::SharedPtr sub_diagnostics_; rclcpp::Subscription::SharedPtr sub_global_rpt_; - rclcpp::Subscription::SharedPtr sub_lane_change_available_; - rclcpp::Subscription::SharedPtr sub_lane_change_ready_; + rclcpp::Subscription::SharedPtr + sub_lane_change_available_; + rclcpp::Subscription::SharedPtr + sub_lane_change_ready_; rclcpp::Subscription::SharedPtr sub_lane_change_candidate_; - rclcpp::Subscription::SharedPtr sub_obstacle_avoid_ready_; + rclcpp::Subscription::SharedPtr + sub_obstacle_avoid_ready_; rclcpp::Subscription::SharedPtr sub_obstacle_avoid_candidate_; - rclcpp::Subscription::SharedPtr sub_max_velocity_; - rclcpp::Subscription::SharedPtr sub_temporary_stop_; + rclcpp::Subscription::SharedPtr sub_max_velocity_; + rclcpp::Subscription::SharedPtr sub_temporary_stop_; rclcpp::Subscription::SharedPtr sub_autoware_traj_; - rclcpp::Subscription::SharedPtr sub_door_control_; + rclcpp::Subscription::SharedPtr sub_door_control_; rclcpp::Subscription::SharedPtr sub_door_status_; // publisher @@ -99,31 +102,35 @@ class AutowareIvAdapter : public rclcpp::Node void callbackTurnSignal(const autoware_vehicle_msgs::msg::TurnSignal::ConstSharedPtr msg_ptr); void callbackTwist(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg_ptr); void callbackGear(const autoware_vehicle_msgs::msg::ShiftStamped::ConstSharedPtr msg_ptr); - void callbackBattery(const std_msgs::msg::Float32::ConstSharedPtr msg_ptr); + void callbackBattery(const autoware_vehicle_msgs::msg::BatteryStatus::ConstSharedPtr msg_ptr); void callbackNavSat(const sensor_msgs::msg::NavSatFix::ConstSharedPtr msg_ptr); void callbackAutowareState( const autoware_system_msgs::msg::AutowareState::ConstSharedPtr msg_ptr); void callbackControlMode(const autoware_vehicle_msgs::msg::ControlMode::ConstSharedPtr msg_ptr); void callbackGateMode(const autoware_control_msgs::msg::GateMode::ConstSharedPtr msg_ptr); - void callbackIsEmergency(const std_msgs::msg::Bool::ConstSharedPtr msg_ptr); + void callbackIsEmergency(const autoware_control_msgs::msg::EmergencyMode::ConstSharedPtr msg_ptr); void callbackHazardStatus( const autoware_system_msgs::msg::HazardStatusStamped::ConstSharedPtr msg_ptr); void callbackStopReason( const autoware_planning_msgs::msg::StopReasonArray::ConstSharedPtr msg_ptr); void callbackDiagnostics(const diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr msg_ptr); void callbackGlobalRpt(const pacmod_msgs::msg::GlobalRpt::ConstSharedPtr msg_ptr); - void callbackLaneChangeAvailable(const std_msgs::msg::Bool::ConstSharedPtr msg_ptr); - void callbackLaneChangeReady(const std_msgs::msg::Bool::ConstSharedPtr msg_ptr); + void callbackLaneChangeAvailable( + const autoware_planning_msgs::msg::LaneChangeStatus::ConstSharedPtr msg_ptr); + void callbackLaneChangeReady( + const autoware_planning_msgs::msg::LaneChangeStatus::ConstSharedPtr msg_ptr); void callbackLaneChangeCandidatePath( const autoware_planning_msgs::msg::Path::ConstSharedPtr msg_ptr); - void callbackLaneObstacleAvoidReady(const std_msgs::msg::Bool::ConstSharedPtr msg_ptr); + void callbackLaneObstacleAvoidReady( + const autoware_planning_msgs::msg::IsAvoidancePossible::ConstSharedPtr msg_ptr); void callbackLaneObstacleAvoidCandidatePath( const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr msg_ptr); - void callbackMaxVelocity(const std_msgs::msg::Float32::ConstSharedPtr msg_ptr); - void callbackTemporaryStop(const std_msgs::msg::Bool::ConstSharedPtr msg_ptr); + void callbackMaxVelocity(const autoware_api_msgs::msg::VelocityLimit::ConstSharedPtr msg_ptr); + void callbackTemporaryStop(const autoware_api_msgs::msg::StopCommand::ConstSharedPtr msg_ptr); void callbackAutowareTrajectory( const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr msg_ptr); - void callbackDoorControl(const std_msgs::msg::Bool::ConstSharedPtr msg_ptr); + void callbackDoorControl( + const autoware_api_msgs::msg::DoorControlCommand::ConstSharedPtr msg_ptr); void callbackDoorStatus(const pacmod_msgs::msg::SystemRptInt::ConstSharedPtr msg_ptr); // timer function diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_lane_change_state_publisher.hpp b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_lane_change_state_publisher.hpp index 0c9227fabc7fd..4de836c3fa569 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_lane_change_state_publisher.hpp +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_lane_change_state_publisher.hpp @@ -36,10 +36,10 @@ class AutowareIvLaneChangeStatePublisher rclcpp::Publisher::SharedPtr pub_state_; void getLaneChangeAvailableInfo( - const std_msgs::msg::Bool::ConstSharedPtr & available_ptr, + const autoware_planning_msgs::msg::LaneChangeStatus::ConstSharedPtr & available_ptr, autoware_api_msgs::msg::LaneChangeStatus * status); void getLaneChangeReadyInfo( - const std_msgs::msg::Bool::ConstSharedPtr & ready_ptr, + const autoware_planning_msgs::msg::LaneChangeStatus::ConstSharedPtr & ready_ptr, autoware_api_msgs::msg::LaneChangeStatus * status); void getCandidatePathInfo( const autoware_planning_msgs::msg::Path::ConstSharedPtr & path_ptr, diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_max_velocity_publisher.hpp b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_max_velocity_publisher.hpp index a7d6ffd68c0ee..435ae672c990e 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_max_velocity_publisher.hpp +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_max_velocity_publisher.hpp @@ -18,6 +18,7 @@ #include "rclcpp/rclcpp.hpp" #include "awapi_awiv_adapter/awapi_autoware_util.hpp" +#include "autoware_planning_msgs/msg/velocity_limit.hpp" namespace autoware_api { @@ -29,11 +30,12 @@ class AutowareIvMaxVelocityPublisher private: // publisher - rclcpp::Publisher::SharedPtr pub_state_; + rclcpp::Publisher::SharedPtr pub_state_; bool calcMaxVelocity( - const std_msgs::msg::Float32::ConstSharedPtr & max_velocity_ptr, - const std_msgs::msg::Bool::ConstSharedPtr & temporary_stop_ptr, float * max_velocity); + const autoware_api_msgs::msg::VelocityLimit::ConstSharedPtr & max_velocity_ptr, + const autoware_api_msgs::msg::StopCommand::ConstSharedPtr & temporary_stop_ptr, + float * max_velocity); double default_max_velocity_; }; diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_obstacle_avoidance_state_publisher.hpp b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_obstacle_avoidance_state_publisher.hpp index a401b5d123999..2a8493ce42a21 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_obstacle_avoidance_state_publisher.hpp +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_obstacle_avoidance_state_publisher.hpp @@ -36,7 +36,7 @@ class AutowareIvObstacleAvoidanceStatePublisher rclcpp::Publisher::SharedPtr pub_state_; void getObstacleAvoidReadyInfo( - const std_msgs::msg::Bool::ConstSharedPtr & ready_ptr, + const autoware_planning_msgs::msg::IsAvoidancePossible::ConstSharedPtr & ready_ptr, autoware_api_msgs::msg::ObstacleAvoidanceStatus * status); void getCandidatePathInfo( const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr & path_ptr, diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_pacmod_util.hpp b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_pacmod_util.hpp index 209364b79d3b7..73402992fc7de 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_pacmod_util.hpp +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_pacmod_util.hpp @@ -20,7 +20,7 @@ #include "autoware_api_msgs/msg/door_status.hpp" #include "pacmod_msgs/msg/system_cmd_int.hpp" #include "pacmod_msgs/msg/system_rpt_int.hpp" -#include "std_msgs/msg/bool.hpp" +#include "autoware_api_msgs/msg/door_control_command.hpp" namespace autoware_api { @@ -31,7 +31,8 @@ autoware_api_msgs::msg::DoorStatus getDoorStatusMsg( pacmod_msgs::msg::SystemCmdInt createClearOverrideDoorCommand( const rclcpp::Clock::SharedPtr & clock); pacmod_msgs::msg::SystemCmdInt createDoorCommand( - const rclcpp::Clock::SharedPtr & clock, const std_msgs::msg::Bool::ConstSharedPtr & msg_ptr); + const rclcpp::Clock::SharedPtr & clock, + const autoware_api_msgs::msg::DoorControlCommand::ConstSharedPtr & msg_ptr); } // namespace pacmod_util } // namespace autoware_api diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_vehicle_state_publisher.hpp b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_vehicle_state_publisher.hpp index 1060699392ebb..7a0bdf5c9668c 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_vehicle_state_publisher.hpp +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_vehicle_state_publisher.hpp @@ -54,7 +54,7 @@ class AutowareIvVehicleStatePublisher const autoware_vehicle_msgs::msg::ShiftStamped::ConstSharedPtr & gear_ptr, autoware_api_msgs::msg::AwapiVehicleStatus * status); void getBatteryInfo( - const std_msgs::msg::Float32::ConstSharedPtr & battery_ptr, + const autoware_vehicle_msgs::msg::BatteryStatus::ConstSharedPtr & battery_ptr, autoware_api_msgs::msg::AwapiVehicleStatus * status); void getGpsInfo( const sensor_msgs::msg::NavSatFix::ConstSharedPtr & nav_sat_ptr, diff --git a/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml b/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml index 089d766a239e8..d06d8fe2caf12 100644 --- a/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml +++ b/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml @@ -83,7 +83,8 @@ - + + @@ -105,6 +106,9 @@ + + + @@ -174,17 +178,17 @@ - + - + - + + + + @@ -150,58 +154,69 @@ + + + + + + + + + + @@ -213,26 +228,31 @@ + + + + + From 11fbc1f7fe00300f7c94052aa2c15a3defaf2a4b Mon Sep 17 00:00:00 2001 From: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Date: Tue, 30 Mar 2021 20:16:53 +0900 Subject: [PATCH 36/71] Format launch files (#1219) Signed-off-by: Kenji Miyake Signed-off-by: tanaka3 --- .../launch/awapi_awiv_adapter.launch.xml | 43 ++++++++++--------- 1 file changed, 22 insertions(+), 21 deletions(-) diff --git a/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml b/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml index 784e9eba0549c..c52d1b8b08c3a 100644 --- a/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml +++ b/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml @@ -53,7 +53,8 @@ - + + @@ -133,19 +134,19 @@ - + - + - + @@ -153,105 +154,105 @@ - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + From 28dd4695d9c899eee90ff9f8e404c8885b1e7917 Mon Sep 17 00:00:00 2001 From: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Date: Mon, 5 Apr 2021 11:05:54 +0900 Subject: [PATCH 37/71] Fix rolling build errors (#1225) * Add missing include files Signed-off-by: Kenji Miyake * Replace rclcpp::Duration Signed-off-by: Kenji Miyake * Use reference for exceptions Signed-off-by: Kenji Miyake * Use from_seconds Signed-off-by: Kenji Miyake Signed-off-by: tanaka3 --- .../include/awapi_awiv_adapter/awapi_awiv_adapter_core.hpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.hpp b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.hpp index 3b641ea6cc38a..d53442b873b6f 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.hpp +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.hpp @@ -18,6 +18,8 @@ #include #include "rclcpp/rclcpp.hpp" +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" #include "autoware_control_msgs/msg/emergency_mode.hpp" #include "autoware_control_msgs/msg/gate_mode.hpp" From a82572f573f1ce3026da159debe11f1e3a9d0487 Mon Sep 17 00:00:00 2001 From: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Date: Mon, 12 Apr 2021 14:59:29 +0900 Subject: [PATCH 38/71] Awapi relay component (#1237) Signed-off-by: tanaka3 --- .../launch/awapi_awiv_adapter.launch.xml | 132 +----- .../launch/awapi_relay_container.launch.py | 377 ++++++++++++++++++ 2 files changed, 382 insertions(+), 127 deletions(-) create mode 100644 awapi/awapi_awiv_adapter/launch/awapi_relay_container.launch.py diff --git a/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml b/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml index c52d1b8b08c3a..320471d582517 100644 --- a/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml +++ b/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml @@ -87,6 +87,9 @@ + + + @@ -132,133 +135,8 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + diff --git a/awapi/awapi_awiv_adapter/launch/awapi_relay_container.launch.py b/awapi/awapi_awiv_adapter/launch/awapi_relay_container.launch.py new file mode 100644 index 0000000000000..c479c23aba61d --- /dev/null +++ b/awapi/awapi_awiv_adapter/launch/awapi_relay_container.launch.py @@ -0,0 +1,377 @@ +# Copyright 2020 Tier IV, Inc. All rights reserved. +# +# 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. + +import launch +from launch.actions import DeclareLaunchArgument, OpaqueFunction, SetLaunchConfiguration +from launch.conditions import IfCondition, UnlessCondition, LaunchConfigurationNotEquals +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import ComposableNodeContainer, LoadComposableNodes +from launch_ros.descriptions import ComposableNode +from launch.substitutions import EnvironmentVariable + + +def generate_launch_description(): + relay_components = [] + + relay_components.append(ComposableNode( + package='topic_tools', + plugin='topic_tools::RelayNode', + name='route_relay', + namespace='awapi', + parameters=[{ + 'input_topic': LaunchConfiguration('input_route'), + 'output_topic': LaunchConfiguration('get_route'), + 'type': 'autoware_planning_msgs/msg/Route', + 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', + default_value='False') + }], + extra_arguments=[{ + 'use_intra_process_comms': LaunchConfiguration('use_intra_process') + }], + )) + + relay_components.append(ComposableNode( + package='topic_tools', + plugin='topic_tools::RelayNode', + name='predict_object_relay', + namespace='awapi', + parameters=[{ + 'input_topic': LaunchConfiguration('input_object'), + 'output_topic': LaunchConfiguration('get_predicted_object'), + 'type': 'autoware_perception_msgs/msg/DynamicObjectArray', + 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', + default_value='False') + }], + extra_arguments=[{ + 'use_intra_process_comms': LaunchConfiguration('use_intra_process') + }], + )) + + relay_components.append(ComposableNode( + package='topic_tools', + plugin='topic_tools::RelayNode', + name='nearest_traffic_light_relay', + namespace='awapi', + parameters=[{ + 'input_topic': LaunchConfiguration('input_nearest_traffic_light_state'), + 'output_topic': LaunchConfiguration('get_nearest_traffic_light_status'), + 'type': 'autoware_perception_msgs/msg/TrafficLightStateStamped', + 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', + default_value='False') + }], + extra_arguments=[{ + 'use_intra_process_comms': LaunchConfiguration('use_intra_process') + }], + )) + + relay_components.append(ComposableNode( + package='topic_tools', + plugin='topic_tools::RelayNode', + name='gate_mode_relay', + namespace='awapi', + parameters=[{ + 'input_topic': LaunchConfiguration('set_gate_mode'), + 'output_topic': LaunchConfiguration('output_gate_mode'), + 'type': 'autoware_control_msgs/msg/GateMode', + 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', + default_value='False') + }], + extra_arguments=[{ + 'use_intra_process_comms': LaunchConfiguration('use_intra_process') + }], + )) + + relay_components.append(ComposableNode( + package='topic_tools', + plugin='topic_tools::RelayNode', + name='emergency_stop_relay', + namespace='awapi', + parameters=[{ + 'input_topic': LaunchConfiguration('set_emergency_stop'), + 'output_topic': LaunchConfiguration('output_emergency_stop'), + 'type': 'autoware_control_msgs/msg/EmergencyMode', + 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', + default_value='False') + }], + extra_arguments=[{ + 'use_intra_process_comms': LaunchConfiguration('use_intra_process') + }], + )) + + relay_components.append(ComposableNode( + package='topic_tools', + plugin='topic_tools::RelayNode', + name='autoware_engage_relay', + namespace='awapi', + parameters=[{ + 'input_topic': LaunchConfiguration('set_engage'), + 'output_topic': LaunchConfiguration('output_autoware_engage'), + 'type': 'autoware_vehicle_msgs/msg/Engage', + 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', + default_value='False') + }], + extra_arguments=[{ + 'use_intra_process_comms': LaunchConfiguration('use_intra_process') + }], + )) + + relay_components.append(ComposableNode( + package='topic_tools', + plugin='topic_tools::RelayNode', + name='vehicle_engage_relay', + namespace='awapi', + parameters=[{ + 'input_topic': LaunchConfiguration('set_engage'), + 'output_topic': LaunchConfiguration('output_vehicle_engage'), + 'type': 'autoware_vehicle_msgs/msg/Engage', + 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', + default_value='False') + }], + extra_arguments=[{ + 'use_intra_process_comms': LaunchConfiguration('use_intra_process') + }], + )) + + relay_components.append(ComposableNode( + package='topic_tools', + plugin='topic_tools::RelayNode', + name='put_route_relay', + namespace='awapi', + parameters=[{ + 'input_topic': LaunchConfiguration('set_route'), + 'output_topic': LaunchConfiguration('output_route'), + 'type': 'autoware_planning_msgs/msg/Route', + 'durability': 'transient_local', + 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', + default_value='False') + }], + extra_arguments=[{ + 'use_intra_process_comms': LaunchConfiguration('use_intra_process') + }], + )) + + relay_components.append(ComposableNode( + package='topic_tools', + plugin='topic_tools::RelayNode', + name='put_goal_relay', + namespace='awapi', + parameters=[{ + 'input_topic': LaunchConfiguration('set_goal'), + 'output_topic': LaunchConfiguration('output_goal'), + 'type': 'geometry_msgs/msg/PoseStamped', + 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', + default_value='False') + }], + extra_arguments=[{ + 'use_intra_process_comms': LaunchConfiguration('use_intra_process') + }], + )) + + relay_components.append(ComposableNode( + package='topic_tools', + plugin='topic_tools::RelayNode', + name='lane_change_approval_relay', + namespace='awapi', + parameters=[{ + 'input_topic': LaunchConfiguration('set_lane_change_approval'), + 'output_topic': LaunchConfiguration('output_lane_change_approval'), + 'type': 'autoware_planning_msgs/msg/LaneChangeCommand', + 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', + default_value='False') + }], + extra_arguments=[{ + 'use_intra_process_comms': LaunchConfiguration('use_intra_process') + }], + )) + + relay_components.append(ComposableNode( + package='topic_tools', + plugin='topic_tools::RelayNode', + name='force_lane_change_relay', + namespace='awapi', + parameters=[{ + 'input_topic': LaunchConfiguration('set_force_lane_change'), + 'output_topic': LaunchConfiguration('output_force_lane_change'), + 'type': 'autoware_planning_msgs/msg/LaneChangeCommand', + 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', + default_value='False') + }], + extra_arguments=[{ + 'use_intra_process_comms': LaunchConfiguration('use_intra_process') + }], + )) + + relay_components.append(ComposableNode( + package='topic_tools', + plugin='topic_tools::RelayNode', + name='obstacle_avoid_approval_relay', + namespace='awapi', + parameters=[{ + 'input_topic': LaunchConfiguration('set_obstacle_avoid_approval'), + 'output_topic': LaunchConfiguration('output_obstacle_avoid_approval'), + 'type': 'autoware_planning_msgs/msg/EnableAvoidance', + 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', + default_value='False') + }], + extra_arguments=[{ + 'use_intra_process_comms': LaunchConfiguration('use_intra_process') + }], + )) + + relay_components.append(ComposableNode( + package='topic_tools', + plugin='topic_tools::RelayNode', + name='traffic_light_relay', + namespace='awapi', + parameters=[{ + 'input_topic': LaunchConfiguration('input_traffic_light_state'), + 'output_topic': LaunchConfiguration('get_traffic_light_status'), + 'type': 'autoware_perception_msgs/msg/TrafficLightStateArray', + 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', + default_value='False') + }], + extra_arguments=[{ + 'use_intra_process_comms': LaunchConfiguration('use_intra_process') + }], + )) + + relay_components.append(ComposableNode( + package='topic_tools', + plugin='topic_tools::RelayNode', + name='overwrite_traffic_light_state_relay', + namespace='awapi', + parameters=[{ + 'input_topic': LaunchConfiguration('set_overwrite_traffic_light_state'), + 'output_topic': LaunchConfiguration('output_overwrite_traffic_light_state'), + 'type': 'autoware_perception_msgs/msg/TrafficLightStateArray', + 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', + default_value='False') + }], + extra_arguments=[{ + 'use_intra_process_comms': LaunchConfiguration('use_intra_process') + }], + )) + + relay_components.append(ComposableNode( + package='topic_tools', + plugin='topic_tools::RelayNode', + name='speed_exceeded_relay', + namespace='awapi', + parameters=[{ + 'input_topic': LaunchConfiguration('input_stop_speed_exceeded'), + 'output_topic': LaunchConfiguration('get_stop_speed_exceeded'), + 'type': 'autoware_planning_msgs/msg/StopSpeedExceeded', + 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', + default_value='False') + }], + extra_arguments=[{ + 'use_intra_process_comms': LaunchConfiguration('use_intra_process') + }], + )) + + relay_components.append(ComposableNode( + package='topic_tools', + plugin='topic_tools::RelayNode', + name='crosswalk_status_relay', + namespace='awapi', + parameters=[{ + 'input_topic': LaunchConfiguration('set_crosswalk_status'), + 'output_topic': LaunchConfiguration('input_external_crosswalk_status'), + 'type': 'autoware_api_msgs/msg/CrosswalkStatus', + 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', + default_value='False') + }], + extra_arguments=[{ + 'use_intra_process_comms': LaunchConfiguration('use_intra_process') + }], + )) + + relay_components.append(ComposableNode( + package='topic_tools', + plugin='topic_tools::RelayNode', + name='intersection_status_relay', + namespace='awapi', + parameters=[{ + 'input_topic': LaunchConfiguration('set_intersection_status'), + 'output_topic': LaunchConfiguration('input_external_intersection_status'), + 'type': 'autoware_api_msgs/msg/IntersectionStatus', + 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', + default_value='False') + }], + extra_arguments=[{ + 'use_intra_process_comms': LaunchConfiguration('use_intra_process') + }], + )) + + relay_components.append(ComposableNode( + package='topic_tools', + plugin='topic_tools::RelayNode', + name='expand_stop_range_relay', + namespace='awapi', + parameters=[{ + 'input_topic': LaunchConfiguration('set_expand_stop_range'), + 'output_topic': LaunchConfiguration('input_expand_stop_range'), + 'type': 'autoware_planning_msgs/msg/ExpandStopRange', + 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', + default_value='False') + }], + extra_arguments=[{ + 'use_intra_process_comms': LaunchConfiguration('use_intra_process') + }], + )) + + relay_components.append(ComposableNode( + package='topic_tools', + plugin='topic_tools::RelayNode', + name='pose_initialization_request_relay', + namespace='awapi', + parameters=[{ + 'input_topic': LaunchConfiguration('set_pose_initialization_request'), + 'output_topic': LaunchConfiguration('input_pose_initialization_request'), + 'type': 'autoware_localization_msgs/msg/PoseInitializationRequest', + 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', + default_value='False') + }], + extra_arguments=[{ + 'use_intra_process_comms': LaunchConfiguration('use_intra_process') + }], + )) + + container = ComposableNodeContainer( + name='awapi_relay_container', + namespace='awapi', + package='rclcpp_components', + executable=LaunchConfiguration('container_executable'), + composable_node_descriptions=relay_components, + output='screen', + parameters=[{ + 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', default_value='False') + }], + ) + + set_container_executable = SetLaunchConfiguration( + 'container_executable', + 'component_container', + condition=UnlessCondition(LaunchConfiguration('use_multithread')) + ) + + set_container_mt_executable = SetLaunchConfiguration( + 'container_executable', + 'component_container_mt', + condition=IfCondition(LaunchConfiguration('use_multithread')) + ) + + return launch.LaunchDescription([set_container_executable, + set_container_mt_executable] + + [container]) From 6a712ee91d4baed99d8c76b115eca19a0c84f0c8 Mon Sep 17 00:00:00 2001 From: Kazuki Miyahara Date: Thu, 15 Apr 2021 12:42:48 +0900 Subject: [PATCH 39/71] Refine BSD license name (#1244) Signed-off-by: tanaka3 --- awapi/awapi_awiv_adapter/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/awapi/awapi_awiv_adapter/package.xml b/awapi/awapi_awiv_adapter/package.xml index c44329cbe488b..3d3216360c7e7 100644 --- a/awapi/awapi_awiv_adapter/package.xml +++ b/awapi/awapi_awiv_adapter/package.xml @@ -5,7 +5,7 @@ The awapi_awiv_adapter package Tomoya Kimura Tomoya Kimura - BSD + Apache License 2.0 ament_cmake_auto From 12cb496880adc37439eb1933195231c33cc33660 Mon Sep 17 00:00:00 2001 From: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Date: Mon, 26 Apr 2021 15:29:13 +0900 Subject: [PATCH 40/71] Remove use_sim_time for set_parameter (#1260) Signed-off-by: wep21 Signed-off-by: tanaka3 --- .../launch/awapi_awiv_adapter.launch.xml | 1 - .../launch/awapi_relay_container.launch.py | 41 ------------------- 2 files changed, 42 deletions(-) diff --git a/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml b/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml index 320471d582517..b8ab30a2ef77e 100644 --- a/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml +++ b/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml @@ -93,7 +93,6 @@ - diff --git a/awapi/awapi_awiv_adapter/launch/awapi_relay_container.launch.py b/awapi/awapi_awiv_adapter/launch/awapi_relay_container.launch.py index c479c23aba61d..0777b24bae94e 100644 --- a/awapi/awapi_awiv_adapter/launch/awapi_relay_container.launch.py +++ b/awapi/awapi_awiv_adapter/launch/awapi_relay_container.launch.py @@ -33,8 +33,6 @@ def generate_launch_description(): 'input_topic': LaunchConfiguration('input_route'), 'output_topic': LaunchConfiguration('get_route'), 'type': 'autoware_planning_msgs/msg/Route', - 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', - default_value='False') }], extra_arguments=[{ 'use_intra_process_comms': LaunchConfiguration('use_intra_process') @@ -50,8 +48,6 @@ def generate_launch_description(): 'input_topic': LaunchConfiguration('input_object'), 'output_topic': LaunchConfiguration('get_predicted_object'), 'type': 'autoware_perception_msgs/msg/DynamicObjectArray', - 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', - default_value='False') }], extra_arguments=[{ 'use_intra_process_comms': LaunchConfiguration('use_intra_process') @@ -67,8 +63,6 @@ def generate_launch_description(): 'input_topic': LaunchConfiguration('input_nearest_traffic_light_state'), 'output_topic': LaunchConfiguration('get_nearest_traffic_light_status'), 'type': 'autoware_perception_msgs/msg/TrafficLightStateStamped', - 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', - default_value='False') }], extra_arguments=[{ 'use_intra_process_comms': LaunchConfiguration('use_intra_process') @@ -84,8 +78,6 @@ def generate_launch_description(): 'input_topic': LaunchConfiguration('set_gate_mode'), 'output_topic': LaunchConfiguration('output_gate_mode'), 'type': 'autoware_control_msgs/msg/GateMode', - 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', - default_value='False') }], extra_arguments=[{ 'use_intra_process_comms': LaunchConfiguration('use_intra_process') @@ -101,8 +93,6 @@ def generate_launch_description(): 'input_topic': LaunchConfiguration('set_emergency_stop'), 'output_topic': LaunchConfiguration('output_emergency_stop'), 'type': 'autoware_control_msgs/msg/EmergencyMode', - 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', - default_value='False') }], extra_arguments=[{ 'use_intra_process_comms': LaunchConfiguration('use_intra_process') @@ -118,8 +108,6 @@ def generate_launch_description(): 'input_topic': LaunchConfiguration('set_engage'), 'output_topic': LaunchConfiguration('output_autoware_engage'), 'type': 'autoware_vehicle_msgs/msg/Engage', - 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', - default_value='False') }], extra_arguments=[{ 'use_intra_process_comms': LaunchConfiguration('use_intra_process') @@ -135,8 +123,6 @@ def generate_launch_description(): 'input_topic': LaunchConfiguration('set_engage'), 'output_topic': LaunchConfiguration('output_vehicle_engage'), 'type': 'autoware_vehicle_msgs/msg/Engage', - 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', - default_value='False') }], extra_arguments=[{ 'use_intra_process_comms': LaunchConfiguration('use_intra_process') @@ -153,8 +139,6 @@ def generate_launch_description(): 'output_topic': LaunchConfiguration('output_route'), 'type': 'autoware_planning_msgs/msg/Route', 'durability': 'transient_local', - 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', - default_value='False') }], extra_arguments=[{ 'use_intra_process_comms': LaunchConfiguration('use_intra_process') @@ -170,8 +154,6 @@ def generate_launch_description(): 'input_topic': LaunchConfiguration('set_goal'), 'output_topic': LaunchConfiguration('output_goal'), 'type': 'geometry_msgs/msg/PoseStamped', - 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', - default_value='False') }], extra_arguments=[{ 'use_intra_process_comms': LaunchConfiguration('use_intra_process') @@ -187,8 +169,6 @@ def generate_launch_description(): 'input_topic': LaunchConfiguration('set_lane_change_approval'), 'output_topic': LaunchConfiguration('output_lane_change_approval'), 'type': 'autoware_planning_msgs/msg/LaneChangeCommand', - 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', - default_value='False') }], extra_arguments=[{ 'use_intra_process_comms': LaunchConfiguration('use_intra_process') @@ -204,8 +184,6 @@ def generate_launch_description(): 'input_topic': LaunchConfiguration('set_force_lane_change'), 'output_topic': LaunchConfiguration('output_force_lane_change'), 'type': 'autoware_planning_msgs/msg/LaneChangeCommand', - 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', - default_value='False') }], extra_arguments=[{ 'use_intra_process_comms': LaunchConfiguration('use_intra_process') @@ -221,8 +199,6 @@ def generate_launch_description(): 'input_topic': LaunchConfiguration('set_obstacle_avoid_approval'), 'output_topic': LaunchConfiguration('output_obstacle_avoid_approval'), 'type': 'autoware_planning_msgs/msg/EnableAvoidance', - 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', - default_value='False') }], extra_arguments=[{ 'use_intra_process_comms': LaunchConfiguration('use_intra_process') @@ -238,8 +214,6 @@ def generate_launch_description(): 'input_topic': LaunchConfiguration('input_traffic_light_state'), 'output_topic': LaunchConfiguration('get_traffic_light_status'), 'type': 'autoware_perception_msgs/msg/TrafficLightStateArray', - 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', - default_value='False') }], extra_arguments=[{ 'use_intra_process_comms': LaunchConfiguration('use_intra_process') @@ -255,8 +229,6 @@ def generate_launch_description(): 'input_topic': LaunchConfiguration('set_overwrite_traffic_light_state'), 'output_topic': LaunchConfiguration('output_overwrite_traffic_light_state'), 'type': 'autoware_perception_msgs/msg/TrafficLightStateArray', - 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', - default_value='False') }], extra_arguments=[{ 'use_intra_process_comms': LaunchConfiguration('use_intra_process') @@ -272,8 +244,6 @@ def generate_launch_description(): 'input_topic': LaunchConfiguration('input_stop_speed_exceeded'), 'output_topic': LaunchConfiguration('get_stop_speed_exceeded'), 'type': 'autoware_planning_msgs/msg/StopSpeedExceeded', - 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', - default_value='False') }], extra_arguments=[{ 'use_intra_process_comms': LaunchConfiguration('use_intra_process') @@ -289,8 +259,6 @@ def generate_launch_description(): 'input_topic': LaunchConfiguration('set_crosswalk_status'), 'output_topic': LaunchConfiguration('input_external_crosswalk_status'), 'type': 'autoware_api_msgs/msg/CrosswalkStatus', - 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', - default_value='False') }], extra_arguments=[{ 'use_intra_process_comms': LaunchConfiguration('use_intra_process') @@ -306,8 +274,6 @@ def generate_launch_description(): 'input_topic': LaunchConfiguration('set_intersection_status'), 'output_topic': LaunchConfiguration('input_external_intersection_status'), 'type': 'autoware_api_msgs/msg/IntersectionStatus', - 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', - default_value='False') }], extra_arguments=[{ 'use_intra_process_comms': LaunchConfiguration('use_intra_process') @@ -323,8 +289,6 @@ def generate_launch_description(): 'input_topic': LaunchConfiguration('set_expand_stop_range'), 'output_topic': LaunchConfiguration('input_expand_stop_range'), 'type': 'autoware_planning_msgs/msg/ExpandStopRange', - 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', - default_value='False') }], extra_arguments=[{ 'use_intra_process_comms': LaunchConfiguration('use_intra_process') @@ -340,8 +304,6 @@ def generate_launch_description(): 'input_topic': LaunchConfiguration('set_pose_initialization_request'), 'output_topic': LaunchConfiguration('input_pose_initialization_request'), 'type': 'autoware_localization_msgs/msg/PoseInitializationRequest', - 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', - default_value='False') }], extra_arguments=[{ 'use_intra_process_comms': LaunchConfiguration('use_intra_process') @@ -355,9 +317,6 @@ def generate_launch_description(): executable=LaunchConfiguration('container_executable'), composable_node_descriptions=relay_components, output='screen', - parameters=[{ - 'use_sim_time': EnvironmentVariable(name='AW_ROS2_USE_SIM_TIME', default_value='False') - }], ) set_container_executable = SetLaunchConfiguration( From 79c36e54f92077018c5849ff5673bfa2442b7192 Mon Sep 17 00:00:00 2001 From: Keisuke Shima Date: Wed, 28 Apr 2021 19:04:27 +0900 Subject: [PATCH 41/71] Clear hazard level when ignored (#1200) (#1289) Signed-off-by: Kenji Miyake Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Signed-off-by: tanaka3 --- awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp b/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp index f5039eb55ef82..44de00ed66748 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp @@ -163,6 +163,7 @@ void AutowareIvAutowareStatePublisher::getHazardStatusInfo( // filter by state if (aw_info.autoware_state_ptr->state != autoware_system_msgs::msg::AutowareState::EMERGENCY) { + status->hazard_status.status.level = autoware_system_msgs::msg::HazardStatus::NO_FAULT; status->hazard_status.status.diagnostics_spf = {}; status->hazard_status.status.diagnostics_lf = {}; status->hazard_status.status.diagnostics_sf = {}; @@ -170,6 +171,7 @@ void AutowareIvAutowareStatePublisher::getHazardStatusInfo( // filter by control_mode if (aw_info.control_mode_ptr->data == autoware_vehicle_msgs::msg::ControlMode::MANUAL) { + status->hazard_status.status.level = autoware_system_msgs::msg::HazardStatus::NO_FAULT; status->hazard_status.status.diagnostics_spf = {}; status->hazard_status.status.diagnostics_lf = {}; status->hazard_status.status.diagnostics_sf = {}; From 5e5b7615662ba5fc2126927c3aa2c20755692180 Mon Sep 17 00:00:00 2001 From: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> Date: Tue, 11 May 2021 13:22:24 +0900 Subject: [PATCH 42/71] fix flake8 (#1306) Signed-off-by: tanaka3 --- .../launch/awapi_relay_container.launch.py | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/awapi/awapi_awiv_adapter/launch/awapi_relay_container.launch.py b/awapi/awapi_awiv_adapter/launch/awapi_relay_container.launch.py index 0777b24bae94e..676c189c14fd1 100644 --- a/awapi/awapi_awiv_adapter/launch/awapi_relay_container.launch.py +++ b/awapi/awapi_awiv_adapter/launch/awapi_relay_container.launch.py @@ -13,12 +13,11 @@ # limitations under the License. import launch -from launch.actions import DeclareLaunchArgument, OpaqueFunction, SetLaunchConfiguration -from launch.conditions import IfCondition, UnlessCondition, LaunchConfigurationNotEquals +from launch.actions import SetLaunchConfiguration +from launch.conditions import IfCondition, UnlessCondition from launch.substitutions import LaunchConfiguration -from launch_ros.actions import ComposableNodeContainer, LoadComposableNodes +from launch_ros.actions import ComposableNodeContainer from launch_ros.descriptions import ComposableNode -from launch.substitutions import EnvironmentVariable def generate_launch_description(): From 25b57c06710a390123a0d196a13fe8621a20142e Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" <43976882+isamu-takagi@users.noreply.github.com> Date: Mon, 24 May 2021 17:05:44 +0900 Subject: [PATCH 43/71] Add perception traffic light state output (#1302) * Add perception traffic light state output Signed-off-by: Takagi, Isamu * Modify message type Signed-off-by: Takagi, Isamu * Apply message type to relay Signed-off-by: Takagi, Isamu Signed-off-by: tanaka3 --- awapi/awapi_awiv_adapter/launch/awapi_relay_container.launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/awapi/awapi_awiv_adapter/launch/awapi_relay_container.launch.py b/awapi/awapi_awiv_adapter/launch/awapi_relay_container.launch.py index 676c189c14fd1..71f2b645af679 100644 --- a/awapi/awapi_awiv_adapter/launch/awapi_relay_container.launch.py +++ b/awapi/awapi_awiv_adapter/launch/awapi_relay_container.launch.py @@ -61,7 +61,7 @@ def generate_launch_description(): parameters=[{ 'input_topic': LaunchConfiguration('input_nearest_traffic_light_state'), 'output_topic': LaunchConfiguration('get_nearest_traffic_light_status'), - 'type': 'autoware_perception_msgs/msg/TrafficLightStateStamped', + 'type': 'autoware_perception_msgs/msg/LookingTrafficLightState', }], extra_arguments=[{ 'use_intra_process_comms': LaunchConfiguration('use_intra_process') From 4a1b0d19349a110970623ee0104f644c4591df59 Mon Sep 17 00:00:00 2001 From: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Date: Tue, 25 May 2021 10:35:01 +0900 Subject: [PATCH 44/71] Fix lint errors (#1378) * Fix lint errors Signed-off-by: Kenji Miyake * Fix variable names Signed-off-by: Kenji Miyake Signed-off-by: tanaka3 --- .../awapi_awiv_adapter/launch/awapi_relay_container.launch.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/awapi/awapi_awiv_adapter/launch/awapi_relay_container.launch.py b/awapi/awapi_awiv_adapter/launch/awapi_relay_container.launch.py index 71f2b645af679..6b6236676ca4b 100644 --- a/awapi/awapi_awiv_adapter/launch/awapi_relay_container.launch.py +++ b/awapi/awapi_awiv_adapter/launch/awapi_relay_container.launch.py @@ -14,7 +14,8 @@ import launch from launch.actions import SetLaunchConfiguration -from launch.conditions import IfCondition, UnlessCondition +from launch.conditions import IfCondition +from launch.conditions import UnlessCondition from launch.substitutions import LaunchConfiguration from launch_ros.actions import ComposableNodeContainer from launch_ros.descriptions import ComposableNode From 438ff5bf9927cd3401e2a9c4af0832738f54c040 Mon Sep 17 00:00:00 2001 From: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Date: Wed, 23 Jun 2021 20:55:51 +0900 Subject: [PATCH 45/71] change msg type looking traffic light state (#1494) * change msg type looking traffic light state (#1455) * change msg type looking traffic light state * fix msg name * using namespace * change function name * fix typo * add readme * Update awapi/awapi_awiv_adapter/Readme.md fix typo Co-authored-by: Yukihiro Saito Co-authored-by: Yukihiro Saito * Remove deprecated message include Signed-off-by: wep21 * Fix lint Signed-off-by: wep21 Co-authored-by: tkimura4 Co-authored-by: Yukihiro Saito Signed-off-by: tanaka3 --- awapi/awapi_awiv_adapter/Readme.md | 20 ++++++++++++++++---- 1 file changed, 16 insertions(+), 4 deletions(-) diff --git a/awapi/awapi_awiv_adapter/Readme.md b/awapi/awapi_awiv_adapter/Readme.md index ac17741bded74..f96b2839e5a7f 100644 --- a/awapi/awapi_awiv_adapter/Readme.md +++ b/awapi/awapi_awiv_adapter/Readme.md @@ -116,11 +116,23 @@ ### /awapi/traffic_light/get/nearest_traffic_light_status - get recognition result of nearest traffic light -- MessageType: autoware_perception_msgs/TrafficLightStateStamped +- MessageType: autoware_perception_msgs/LookingTrafficLightState + +| | type | name | unit | note | +| --- | :-------------------------------------------------- | :--------- | :--- | :------------------------------------------------------------ | +| | std_msgs/Header | header | | | +| | autoware_perception_msgs/TrafficLightStateWithJudge | perception | | traffic light information from autoware perception module | +| | autoware_perception_msgs/TrafficLightStateWithJudge | external | | traffic light information from external tool/module | +| | autoware_perception_msgs/TrafficLightStateWithJudge | final | | traffic light information used by the planning module finally | + + +- The contents of TrafficLightStateWithJudge.msg is following. + +| | type | name | unit | note | +| --- | :----------------------------------------- | :---- | :------------------- | :------------------------------------------------------------- | +| | autoware_perception_msgs/TrafficLightState | state | | traffic light color/arrow | +| | uint8 | judge | 0:NONE, 1:STOP, 2:GO | go/stop judgment based on the color/arrow of the traffic light | -| ✓ | type | name | unit | note | -| --- | :------------------------------------------------ | :--- | :--- | :--- | -| | autoware_perception_msgs/TrafficLightStateStamped | | | | ### /awapi/vehicle/get/door From fa1bdf66f6211eebe1a7fa14dd1e86d764f14392 Mon Sep 17 00:00:00 2001 From: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Date: Tue, 20 Jul 2021 18:01:14 +0900 Subject: [PATCH 46/71] Add markdownlint and prettier (#1661) * Add markdownlint and prettier Signed-off-by: Kenji Miyake * Ignore .param.yaml Signed-off-by: Kenji Miyake * Apply format Signed-off-by: Kenji Miyake Signed-off-by: tanaka3 --- awapi/awapi_awiv_adapter/Readme.md | 40 ++++++++++++++---------------- 1 file changed, 19 insertions(+), 21 deletions(-) diff --git a/awapi/awapi_awiv_adapter/Readme.md b/awapi/awapi_awiv_adapter/Readme.md index f96b2839e5a7f..53517c490a3a8 100644 --- a/awapi/awapi_awiv_adapter/Readme.md +++ b/awapi/awapi_awiv_adapter/Readme.md @@ -68,8 +68,8 @@ - False: not exceed the stop speed ( = "no stop line in the trajectory" or "possible to stop before the stop line" ) - MessageType: autoware_planning_msgs/StopSpeedExceedStatus -| ✓ | type | name | unit | note | -| --- | :------------ | :--- | :--- | :--- | +| ✓ | type | name | unit | note | +| --- | :------------------------------------------- | :--- | :--- | :--- | | | autoware_planning_msgs/StopSpeedExceedStatus | | - | | ### /awapi/prediction/get/objects @@ -121,11 +121,10 @@ | | type | name | unit | note | | --- | :-------------------------------------------------- | :--------- | :--- | :------------------------------------------------------------ | | | std_msgs/Header | header | | | -| | autoware_perception_msgs/TrafficLightStateWithJudge | perception | | traffic light information from autoware perception module | +| | autoware_perception_msgs/TrafficLightStateWithJudge | perception | | traffic light information from autoware perception module | | | autoware_perception_msgs/TrafficLightStateWithJudge | external | | traffic light information from external tool/module | | | autoware_perception_msgs/TrafficLightStateWithJudge | final | | traffic light information used by the planning module finally | - - The contents of TrafficLightStateWithJudge.msg is following. | | type | name | unit | note | @@ -133,7 +132,6 @@ | | autoware_perception_msgs/TrafficLightState | state | | traffic light color/arrow | | | uint8 | judge | 0:NONE, 1:STOP, 2:GO | go/stop judgment based on the color/arrow of the traffic light | - ### /awapi/vehicle/get/door - get door status @@ -155,8 +153,8 @@ - set upper velocity - MessageType: autoware_api_msgs/VelocityLimit -| ✓ | type | name | unit | note | -| --- | :--------------- | :--- | :--- | :----------- | +| ✓ | type | name | unit | note | +| --- | :------------------------------ | :--- | :--- | :----------- | | ✓ | autoware_api_msgs/VelocityLimit | | | max velocity | ### /awapi/vehicle/put/stop @@ -170,8 +168,8 @@ - (if upper velocity have never received, send _default_max_velocity_ value.) - _default_max_velocity_ refers to the param: _/planning/scenario_planning/motion_velocity_optimizer/max_velocity_ - | ✓ | type | name | unit | note | - | --- | :------------ | :--- | :--- | :--- | + | ✓ | type | name | unit | note | + | --- | :---------------------------- | :--- | :--- | :--- | | ✓ | autoware_api_msgs/StopCommand | | | | ### /awapi/autoware/put/gate_mode @@ -198,8 +196,8 @@ - send engage signal (both of autoware/engage and vehicle/engage) - MessageType: autoware_vehicle_msgs/Engage -| ✓ | type | name | unit | note | -| --- | :------------------------------- | :--- | :--- | :--- | +| ✓ | type | name | unit | note | +| --- | :--------------------------- | :--- | :--- | :--- | | ✓ | autoware_vehicle_msgs/Engage | | | | ### /awapi/autoware/put/goal @@ -226,8 +224,8 @@ - send True: start lane change when **lane_change_ready** is true - MessageType: autoware_planning_msgs/LaneChangeCommand -| ✓ | type | name | unit | note | -| --- | :------------ | :--- | :--- | :--- | +| ✓ | type | name | unit | note | +| --- | :------------------------------------------- | :--- | :--- | :--- | | | autoware_planning_msgs/msg/LaneChangeCommand | | | | ### /awapi/lane_change/put/force @@ -236,8 +234,8 @@ - send True: start lane change when **force_lane_change_available** is true - MessageType: autoware_planning_msgs/LaneChangeCommand -| ✓ | type | name | unit | note | -| --- | :------------ | :--- | :--- | :--- | +| ✓ | type | name | unit | note | +| --- | :--------------------------------------- | :--- | :--- | :--- | | | autoware_planning_msgs/LaneChangeCommand | | | | ### /awapi/object_avoidance/put/approval @@ -245,8 +243,8 @@ - send object avoidance approval flag - MessageType: autoware_planning_msgs/EnableAvoidance -| ✓ | type | name | unit | note | -| --- | :------------ | :--- | :--- | :--- | +| ✓ | type | name | unit | note | +| --- | :------------------------------------- | :--- | :--- | :--- | | | autoware_planning_msgs/EnableAvoidance | | | | ### /awapi/object_avoidance/put/force @@ -273,8 +271,8 @@ - send True: open door - send False: close door -| ✓ | type | name | unit | note | -| --- | :------------ | :--- | :--- | :------------------------------------------ | +| ✓ | type | name | unit | note | +| --- | :---------------------------- | :--- | :--- | :------------------------------------------ | | | autoware_api_msgs/DoorCommand | | | available only for the vehicle using pacmod | ### /awapi/autoware/put/crosswalk_states @@ -304,6 +302,6 @@ - send expand range of the polygon used by obstacle stop [m] - MessageType: autoware_planning_msgs/ExpandStopRange -| ✓ | type | name | unit | note | -| --- | :------ | :--- | :--- | :--- | +| ✓ | type | name | unit | note | +| --- | :------------------------------------- | :--- | :--- | :--- | | | autoware_planning_msgs/ExpandStopRange | | | | From 619f33098693ddb708ac71d8e43a3cdba0faff16 Mon Sep 17 00:00:00 2001 From: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Date: Sun, 15 Aug 2021 07:28:48 +0900 Subject: [PATCH 47/71] Fix -Wunused-parameter (#1836) * Fix -Wunused-parameter Signed-off-by: Kenji Miyake * Fix mistake Signed-off-by: Kenji Miyake * fix spell * Fix lint issues Signed-off-by: Kenji Miyake * Ignore flake8 warnings Signed-off-by: Kenji Miyake Co-authored-by: Hiroki OTA Signed-off-by: tanaka3 --- awapi/awapi_awiv_adapter/CMakeLists.txt | 2 +- awapi/awapi_awiv_adapter/src/awapi_stop_reason_aggregator.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/awapi/awapi_awiv_adapter/CMakeLists.txt b/awapi/awapi_awiv_adapter/CMakeLists.txt index aa06b339e0a34..2edfcc59381c4 100644 --- a/awapi/awapi_awiv_adapter/CMakeLists.txt +++ b/awapi/awapi_awiv_adapter/CMakeLists.txt @@ -7,7 +7,7 @@ if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_EXTENSIONS OFF) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wno-unused-parameter -Wall -Wextra -Wpedantic -Werror) + add_compile_options(-Wall -Wextra -Wpedantic -Werror) endif() find_package(ament_cmake_auto REQUIRED) diff --git a/awapi/awapi_awiv_adapter/src/awapi_stop_reason_aggregator.cpp b/awapi/awapi_awiv_adapter/src/awapi_stop_reason_aggregator.cpp index d5871190785e6..ca9deb81240b2 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_stop_reason_aggregator.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_stop_reason_aggregator.cpp @@ -41,7 +41,7 @@ AutowareIvStopReasonAggregator::updateStopReasonArray( void AutowareIvStopReasonAggregator::applyUpdate( const autoware_planning_msgs::msg::StopReasonArray::ConstSharedPtr & msg_ptr, - const AutowareInfo & aw_info) + [[maybe_unused]] const AutowareInfo & aw_info) { /* remove old stop_reason that matches reason with received msg */ // make reason-matching msg list From 24e2461811fe55157e517466457fe8c2cd506b97 Mon Sep 17 00:00:00 2001 From: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Date: Sun, 15 Aug 2021 14:13:39 +0900 Subject: [PATCH 48/71] Fix compiler warnings (#1837) * Fix -Wunused-private-field Signed-off-by: Kenji Miyake * Fix -Wunused-variable Signed-off-by: Kenji Miyake * Fix -Wformat-security Signed-off-by: Kenji Miyake * Fix -Winvalid-constexpr Signed-off-by: Kenji Miyake * Fix -Wdelete-non-abstract-non-virtual-dtor Signed-off-by: Kenji Miyake * Fix -Wdelete-abstract-non-virtual-dtor Signed-off-by: Kenji Miyake * Fix -Winconsistent-missing-override Signed-off-by: Kenji Miyake * Fix -Wrange-loop-construct Signed-off-by: Kenji Miyake * Fix "invalid application of 'sizeof' to an incomplete type" Signed-off-by: Kenji Miyake * Ignore -Wgnu-anonymous-struct and -Wnested-anon-types Signed-off-by: Kenji Miyake * Fix lint Signed-off-by: Kenji Miyake * Ignore -Wno-deprecated-declarations in CUDA-related packages Signed-off-by: Kenji Miyake * Fix mistake Signed-off-by: Kenji Miyake * Fix -Wunused-parameter Signed-off-by: Kenji Miyake Signed-off-by: tanaka3 --- .../awapi_awiv_adapter/awapi_awiv_adapter_core.hpp | 1 - .../src/awapi_stop_reason_aggregator.cpp | 10 +++++----- 2 files changed, 5 insertions(+), 6 deletions(-) diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.hpp b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.hpp index d53442b873b6f..191e264b678f4 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.hpp +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.hpp @@ -156,7 +156,6 @@ class AutowareIvAdapter : public rclcpp::Node std::unique_ptr max_velocity_publisher_; double status_pub_hz_; double stop_reason_timeout_; - double default_max_velocity; double stop_reason_thresh_dist_; }; diff --git a/awapi/awapi_awiv_adapter/src/awapi_stop_reason_aggregator.cpp b/awapi/awapi_awiv_adapter/src/awapi_stop_reason_aggregator.cpp index ca9deb81240b2..3e63ac54c2b07 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_stop_reason_aggregator.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_stop_reason_aggregator.cpp @@ -68,8 +68,8 @@ bool AutowareIvStopReasonAggregator::checkMatchingReason( const autoware_planning_msgs::msg::StopReasonArray::ConstSharedPtr & msg_stop_reason_array, const autoware_planning_msgs::msg::StopReasonArray & stop_reason_array) { - for (const auto msg_stop_reason : msg_stop_reason_array->stop_reasons) { - for (const auto stop_reason : stop_reason_array.stop_reasons) { + for (const auto & msg_stop_reason : msg_stop_reason_array->stop_reasons) { + for (const auto & stop_reason : stop_reason_array.stop_reasons) { if (msg_stop_reason.reason == stop_reason.reason) { // find matching reason return true; @@ -140,8 +140,8 @@ AutowareIvStopReasonAggregator::makeStopReasonArray(const AutowareInfo & aw_info stop_reason_array_msg.header.stamp = clock_->now(); // input stop reason - for (const auto stop_reason_array : stop_reason_array_vec_) { - for (const auto stop_reason : stop_reason_array.stop_reasons) { + for (const auto & stop_reason_array : stop_reason_array_vec_) { + for (const auto & stop_reason : stop_reason_array.stop_reasons) { appendStopReasonToArray(stop_reason, &stop_reason_array_msg, aw_info); } } @@ -176,7 +176,7 @@ autoware_planning_msgs::msg::StopReason AutowareIvStopReasonAggregator::getNearS autoware_planning_msgs::msg::StopReason near_stop_reason; near_stop_reason.reason = stop_reason.reason; - for (const auto stop_factor : stop_reason.stop_factors) { + for (const auto & stop_factor : stop_reason.stop_factors) { if (stop_factor.dist_to_stop_pose < thresh_dist_to_stop_pose_) { // append only near stop factor near_stop_reason.stop_factors.emplace_back(stop_factor); From 9850e9ff844d33614f40a2fe915904c24285ba78 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" <43976882+isamu-takagi@users.noreply.github.com> Date: Tue, 31 Aug 2021 17:01:55 +0900 Subject: [PATCH 49/71] Add autoware api (#1979) Signed-off-by: tanaka3 --- .../launch/awapi_awiv_adapter.launch.xml | 6 ++---- .../launch/awapi_relay_container.launch.py | 15 --------------- 2 files changed, 2 insertions(+), 19 deletions(-) diff --git a/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml b/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml index b8ab30a2ef77e..72be5407a781c 100644 --- a/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml +++ b/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml @@ -43,8 +43,7 @@ - - + @@ -55,7 +54,7 @@ - + @@ -64,7 +63,6 @@ - diff --git a/awapi/awapi_awiv_adapter/launch/awapi_relay_container.launch.py b/awapi/awapi_awiv_adapter/launch/awapi_relay_container.launch.py index 6b6236676ca4b..ef363bcc74aa9 100644 --- a/awapi/awapi_awiv_adapter/launch/awapi_relay_container.launch.py +++ b/awapi/awapi_awiv_adapter/launch/awapi_relay_container.launch.py @@ -69,21 +69,6 @@ def generate_launch_description(): }], )) - relay_components.append(ComposableNode( - package='topic_tools', - plugin='topic_tools::RelayNode', - name='gate_mode_relay', - namespace='awapi', - parameters=[{ - 'input_topic': LaunchConfiguration('set_gate_mode'), - 'output_topic': LaunchConfiguration('output_gate_mode'), - 'type': 'autoware_control_msgs/msg/GateMode', - }], - extra_arguments=[{ - 'use_intra_process_comms': LaunchConfiguration('use_intra_process') - }], - )) - relay_components.append(ComposableNode( package='topic_tools', plugin='topic_tools::RelayNode', From 1935426eb58b00dc7c984a7c923709ffbe6c2ba2 Mon Sep 17 00:00:00 2001 From: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Date: Fri, 3 Sep 2021 15:15:23 +0900 Subject: [PATCH 50/71] Feature/add virtual traffic light planner (#1588) Signed-off-by: tanaka3 --- awapi/awapi_awiv_adapter/CMakeLists.txt | 3 +- .../awapi_autoware_util.hpp | 4 + .../awapi_awiv_adapter_core.hpp | 15 +++ .../awapi_v2x_aggregator.hpp | 53 +++++++++ .../launch/awapi_awiv_adapter.launch.xml | 8 ++ awapi/awapi_awiv_adapter/package.xml | 1 + .../src/awapi_awiv_adapter_core.cpp | 30 +++++ .../src/awapi_v2x_aggregator.cpp | 112 ++++++++++++++++++ 8 files changed, 225 insertions(+), 1 deletion(-) create mode 100644 awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_v2x_aggregator.hpp create mode 100644 awapi/awapi_awiv_adapter/src/awapi_v2x_aggregator.cpp diff --git a/awapi/awapi_awiv_adapter/CMakeLists.txt b/awapi/awapi_awiv_adapter/CMakeLists.txt index 2edfcc59381c4..2f6171b969440 100644 --- a/awapi/awapi_awiv_adapter/CMakeLists.txt +++ b/awapi/awapi_awiv_adapter/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.5) project(awapi_awiv_adapter) if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) endif() @@ -19,6 +19,7 @@ ament_auto_add_executable(awapi_awiv_adapter src/awapi_vehicle_state_publisher.cpp src/awapi_autoware_state_publisher.cpp src/awapi_stop_reason_aggregator.cpp + src/awapi_v2x_aggregator.cpp src/awapi_lane_change_state_publisher.cpp src/awapi_obstacle_avoidance_state_publisher.cpp src/awapi_max_velocity_publisher.cpp diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.hpp b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.hpp index 2314ee4b29d3b..96d94990f9b46 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.hpp +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.hpp @@ -39,6 +39,8 @@ #include "autoware_planning_msgs/msg/velocity_limit.hpp" #include "autoware_system_msgs/msg/autoware_state.hpp" #include "autoware_system_msgs/msg/hazard_status_stamped.hpp" +#include "autoware_v2x_msgs/msg/infrastructure_command_array.hpp" +#include "autoware_v2x_msgs/msg/virtual_traffic_light_state_array.hpp" #include "autoware_vehicle_msgs/msg/battery_status.hpp" #include "autoware_vehicle_msgs/msg/control_mode.hpp" #include "autoware_vehicle_msgs/msg/shift_stamped.hpp" @@ -66,6 +68,8 @@ struct AutowareInfo autoware_control_msgs::msg::EmergencyMode::ConstSharedPtr is_emergency_ptr; autoware_system_msgs::msg::HazardStatusStamped::ConstSharedPtr hazard_status_ptr; autoware_planning_msgs::msg::StopReasonArray::ConstSharedPtr stop_reason_ptr; + autoware_v2x_msgs::msg::InfrastructureCommandArray::ConstSharedPtr v2x_command_ptr; + autoware_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr v2x_state_ptr; diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr diagnostic_ptr; pacmod_msgs::msg::GlobalRpt::ConstSharedPtr global_rpt_ptr; autoware_planning_msgs::msg::LaneChangeStatus::ConstSharedPtr lane_change_available_ptr; diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.hpp b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.hpp index 191e264b678f4..09f8e88cf6084 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.hpp +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.hpp @@ -27,6 +27,8 @@ #include "autoware_planning_msgs/msg/stop_reason_array.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" #include "autoware_system_msgs/msg/autoware_state.hpp" +#include "autoware_v2x_msgs/msg/infrastructure_command_array.hpp" +#include "autoware_v2x_msgs/msg/virtual_traffic_light_state_array.hpp" #include "autoware_vehicle_msgs/msg/control_mode.hpp" #include "autoware_vehicle_msgs/msg/shift_stamped.hpp" #include "autoware_vehicle_msgs/msg/steering.hpp" @@ -44,6 +46,7 @@ #include "awapi_awiv_adapter/awapi_obstacle_avoidance_state_publisher.hpp" #include "awapi_awiv_adapter/awapi_pacmod_util.hpp" #include "awapi_awiv_adapter/awapi_stop_reason_aggregator.hpp" +#include "awapi_awiv_adapter/awapi_v2x_aggregator.hpp" #include "awapi_awiv_adapter/awapi_vehicle_state_publisher.hpp" namespace autoware_api @@ -69,6 +72,10 @@ class AutowareIvAdapter : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_hazard_status_; rclcpp::Subscription::SharedPtr sub_stop_reason_; + rclcpp::Subscription::SharedPtr + sub_v2x_command_; + rclcpp::Subscription::SharedPtr + sub_v2x_state_; rclcpp::Subscription::SharedPtr sub_diagnostics_; rclcpp::Subscription::SharedPtr sub_global_rpt_; rclcpp::Subscription::SharedPtr @@ -92,6 +99,9 @@ class AutowareIvAdapter : public rclcpp::Node // publisher rclcpp::Publisher::SharedPtr pub_door_control_; rclcpp::Publisher::SharedPtr pub_door_status_; + rclcpp::Publisher::SharedPtr pub_v2x_command_; + rclcpp::Publisher::SharedPtr + pub_v2x_state_; // timer rclcpp::TimerBase::SharedPtr timer_; @@ -117,6 +127,10 @@ class AutowareIvAdapter : public rclcpp::Node const autoware_system_msgs::msg::HazardStatusStamped::ConstSharedPtr msg_ptr); void callbackStopReason( const autoware_planning_msgs::msg::StopReasonArray::ConstSharedPtr msg_ptr); + void callbackV2XCommand( + const autoware_v2x_msgs::msg::InfrastructureCommandArray::ConstSharedPtr msg_ptr); + void callbackV2XState( + const autoware_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr msg_ptr); void callbackDiagnostics(const diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr msg_ptr); void callbackGlobalRpt(const pacmod_msgs::msg::GlobalRpt::ConstSharedPtr msg_ptr); void callbackLaneChangeAvailable( @@ -150,6 +164,7 @@ class AutowareIvAdapter : public rclcpp::Node std::unique_ptr vehicle_state_publisher_; std::unique_ptr autoware_state_publisher_; std::unique_ptr stop_reason_aggregator_; + std::unique_ptr v2x_aggregator_; std::unique_ptr lane_change_state_publisher_; std::unique_ptr obstacle_avoidance_state_publisher_; diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_v2x_aggregator.hpp b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_v2x_aggregator.hpp new file mode 100644 index 0000000000000..cc568a442ece5 --- /dev/null +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_v2x_aggregator.hpp @@ -0,0 +1,53 @@ +// Copyright 2021 Tier IV, Inc. +// +// 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. + +#ifndef AWAPI_AWIV_ADAPTER__AWAPI_V2X_AGGREGATOR_HPP_ +#define AWAPI_AWIV_ADAPTER__AWAPI_V2X_AGGREGATOR_HPP_ + +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "autoware_v2x_msgs/msg/infrastructure_command_array.hpp" +#include "autoware_v2x_msgs/msg/virtual_traffic_light_state_array.hpp" + +namespace autoware_api +{ +using Command = autoware_v2x_msgs::msg::InfrastructureCommand; +using CommandArray = autoware_v2x_msgs::msg::InfrastructureCommandArray; +using State = autoware_v2x_msgs::msg::VirtualTrafficLightState; +using StateArray = autoware_v2x_msgs::msg::VirtualTrafficLightStateArray; + +class AutowareIvV2XAggregator +{ +public: + explicit AutowareIvV2XAggregator(rclcpp::Node & node); + + CommandArray::ConstSharedPtr updateV2XCommand(const CommandArray::ConstSharedPtr & msg); + + StateArray::ConstSharedPtr updateV2XState(const StateArray::ConstSharedPtr & msg); + +private: + rclcpp::Logger logger_; + rclcpp::Clock::SharedPtr clock_; + const double max_delay_sec_{5.0}; + const double max_clock_error_sec_{300.0}; + std::map command_map_; + std::map state_map_; +}; + +} // namespace autoware_api + +#endif // AWAPI_AWIV_ADAPTER__AWAPI_V2X_AGGREGATOR_HPP_ diff --git a/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml b/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml index 72be5407a781c..fca85b87585cc 100644 --- a/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml +++ b/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml @@ -18,6 +18,8 @@ + + @@ -84,6 +86,8 @@ + + @@ -104,6 +108,8 @@ + + @@ -124,6 +130,8 @@ + + diff --git a/awapi/awapi_awiv_adapter/package.xml b/awapi/awapi_awiv_adapter/package.xml index 3d3216360c7e7..8a5194e31629f 100644 --- a/awapi/awapi_awiv_adapter/package.xml +++ b/awapi/awapi_awiv_adapter/package.xml @@ -14,6 +14,7 @@ autoware_system_msgs autoware_planning_msgs autoware_control_msgs + autoware_v2x_msgs autoware_vehicle_msgs diagnostic_msgs geometry_msgs diff --git a/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp b/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp index 609aafeb8dd1b..4f353090e1dff 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp @@ -44,6 +44,8 @@ AutowareIvAdapter::AutowareIvAdapter() std::make_unique( *this, stop_reason_timeout_, stop_reason_thresh_dist_); + v2x_aggregator_ = + std::make_unique(*this); lane_change_state_publisher_ = std::make_unique(*this); obstacle_avoidance_state_publisher_ = std::make_unique(*this); @@ -55,6 +57,10 @@ AutowareIvAdapter::AutowareIvAdapter() this->create_publisher("output/door_control", 1); pub_door_status_ = this->create_publisher("output/door_status", 1); + pub_v2x_command_ = this->create_publisher( + "output/v2x_command", 1); + pub_v2x_state_ = this->create_publisher( + "output/v2x_state", 1); // subscriber @@ -87,6 +93,10 @@ AutowareIvAdapter::AutowareIvAdapter() "input/hazard_status", 1, std::bind(&AutowareIvAdapter::callbackHazardStatus, this, _1)); sub_stop_reason_ = this->create_subscription( "input/stop_reason", 100, std::bind(&AutowareIvAdapter::callbackStopReason, this, _1)); + sub_v2x_command_ = this->create_subscription( + "input/v2x_command", 100, std::bind(&AutowareIvAdapter::callbackV2XCommand, this, _1)); + sub_v2x_state_ = this->create_subscription( + "input/v2x_state", 100, std::bind(&AutowareIvAdapter::callbackV2XState, this, _1)); sub_diagnostics_ = this->create_subscription( "input/diagnostics", 1, std::bind(&AutowareIvAdapter::callbackDiagnostics, this, _1)); sub_global_rpt_ = this->create_subscription( @@ -161,6 +171,14 @@ void AutowareIvAdapter::timerCallback() // publish pacmod door status pub_door_status_->publish(pacmod_util::getDoorStatusMsg(aw_info_.door_state_ptr)); + + // publish v2x command and state + if (aw_info_.v2x_command_ptr) { + pub_v2x_command_->publish(*aw_info_.v2x_command_ptr); + } + if (aw_info_.v2x_state_ptr) { + pub_v2x_state_->publish(*aw_info_.v2x_state_ptr); + } } void AutowareIvAdapter::callbackSteer( @@ -256,6 +274,18 @@ void AutowareIvAdapter::callbackStopReason( aw_info_.stop_reason_ptr = stop_reason_aggregator_->updateStopReasonArray(msg_ptr, aw_info_); } +void AutowareIvAdapter::callbackV2XCommand( + const autoware_v2x_msgs::msg::InfrastructureCommandArray::ConstSharedPtr msg_ptr) +{ + aw_info_.v2x_command_ptr = v2x_aggregator_->updateV2XCommand(msg_ptr); +} + +void AutowareIvAdapter::callbackV2XState( + const autoware_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr msg_ptr) +{ + aw_info_.v2x_state_ptr = v2x_aggregator_->updateV2XState(msg_ptr); +} + void AutowareIvAdapter::callbackDiagnostics( const diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr msg_ptr) { diff --git a/awapi/awapi_awiv_adapter/src/awapi_v2x_aggregator.cpp b/awapi/awapi_awiv_adapter/src/awapi_v2x_aggregator.cpp new file mode 100644 index 0000000000000..4944bf6e5bb6f --- /dev/null +++ b/awapi/awapi_awiv_adapter/src/awapi_v2x_aggregator.cpp @@ -0,0 +1,112 @@ +// Copyright 2021 Tier IV, Inc. +// +// 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. + +#include "awapi_awiv_adapter/awapi_v2x_aggregator.hpp" + +#include +#include + +namespace autoware_api +{ + +namespace +{ +std::string createKey(const Command & command) +{ + return command.type + "-" + command.id; +} + +std::string createKey(const State & state) +{ + return state.type + "-" + state.id; +} +} // namespace + +AutowareIvV2XAggregator::AutowareIvV2XAggregator(rclcpp::Node & node) +: logger_(node.get_logger().get_child("awapi_awiv_v2x_aggregator")), + clock_(node.get_clock()) +{ +} + +CommandArray::ConstSharedPtr AutowareIvV2XAggregator::updateV2XCommand( + const CommandArray::ConstSharedPtr & msg) +{ + // Update data + for (const auto & command : msg->commands) { + const auto key = createKey(command); + command_map_[key] = command; + } + + // Pick valid data + auto output = std::make_shared(); + output->stamp = clock_->now(); + for (const auto & [key, command] : command_map_) { + // Calculate time diff + const auto delay = (clock_->now() - command.stamp).seconds(); + + // Ignore future data considering clock's error + if (delay < -max_clock_error_sec_) { + RCLCPP_DEBUG( + logger_, "future command: delay=%f, max_clock_error=%f", delay, max_clock_error_sec_); + continue; + } + + // Ignore old data + if (delay > max_delay_sec_) { + RCLCPP_DEBUG(logger_, "old command: delay=%f, max_delay_sec=%f", delay, max_delay_sec_); + continue; + } + + output->commands.push_back(command); + } + + return output; +} + +StateArray::ConstSharedPtr AutowareIvV2XAggregator::updateV2XState( + const StateArray::ConstSharedPtr & msg) +{ + // Update data + for (const auto & state : msg->states) { + const auto key = createKey(state); + state_map_[key] = state; + } + + // Pick valid data + auto output = std::make_shared(); + output->stamp = clock_->now(); + for (const auto & [key, state] : state_map_) { + // Calculate time diff + const auto delay = (clock_->now() - state.stamp).seconds(); + + // Ignore future data considering clock's error + if (delay < -max_clock_error_sec_) { + RCLCPP_DEBUG( + logger_, "future state: delay=%f, max_clock_error=%f", delay, max_clock_error_sec_); + continue; + } + + // Ignore old data + if (delay > max_delay_sec_) { + RCLCPP_DEBUG(logger_, "old state: delay=%f, max_delay_sec=%f", delay, max_delay_sec_); + continue; + } + + output->states.push_back(state); + } + + return output; +} + +} // namespace autoware_api From 361c9e9d0361ce6802c753c78bc418f5bb8d7f0a Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" <43976882+isamu-takagi@users.noreply.github.com> Date: Tue, 7 Sep 2021 16:30:39 +0900 Subject: [PATCH 51/71] Move launch file of external_cmd_selector (#2017) * Move launch file of external_cmd_selector Signed-off-by: Takagi, Isamu * Add copyright Signed-off-by: Takagi, Isamu * Use DeclareLaunchArgument Signed-off-by: Takagi, Isamu * Fix external command api name Signed-off-by: Takagi, Isamu * Move common parameters Signed-off-by: Takagi, Isamu * Fix format Signed-off-by: Takagi, Isamu * Fix format Signed-off-by: Takagi, Isamu Signed-off-by: tanaka3 --- awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml b/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml index fca85b87585cc..67e8fb1b7b52a 100644 --- a/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml +++ b/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml @@ -45,7 +45,7 @@ - + From f48b82dc24fed5d64a6fc07c967e7031bbbdabf0 Mon Sep 17 00:00:00 2001 From: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Date: Wed, 8 Sep 2021 18:28:23 +0900 Subject: [PATCH 52/71] Use EmergencyState instead of deprecated EmergencyMode (#2030) * Use EmergencyState instead of deprecated EmergencyMode Signed-off-by: Kenji Miyake * Use stamped type Signed-off-by: Kenji Miyake Signed-off-by: tanaka3 --- awapi/awapi_awiv_adapter/Readme.md | 10 ---------- .../awapi_autoware_state_publisher.hpp | 4 ++-- .../awapi_awiv_adapter/awapi_autoware_util.hpp | 4 ++-- .../awapi_awiv_adapter_core.hpp | 7 ++++--- .../launch/awapi_awiv_adapter.launch.xml | 6 ++---- .../launch/awapi_relay_container.launch.py | 15 --------------- .../src/awapi_autoware_state_publisher.cpp | 16 ++++++++++------ .../src/awapi_awiv_adapter_core.cpp | 10 +++++----- 8 files changed, 25 insertions(+), 47 deletions(-) diff --git a/awapi/awapi_awiv_adapter/Readme.md b/awapi/awapi_awiv_adapter/Readme.md index 53517c490a3a8..ee487398f4b78 100644 --- a/awapi/awapi_awiv_adapter/Readme.md +++ b/awapi/awapi_awiv_adapter/Readme.md @@ -181,16 +181,6 @@ | --- | :----------------------------- | :--- | :--- | :--- | | | autoware_control_msgs/GateMode | | | | -### /awapi/autoware/put/emergency_stop - -- send emergency_stop signal -- MessageType: autoware_control_msgs/EmergencyMode -- **To enable this functionality, autoware have to be in the Remote Mode or set _/control/vehicle_cmd_gate/use_external_emergency_stop_ to true.** - -| ✓ | type | name | unit | note | -| --- | :---------------------------------- | :--- | :--- | :--- | -| ✓ | autoware_control_msgs/EmergencyMode | | | | - ### /awapi/autoware/put/engage - send engage signal (both of autoware/engage and vehicle/engage) diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.hpp b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.hpp index e9f16185385a4..bc47f8d2aeb6f 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.hpp +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.hpp @@ -53,8 +53,8 @@ class AutowareIvAutowareStatePublisher void getGateModeInfo( const autoware_control_msgs::msg::GateMode::ConstSharedPtr & gate_mode_ptr, autoware_api_msgs::msg::AwapiAutowareStatus * status); - void getIsEmergencyInfo( - const autoware_control_msgs::msg::EmergencyMode::ConstSharedPtr & is_emergency_ptr, + void getEmergencyStateInfo( + const autoware_system_msgs::msg::EmergencyStateStamped::ConstSharedPtr & emergency_state_ptr, autoware_api_msgs::msg::AwapiAutowareStatus * status); void getCurrentMaxVelInfo( const autoware_planning_msgs::msg::VelocityLimit::ConstSharedPtr & current_max_velocity_ptr, diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.hpp b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.hpp index 96d94990f9b46..7bb492bcaa2df 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.hpp +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.hpp @@ -29,7 +29,7 @@ #include "autoware_api_msgs/msg/velocity_limit.hpp" #include "autoware_api_msgs/msg/stop_command.hpp" -#include "autoware_control_msgs/msg/emergency_mode.hpp" +#include "autoware_system_msgs/msg/emergency_state_stamped.hpp" #include "autoware_control_msgs/msg/gate_mode.hpp" #include "autoware_planning_msgs/msg/is_avoidance_possible.hpp" #include "autoware_planning_msgs/msg/lane_change_status.hpp" @@ -65,7 +65,7 @@ struct AutowareInfo autoware_system_msgs::msg::AutowareState::ConstSharedPtr autoware_state_ptr; autoware_vehicle_msgs::msg::ControlMode::ConstSharedPtr control_mode_ptr; autoware_control_msgs::msg::GateMode::ConstSharedPtr gate_mode_ptr; - autoware_control_msgs::msg::EmergencyMode::ConstSharedPtr is_emergency_ptr; + autoware_system_msgs::msg::EmergencyStateStamped::ConstSharedPtr emergency_state_ptr; autoware_system_msgs::msg::HazardStatusStamped::ConstSharedPtr hazard_status_ptr; autoware_planning_msgs::msg::StopReasonArray::ConstSharedPtr stop_reason_ptr; autoware_v2x_msgs::msg::InfrastructureCommandArray::ConstSharedPtr v2x_command_ptr; diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.hpp b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.hpp index 09f8e88cf6084..c512c7712fd45 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.hpp +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.hpp @@ -21,7 +21,7 @@ #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" -#include "autoware_control_msgs/msg/emergency_mode.hpp" +#include "autoware_system_msgs/msg/emergency_state_stamped.hpp" #include "autoware_control_msgs/msg/gate_mode.hpp" #include "autoware_planning_msgs/msg/path.hpp" #include "autoware_planning_msgs/msg/stop_reason_array.hpp" @@ -68,7 +68,7 @@ class AutowareIvAdapter : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_autoware_state_; rclcpp::Subscription::SharedPtr sub_control_mode_; rclcpp::Subscription::SharedPtr sub_gate_mode_; - rclcpp::Subscription::SharedPtr sub_emergency_; + rclcpp::Subscription::SharedPtr sub_emergency_; rclcpp::Subscription::SharedPtr sub_hazard_status_; rclcpp::Subscription::SharedPtr sub_stop_reason_; @@ -122,7 +122,8 @@ class AutowareIvAdapter : public rclcpp::Node const autoware_system_msgs::msg::AutowareState::ConstSharedPtr msg_ptr); void callbackControlMode(const autoware_vehicle_msgs::msg::ControlMode::ConstSharedPtr msg_ptr); void callbackGateMode(const autoware_control_msgs::msg::GateMode::ConstSharedPtr msg_ptr); - void callbackIsEmergency(const autoware_control_msgs::msg::EmergencyMode::ConstSharedPtr msg_ptr); + void callbackEmergencyState( + const autoware_system_msgs::msg::EmergencyStateStamped::ConstSharedPtr msg_ptr); void callbackHazardStatus( const autoware_system_msgs::msg::HazardStatusStamped::ConstSharedPtr msg_ptr); void callbackStopReason( diff --git a/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml b/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml index 67e8fb1b7b52a..13b31f4a864a1 100644 --- a/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml +++ b/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml @@ -15,7 +15,7 @@ - + @@ -45,7 +45,6 @@ - @@ -65,7 +64,6 @@ - @@ -105,7 +103,7 @@ - + diff --git a/awapi/awapi_awiv_adapter/launch/awapi_relay_container.launch.py b/awapi/awapi_awiv_adapter/launch/awapi_relay_container.launch.py index ef363bcc74aa9..90cce78e55639 100644 --- a/awapi/awapi_awiv_adapter/launch/awapi_relay_container.launch.py +++ b/awapi/awapi_awiv_adapter/launch/awapi_relay_container.launch.py @@ -69,21 +69,6 @@ def generate_launch_description(): }], )) - relay_components.append(ComposableNode( - package='topic_tools', - plugin='topic_tools::RelayNode', - name='emergency_stop_relay', - namespace='awapi', - parameters=[{ - 'input_topic': LaunchConfiguration('set_emergency_stop'), - 'output_topic': LaunchConfiguration('output_emergency_stop'), - 'type': 'autoware_control_msgs/msg/EmergencyMode', - }], - extra_arguments=[{ - 'use_intra_process_comms': LaunchConfiguration('use_intra_process') - }], - )) - relay_components.append(ComposableNode( package='topic_tools', plugin='topic_tools::RelayNode', diff --git a/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp b/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp index 44de00ed66748..8e2b0379e2cda 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp @@ -43,7 +43,7 @@ void AutowareIvAutowareStatePublisher::statePublisher(const AutowareInfo & aw_in getAutowareStateInfo(aw_info.autoware_state_ptr, &status); getControlModeInfo(aw_info.control_mode_ptr, &status); getGateModeInfo(aw_info.gate_mode_ptr, &status); - getIsEmergencyInfo(aw_info.is_emergency_ptr, &status); + getEmergencyStateInfo(aw_info.emergency_state_ptr, &status); getCurrentMaxVelInfo(aw_info.current_max_velocity_ptr, &status); getHazardStatusInfo(aw_info, &status); getStopReasonInfo(aw_info.stop_reason_ptr, &status); @@ -96,17 +96,21 @@ void AutowareIvAutowareStatePublisher::getGateModeInfo( status->gate_mode = gate_mode_ptr->data; } -void AutowareIvAutowareStatePublisher::getIsEmergencyInfo( - const autoware_control_msgs::msg::EmergencyMode::ConstSharedPtr & is_emergency_ptr, +void AutowareIvAutowareStatePublisher::getEmergencyStateInfo( + const autoware_system_msgs::msg::EmergencyStateStamped::ConstSharedPtr & emergency_state_ptr, autoware_api_msgs::msg::AwapiAutowareStatus * status) { - if (!is_emergency_ptr) { - RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *clock_, 5000 /* ms */, "is_emergency is nullptr"); + if (!emergency_state_ptr) { + RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *clock_, 5000 /* ms */, "emergency_state is nullptr"); return; } // get emergency - status->emergency_stopped = is_emergency_ptr->is_emergency; + using autoware_system_msgs::msg::EmergencyState; + status->emergency_stopped = + (emergency_state_ptr->state.state == EmergencyState::MRM_OPERATING) || + (emergency_state_ptr->state.state == EmergencyState::MRM_SUCCEEDED) || + (emergency_state_ptr->state.state == EmergencyState::MRM_FAILED); } void AutowareIvAutowareStatePublisher::getCurrentMaxVelInfo( diff --git a/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp b/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp index 4f353090e1dff..165ea308516cb 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp @@ -86,8 +86,8 @@ AutowareIvAdapter::AutowareIvAdapter() "input/control_mode", 1, std::bind(&AutowareIvAdapter::callbackControlMode, this, _1)); sub_gate_mode_ = this->create_subscription( "input/gate_mode", durable_qos, std::bind(&AutowareIvAdapter::callbackGateMode, this, _1)); - sub_emergency_ = this->create_subscription( - "input/is_emergency", 1, std::bind(&AutowareIvAdapter::callbackIsEmergency, this, _1)); + sub_emergency_ = this->create_subscription( + "input/emergency_state", 1, std::bind(&AutowareIvAdapter::callbackEmergencyState, this, _1)); sub_hazard_status_ = this->create_subscription( "input/hazard_status", 1, std::bind(&AutowareIvAdapter::callbackHazardStatus, this, _1)); @@ -256,10 +256,10 @@ void AutowareIvAdapter::callbackGateMode( aw_info_.gate_mode_ptr = msg_ptr; } -void AutowareIvAdapter::callbackIsEmergency( - const autoware_control_msgs::msg::EmergencyMode::ConstSharedPtr msg_ptr) +void AutowareIvAdapter::callbackEmergencyState( + const autoware_system_msgs::msg::EmergencyStateStamped::ConstSharedPtr msg_ptr) { - aw_info_.is_emergency_ptr = msg_ptr; + aw_info_.emergency_state_ptr = msg_ptr; } void AutowareIvAdapter::callbackHazardStatus( From 845bfb7a24dc1c4e8de224d9f1be02faeee1319b Mon Sep 17 00:00:00 2001 From: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com> Date: Thu, 9 Sep 2021 18:44:51 +0900 Subject: [PATCH 53/71] add sort-package-xml hook in pre-commit (#1881) * add sort xml hook in pre-commit * change retval to exit_status * rename * add prettier plugin-xml * use early return * add license note * add tier4 license * restore prettier * change license order * move local hooks to public repo * move prettier-xml to pre-commit-hooks-ros * update version for bug-fix * apply pre-commit Signed-off-by: tanaka3 --- awapi/awapi_awiv_adapter/package.xml | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/awapi/awapi_awiv_adapter/package.xml b/awapi/awapi_awiv_adapter/package.xml index 8a5194e31629f..1488089ab5386 100644 --- a/awapi/awapi_awiv_adapter/package.xml +++ b/awapi/awapi_awiv_adapter/package.xml @@ -9,18 +9,18 @@ ament_cmake_auto - rclcpp autoware_api_msgs - autoware_system_msgs - autoware_planning_msgs autoware_control_msgs + autoware_planning_msgs + autoware_system_msgs autoware_v2x_msgs autoware_vehicle_msgs diagnostic_msgs geometry_msgs pacmod_msgs - std_msgs + rclcpp sensor_msgs + std_msgs tf2 tf2_geometry_msgs From 156c0a658bc81158ecc0ca5c63ccce2fbc96232c Mon Sep 17 00:00:00 2001 From: k-obitsu <56008637+k-obitsu@users.noreply.github.com> Date: Wed, 15 Sep 2021 13:20:35 +0900 Subject: [PATCH 54/71] fix awapi_autoware_state_publisher (#2085) Signed-off-by: tanaka3 --- .../src/awapi_autoware_state_publisher.cpp | 28 ------------------- 1 file changed, 28 deletions(-) diff --git a/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp b/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp index 8e2b0379e2cda..37e93e588ef10 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp @@ -164,22 +164,6 @@ void AutowareIvAutowareStatePublisher::getHazardStatusInfo( diagnostics_filter::extractLeafDiagnostics(status->hazard_status.status.diagnostics_sf); status->hazard_status.status.diagnostics_nf = diagnostics_filter::extractLeafDiagnostics(status->hazard_status.status.diagnostics_nf); - - // filter by state - if (aw_info.autoware_state_ptr->state != autoware_system_msgs::msg::AutowareState::EMERGENCY) { - status->hazard_status.status.level = autoware_system_msgs::msg::HazardStatus::NO_FAULT; - status->hazard_status.status.diagnostics_spf = {}; - status->hazard_status.status.diagnostics_lf = {}; - status->hazard_status.status.diagnostics_sf = {}; - } - - // filter by control_mode - if (aw_info.control_mode_ptr->data == autoware_vehicle_msgs::msg::ControlMode::MANUAL) { - status->hazard_status.status.level = autoware_system_msgs::msg::HazardStatus::NO_FAULT; - status->hazard_status.status.diagnostics_spf = {}; - status->hazard_status.status.diagnostics_lf = {}; - status->hazard_status.status.diagnostics_sf = {}; - } } void AutowareIvAutowareStatePublisher::getStopReasonInfo( @@ -244,18 +228,6 @@ void AutowareIvAutowareStatePublisher::getErrorDiagInfo( return; } - // filter by state - if (aw_info.autoware_state_ptr->state != AutowareState::EMERGENCY) { - status->error_diagnostics = {}; - return; - } - - // filter by control_mode - if (aw_info.control_mode_ptr->data == ControlMode::MANUAL) { - status->error_diagnostics = {}; - return; - } - // get diag using diagnostic_msgs::msg::DiagnosticStatus; const auto & hazard_status = aw_info.hazard_status_ptr->status; From b538e96ba762a1f7d19731159764af83ba51506d Mon Sep 17 00:00:00 2001 From: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Date: Thu, 21 Oct 2021 19:27:49 +0900 Subject: [PATCH 55/71] Feature/porting behavior path planner (#1645) * Add behavior path planner pkg with Lane Change (#1525) * add lanelet extension funcs Signed-off-by: Takamasa Horibe * add planning msgs for FOA Signed-off-by: Takamasa Horibe * add behavior_path_planner pkg Signed-off-by: Takamasa Horibe * apply clang format Signed-off-by: Takamasa Horibe * add error handling for config load failure Signed-off-by: Takamasa Horibe * replace word: foa with remote control Signed-off-by: Takamasa Horibe * add readme Signed-off-by: Takamasa Horibe * use pointer for return value of path Signed-off-by: Takamasa Horibe * fix hz Signed-off-by: Takamasa Horibe * remove debug print Signed-off-by: Takamasa Horibe * remove shide-shift & avoidance related files Signed-off-by: Takamasa Horibe * Clip path by goal * add build depend for behavior tree cpp Signed-off-by: Takamasa Horibe * temporally disable lint test in lanelet2_extension Signed-off-by: Takamasa Horibe Co-authored-by: rej55 * Add avoidance module in behavior_path_planner (#1528) * Revert "remove shide-shift & avoidance related files" This reverts commit d819ea0291fca251012e4b9ffd16de3896830aa2. * refactor findNewShiftPoint func Signed-off-by: Takamasa Horibe * remove duplicated decleration Signed-off-by: Takamasa Horibe * fix barkward length issue - add clipPathLenght func in avoidance Signed-off-by: Takamasa Horibe * refactor: - translate english - minor modification for traffic distance Signed-off-by: Takamasa Horibe * support debug marker in behavior_path_planner Signed-off-by: Takamasa Horibe * clean up side shift module Signed-off-by: Takamasa Horibe * change topic name Signed-off-by: Takamasa Horibe * remove japanese Signed-off-by: Takamasa Horibe * Update planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp Co-authored-by: Yukihiro Saito * fix typo Signed-off-by: Takamasa Horibe * remove unused var Signed-off-by: Takamasa Horibe * adress reviewer comments: - add const for variables - add comment - fix typo Signed-off-by: Takamasa Horibe * fix typo Signed-off-by: Takamasa Horibe Co-authored-by: Yukihiro Saito * Replace behavior_path utilities with autoware_utils (#1532) * replace calcDistance Signed-off-by: Takamasa Horibe * replace arange Signed-off-by: Takamasa Horibe * replave convertToEigenPt with autoware_utils::fromMsg Signed-off-by: Takamasa Horibe * replace normalizeRadian Signed-off-by: Takamasa Horibe * cosmetic change Signed-off-by: Takamasa Horibe * import #1526 into behavior path planner (#1531) Signed-off-by: Takamasa Horibe * Fix/behavior path empty path output guard (#1536) * add guard Signed-off-by: Takamasa Horibe * Update planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/src/behavior_path_planner.cpp * fix lateral jerk calculation (#1549) Signed-off-by: Takamasa Horibe * fix: error handling on exception in behavior_path_planner (#1551) Signed-off-by: Takamasa Horibe * Fix ignore too steep avoidance path (#1550) * ignore too steep path Signed-off-by: Takamasa Horibe * Update planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp * parametrize lateral jerk limit Signed-off-by: Takamasa Horibe * Update planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp Co-authored-by: tkimura4 Co-authored-by: tkimura4 * use offsetNoThrow and add error log (#1615) Signed-off-by: Takamasa Horibe * Ignore object ahead goal for avoidance (#1618) * Ignore object ahead goal for avoidance * Add flag * Fix position of definition of goal_pose * Fix arclength calculation * Fix position of definition of goal_pose * fix intersection stop line (#1636) * fix intersection stop line * fix typo * add document (#1635) Signed-off-by: Takamasa Horibe * Port behavior path planner to ros2 Signed-off-by: wep21 * Apply lint Signed-off-by: wep21 * Fix typo Signed-off-by: wep21 * Fix map qos Signed-off-by: wep21 * debug slope calculation in behavior (#1566) * update * update * revert change of autoware_utils * define getPose in behavior_path_planner * update * update * update * update * interpolate z in obstacle_avoidance_planner * update velocity controller * fix detection area and scene * Update planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/src/utilities.cpp Co-authored-by: tkimura4 * update comment in velocity controller * remove debug print * update Co-authored-by: tkimura4 * Address review: Fix config file name Signed-off-by: wep21 * pre-commit fixes Signed-off-by: wep21 * Fix redeclaring parameters Signed-off-by: wep21 * Add missing tf2 geometry function Signed-off-by: wep21 * Apply lint Signed-off-by: wep21 * Fix rclcpp Time initialization Signed-off-by: wep21 * Use now() instead of msg stamp Signed-off-by: wep21 * Use throttle output in getExpandedLanelet Signed-off-by: wep21 * Add missing const Signed-off-by: wep21 * Fix lint Signed-off-by: wep21 Co-authored-by: Takamasa Horibe Co-authored-by: rej55 Co-authored-by: Yukihiro Saito Co-authored-by: tkimura4 Co-authored-by: Takayuki Murooka Signed-off-by: tanaka3 --- .../launch/awapi_awiv_adapter.launch.xml | 10 +++ .../launch/awapi_relay_container.launch.py | 75 +++++++++++++++++++ 2 files changed, 85 insertions(+) diff --git a/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml b/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml index 13b31f4a864a1..6a5cc6a59da88 100644 --- a/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml +++ b/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml @@ -25,6 +25,9 @@ + + + @@ -51,6 +54,8 @@ + + @@ -69,6 +74,8 @@ + + @@ -86,6 +93,9 @@ + + + diff --git a/awapi/awapi_awiv_adapter/launch/awapi_relay_container.launch.py b/awapi/awapi_awiv_adapter/launch/awapi_relay_container.launch.py index 90cce78e55639..d1440f1967d2a 100644 --- a/awapi/awapi_awiv_adapter/launch/awapi_relay_container.launch.py +++ b/awapi/awapi_awiv_adapter/launch/awapi_relay_container.launch.py @@ -69,6 +69,51 @@ def generate_launch_description(): }], )) + relay_components.append(ComposableNode( + package='topic_tools', + plugin='topic_tools::RelayNode', + name='ready_module_relay', + namespace='awapi', + parameters=[{ + 'input_topic': LaunchConfiguration('input_path_change_ready'), + 'output_topic': LaunchConfiguration('get_path_change_ready'), + 'type': 'autoware_planning_msgs/msg/PathChangeModule', + }], + extra_arguments=[{ + 'use_intra_process_comms': LaunchConfiguration('use_intra_process') + }], + )) + + relay_components.append(ComposableNode( + package='topic_tools', + plugin='topic_tools::RelayNode', + name='force_available_relay', + namespace='awapi', + parameters=[{ + 'input_topic': LaunchConfiguration('input_path_change_force_available'), + 'output_topic': LaunchConfiguration('get_path_change_force_available'), + 'type': 'autoware_planning_msgs/msg/PathChangeModuleArray', + }], + extra_arguments=[{ + 'use_intra_process_comms': LaunchConfiguration('use_intra_process') + }], + )) + + relay_components.append(ComposableNode( + package='topic_tools', + plugin='topic_tools::RelayNode', + name='running_modules_relay', + namespace='awapi', + parameters=[{ + 'input_topic': LaunchConfiguration('input_path_change_running'), + 'output_topic': LaunchConfiguration('get_path_change_running'), + 'type': 'autoware_planning_msgs/msg/PathChangeModuleArray', + }], + extra_arguments=[{ + 'use_intra_process_comms': LaunchConfiguration('use_intra_process') + }], + )) + relay_components.append(ComposableNode( package='topic_tools', plugin='topic_tools::RelayNode', @@ -160,6 +205,36 @@ def generate_launch_description(): }], )) + relay_components.append(ComposableNode( + package='topic_tools', + plugin='topic_tools::RelayNode', + name='external_approval_relay', + namespace='awapi', + parameters=[{ + 'input_topic': LaunchConfiguration('set_path_change_approval'), + 'output_topic': LaunchConfiguration('output_path_change_approval'), + 'type': 'autoware_planning_msgs/msg/Approval', + }], + extra_arguments=[{ + 'use_intra_process_comms': LaunchConfiguration('use_intra_process') + }], + )) + + relay_components.append(ComposableNode( + package='topic_tools', + plugin='topic_tools::RelayNode', + name='force_approval_relay', + namespace='awapi', + parameters=[{ + 'input_topic': LaunchConfiguration('set_path_change_force'), + 'output_topic': LaunchConfiguration('output_path_change_force'), + 'type': 'autoware_planning_msgs/msg/PathChangeModule', + }], + extra_arguments=[{ + 'use_intra_process_comms': LaunchConfiguration('use_intra_process') + }], + )) + relay_components.append(ComposableNode( package='topic_tools', plugin='topic_tools::RelayNode', From f64340efa970345cad3267aa88cf5eee65b45cca Mon Sep 17 00:00:00 2001 From: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Date: Thu, 21 Oct 2021 19:27:49 +0900 Subject: [PATCH 56/71] Feature/porting motion velocity smoother (#1653) * Add motion_velocity_smoother (#1333) * Refactor node and utilities * Fix utilities * Fix utilities * Fixing... * Runs with L2 smoother * Use boost::optional * Add Linf, JerkFiltered * JerkFiltered * Fix * Fix awapi * Fix bug * Add config files * Fix bug * Fix bug and apply clang-format * Remove unused variable * Fix bug * Change C-style cast to static_cast * Add destructors * Use smart pointers for members * Add test * Fix test code * Tmp: add L2 norm of jerk in objective function * Fix external velocity limit * Fix interpolate in velocity controller and remove prevent move to close stop line * add initial velocity and acceleration to the filter function * Fix index calculation * add new marge function * handle edge case * Tmp: skip osqp_interface build test * Revert get_modified_package.sh * Fix CI * Fix version of osqp * Fix * Add design docment (JPN) * Fix linear interpolation * Refactor node and utilities * Fix utilities * Fix utilities * Fixing... * Runs with L2 smoother * Use boost::optional * Add Linf, JerkFiltered * JerkFiltered * Fix * Fix awapi * Fix bug * Add config files * Fix bug * Fix bug and apply clang-format * Remove unused variable * Fix bug * Change C-style cast to static_cast * Add destructors * Use smart pointers for members * Add test * Fix test code * Tmp: add L2 norm of jerk in objective function * Fix external velocity limit * Fix interpolate in velocity controller and remove prevent move to close stop line * add initial velocity and acceleration to the filter function * Fix index calculation * add new marge function * handle edge case * Tmp: skip osqp_interface build test * Revert get_modified_package.sh * Fix CI * Fix version of osqp * Fix * Add design docment (JPN) * Fix linear interpolation * add new sample function * Fix * Revert velocity_controller * Reset motion_velocity_optimizer * Fix parameter setting * Refactor and fix bugs * Use autoware_utils * Fix doc and bug * Fix doc * Fix external velocity limit and parametrize margin and jerk weight * Fix typo * Fix typo and remove old readme * add stop point calculation and modify objective function * Add parameter handling functions and add namespace * Rename calcClosestTrajectoryPoint to calcInterpolatedTrajectoryPoint and remove unused function * Fix applyExternalVelocityLimit and fix comments * Fix variable name * Fix variable name * Fix yaml comment * Add const * Fix interpolaion * Remove run() and change type of prev_closest_point_ * Update planning/scenario_planning/common/motion_velocity_smoother/src/smoother/smoother_base.cpp Co-authored-by: tkimura4 * fix stop point error * Fix linear interpolation and external velocity calculation * Remove debug code * Rename BaseParam * Remove unused func and fix misc * Fix package.xml, include, apply formatting * Fix external velocity limit * add debug calculation time * modify calculation time debugger * modify calculation time publisher * rescale the calculation time * Fix some problem * Update planning/scenario_planning/common/motion_velocity_smoother/launch/motion_velocity_smoother.launch Co-authored-by: tkimura4 Co-authored-by: yutaka Co-authored-by: tkimura4 * Add Resample Procedure after the optimization in motion_velocity_smoother (#1530) * do resample after the optimization * modify resample timing * add 0 at the end of the resampeld output trajectory * Manage parameters * Fix format * Fix format * Devide resample function in other file * Update default_motion_velocity_smoother.yaml * Update default_motion_velocity_smoother.yaml Co-authored-by: Fumiya Watanabe * Output debug trajectories in motion velocity smoother (#1533) * add debug trajectory * add rqt_multiplot setting files * add calculation tiem visualization * guard emtpy output (#1537) * Motion velocity smoother doc (#1563) * Fix doc * Add english doc (tmp) * Add english file * Remove unused files * Fix * Fix typo * Modify 1-size trajectory handling & warning messages (#1540) * add new guard to delete unnecessary message * change message * add warning messages * chnage message to warning * chnage to throttle * Fix jerk filter calculation (#1593) * modify jerk filter calculation * remove unnecessary code * change the way of interp. of pose in smoother (#1600) * Update smoother resampling (#1595) * add new guard to delete unnecessary message * change message * add warning messages * chnage message to warning * chnage to throttle * modify jerk filter calculation * modify jerk filter calculation * separate resampling code * add new parameter * add new parameter to dynamic reconfigure * remove unnecessary code * add sampling before optimization * add const and blocker * change access to at * fix terminal length value * add comments * clean code * add comment * Fix/insert backward on reverse trajectory (#1602) * remove unused code Signed-off-by: Takamasa Horibe * move debug code Signed-off-by: Takamasa Horibe * fix insert backward on reverse trajectory Signed-off-by: Takamasa Horibe * Porting motion velocity smoother to ros2 Signed-off-by: wep21 * Fix launch and fix warning * pre-commit fixes * Remove unused argument * Comment out unused parameter Signed-off-by: wep21 * Add distance threshold in motion_velocity_smoother (#1659) * Add distance threshold * Remove parameters and use default value * Fix for pre-commit Signed-off-by: wep21 * Support Non-zero Stop Point Acceleration (#1651) * Support non-zero stop point acceleration * modify zero velocity id search function (#1690) * modify zero velocity id search function * Fix resample function and error handling * Add comment Co-authored-by: Fumiya Watanabe * Apply lint Signed-off-by: wep21 * pre-commit fixes Signed-off-by: wep21 Co-authored-by: Fumiya Watanabe Co-authored-by: yutaka Co-authored-by: tkimura4 Co-authored-by: purewater0901 <43805014+purewater0901@users.noreply.github.com> Co-authored-by: Takamasa Horibe Co-authored-by: pre-commit Signed-off-by: tanaka3 --- awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml b/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml index 6a5cc6a59da88..f89bf9a5935b5 100644 --- a/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml +++ b/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml @@ -37,7 +37,7 @@ - + @@ -64,7 +64,7 @@ - + From b08c7dbc0130452ffb4b79230015cf6c763d06ee Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Mon, 18 Oct 2021 09:19:32 +0900 Subject: [PATCH 57/71] Fix/update max velocity topic name (#2236) Signed-off-by: tanaka3 --- awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml b/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml index f89bf9a5935b5..60bb9b52e73e4 100644 --- a/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml +++ b/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml @@ -47,7 +47,7 @@ - + From 1ac4a799f75c1301e43b38558e93f07b9e6f8ff3 Mon Sep 17 00:00:00 2001 From: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Date: Tue, 2 Nov 2021 21:49:19 +0900 Subject: [PATCH 58/71] Change formatter to clang-format and black (#2332) * Revert "Temporarily comment out pre-commit hooks" This reverts commit 748e9cdb145ce12f8b520bcbd97f5ff899fc28a3. * Replace ament_lint_common with autoware_lint_common Signed-off-by: Kenji Miyake * Remove ament_cmake_uncrustify and ament_clang_format Signed-off-by: Kenji Miyake * Apply Black Signed-off-by: Kenji Miyake * Apply clang-format Signed-off-by: Kenji Miyake * Fix build errors Signed-off-by: Kenji Miyake * Fix for cpplint * Fix include double quotes to angle brackets Signed-off-by: Kenji Miyake * Apply clang-format Signed-off-by: Kenji Miyake * Fix build errors Signed-off-by: Kenji Miyake Signed-off-by: tanaka3 --- .../awapi_autoware_state_publisher.hpp | 11 +- .../awapi_autoware_util.hpp | 70 +- .../awapi_awiv_adapter_core.hpp | 55 +- .../awapi_lane_change_state_publisher.hpp | 7 +- .../awapi_max_velocity_publisher.hpp | 7 +- ...api_obstacle_avoidance_state_publisher.hpp | 7 +- .../awapi_awiv_adapter/awapi_pacmod_util.hpp | 10 +- .../awapi_stop_reason_aggregator.hpp | 6 +- .../awapi_v2x_aggregator.hpp | 9 +- .../awapi_vehicle_state_publisher.hpp | 9 +- .../awapi_awiv_adapter/diagnostics_filter.hpp | 4 +- .../launch/awapi_relay_container.launch.py | 690 ++++++++++-------- awapi/awapi_awiv_adapter/package.xml | 2 +- .../src/awapi_autoware_state_publisher.cpp | 33 +- .../src/awapi_autoware_util.cpp | 15 +- .../src/awapi_awiv_adapter_core.cpp | 34 +- .../src/awapi_awiv_adapter_node.cpp | 6 +- .../src/awapi_lane_change_state_publisher.cpp | 10 +- .../src/awapi_max_velocity_publisher.cpp | 9 +- ...api_obstacle_avoidance_state_publisher.cpp | 3 +- .../src/awapi_pacmod_util.cpp | 13 +- .../src/awapi_stop_reason_aggregator.cpp | 10 +- .../src/awapi_v2x_aggregator.cpp | 14 +- .../src/awapi_vehicle_state_publisher.cpp | 8 +- 24 files changed, 533 insertions(+), 509 deletions(-) diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.hpp b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.hpp index bc47f8d2aeb6f..60b5389197f50 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.hpp +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.hpp @@ -15,15 +15,16 @@ #ifndef AWAPI_AWIV_ADAPTER__AWAPI_AUTOWARE_STATE_PUBLISHER_HPP_ #define AWAPI_AWIV_ADAPTER__AWAPI_AUTOWARE_STATE_PUBLISHER_HPP_ +#include "awapi_awiv_adapter/awapi_autoware_util.hpp" + +#include + +#include + #include #include #include -#include "rclcpp/rclcpp.hpp" - -#include "awapi_awiv_adapter/awapi_autoware_util.hpp" -#include "autoware_api_msgs/msg/awapi_autoware_status.hpp" - namespace autoware_api { class AutowareIvAutowareStatePublisher diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.hpp b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.hpp index 7bb492bcaa2df..0f70b9bf14a84 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.hpp +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.hpp @@ -15,41 +15,42 @@ #ifndef AWAPI_AWIV_ADAPTER__AWAPI_AUTOWARE_UTIL_HPP_ #define AWAPI_AWIV_ADAPTER__AWAPI_AUTOWARE_UTIL_HPP_ +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + #include #include -#include "geometry_msgs/msg/pose_stamped.hpp" -#include "geometry_msgs/msg/twist_stamped.hpp" -#include "pacmod_msgs/msg/global_rpt.hpp" -#include "rclcpp/rclcpp.hpp" -#include "sensor_msgs/msg/nav_sat_fix.hpp" -#include "tf2/utils.h" -#include "tf2_ros/transform_broadcaster.h" -#include "tf2_ros/transform_listener.h" - -#include "autoware_api_msgs/msg/velocity_limit.hpp" -#include "autoware_api_msgs/msg/stop_command.hpp" -#include "autoware_system_msgs/msg/emergency_state_stamped.hpp" -#include "autoware_control_msgs/msg/gate_mode.hpp" -#include "autoware_planning_msgs/msg/is_avoidance_possible.hpp" -#include "autoware_planning_msgs/msg/lane_change_status.hpp" -#include "autoware_planning_msgs/msg/path.hpp" -#include "autoware_planning_msgs/msg/stop_reason_array.hpp" -#include "autoware_planning_msgs/msg/trajectory.hpp" -#include "autoware_planning_msgs/msg/velocity_limit.hpp" -#include "autoware_system_msgs/msg/autoware_state.hpp" -#include "autoware_system_msgs/msg/hazard_status_stamped.hpp" -#include "autoware_v2x_msgs/msg/infrastructure_command_array.hpp" -#include "autoware_v2x_msgs/msg/virtual_traffic_light_state_array.hpp" -#include "autoware_vehicle_msgs/msg/battery_status.hpp" -#include "autoware_vehicle_msgs/msg/control_mode.hpp" -#include "autoware_vehicle_msgs/msg/shift_stamped.hpp" -#include "autoware_vehicle_msgs/msg/steering.hpp" -#include "autoware_vehicle_msgs/msg/turn_signal.hpp" -#include "autoware_vehicle_msgs/msg/vehicle_command.hpp" -#include "diagnostic_msgs/msg/diagnostic_array.hpp" -#include "pacmod_msgs/msg/system_rpt_int.hpp" - namespace autoware_api { struct AutowareInfo @@ -84,7 +85,7 @@ struct AutowareInfo pacmod_msgs::msg::SystemRptInt::ConstSharedPtr door_state_ptr; }; -template +template T waitForParam( rclcpp::Node * node, const std::string & remote_node_name, const std::string & param_name) { @@ -98,8 +99,7 @@ T waitForParam( return {}; } RCLCPP_INFO_THROTTLE( - node->get_logger(), *node->get_clock(), 1000 /* ms */, - "waiting for node: %s, param: %s\n", + node->get_logger(), *node->get_clock(), 1000 /* ms */, "waiting for node: %s, param: %s\n", remote_node_name.c_str(), param_name.c_str()); } diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.hpp b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.hpp index c512c7712fd45..bf8b6c475fc36 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.hpp +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.hpp @@ -15,30 +15,6 @@ #ifndef AWAPI_AWIV_ADAPTER__AWAPI_AWIV_ADAPTER_CORE_HPP_ #define AWAPI_AWIV_ADAPTER__AWAPI_AWIV_ADAPTER_CORE_HPP_ -#include - -#include "rclcpp/rclcpp.hpp" -#include "tf2_ros/buffer.h" -#include "tf2_ros/transform_listener.h" - -#include "autoware_system_msgs/msg/emergency_state_stamped.hpp" -#include "autoware_control_msgs/msg/gate_mode.hpp" -#include "autoware_planning_msgs/msg/path.hpp" -#include "autoware_planning_msgs/msg/stop_reason_array.hpp" -#include "autoware_planning_msgs/msg/trajectory.hpp" -#include "autoware_system_msgs/msg/autoware_state.hpp" -#include "autoware_v2x_msgs/msg/infrastructure_command_array.hpp" -#include "autoware_v2x_msgs/msg/virtual_traffic_light_state_array.hpp" -#include "autoware_vehicle_msgs/msg/control_mode.hpp" -#include "autoware_vehicle_msgs/msg/shift_stamped.hpp" -#include "autoware_vehicle_msgs/msg/steering.hpp" -#include "autoware_vehicle_msgs/msg/turn_signal.hpp" -#include "autoware_vehicle_msgs/msg/vehicle_command.hpp" -#include "diagnostic_msgs/msg/diagnostic_array.hpp" -#include "geometry_msgs/msg/twist_stamped.hpp" -#include "pacmod_msgs/msg/global_rpt.hpp" -#include "sensor_msgs/msg/nav_sat_fix.hpp" - #include "awapi_awiv_adapter/awapi_autoware_state_publisher.hpp" #include "awapi_awiv_adapter/awapi_autoware_util.hpp" #include "awapi_awiv_adapter/awapi_lane_change_state_publisher.hpp" @@ -49,6 +25,31 @@ #include "awapi_awiv_adapter/awapi_v2x_aggregator.hpp" #include "awapi_awiv_adapter/awapi_vehicle_state_publisher.hpp" +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include + namespace autoware_api { class AutowareIvAdapter : public rclcpp::Node @@ -91,8 +92,7 @@ class AutowareIvAdapter : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_current_max_velocity_; rclcpp::Subscription::SharedPtr sub_temporary_stop_; - rclcpp::Subscription::SharedPtr - sub_autoware_traj_; + rclcpp::Subscription::SharedPtr sub_autoware_traj_; rclcpp::Subscription::SharedPtr sub_door_control_; rclcpp::Subscription::SharedPtr sub_door_status_; @@ -167,8 +167,7 @@ class AutowareIvAdapter : public rclcpp::Node std::unique_ptr stop_reason_aggregator_; std::unique_ptr v2x_aggregator_; std::unique_ptr lane_change_state_publisher_; - std::unique_ptr - obstacle_avoidance_state_publisher_; + std::unique_ptr obstacle_avoidance_state_publisher_; std::unique_ptr max_velocity_publisher_; double status_pub_hz_; double stop_reason_timeout_; diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_lane_change_state_publisher.hpp b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_lane_change_state_publisher.hpp index 4de836c3fa569..a379218bbd1f7 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_lane_change_state_publisher.hpp +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_lane_change_state_publisher.hpp @@ -15,10 +15,11 @@ #ifndef AWAPI_AWIV_ADAPTER__AWAPI_LANE_CHANGE_STATE_PUBLISHER_HPP_ #define AWAPI_AWIV_ADAPTER__AWAPI_LANE_CHANGE_STATE_PUBLISHER_HPP_ -#include "rclcpp/rclcpp.hpp" - #include "awapi_awiv_adapter/awapi_autoware_util.hpp" -#include "autoware_api_msgs/msg/lane_change_status.hpp" + +#include + +#include namespace autoware_api { diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_max_velocity_publisher.hpp b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_max_velocity_publisher.hpp index 435ae672c990e..219c1695a48b5 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_max_velocity_publisher.hpp +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_max_velocity_publisher.hpp @@ -15,10 +15,11 @@ #ifndef AWAPI_AWIV_ADAPTER__AWAPI_MAX_VELOCITY_PUBLISHER_HPP_ #define AWAPI_AWIV_ADAPTER__AWAPI_MAX_VELOCITY_PUBLISHER_HPP_ -#include "rclcpp/rclcpp.hpp" - #include "awapi_awiv_adapter/awapi_autoware_util.hpp" -#include "autoware_planning_msgs/msg/velocity_limit.hpp" + +#include + +#include namespace autoware_api { diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_obstacle_avoidance_state_publisher.hpp b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_obstacle_avoidance_state_publisher.hpp index 2a8493ce42a21..568464c6eb4cc 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_obstacle_avoidance_state_publisher.hpp +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_obstacle_avoidance_state_publisher.hpp @@ -15,10 +15,11 @@ #ifndef AWAPI_AWIV_ADAPTER__AWAPI_OBSTACLE_AVOIDANCE_STATE_PUBLISHER_HPP_ #define AWAPI_AWIV_ADAPTER__AWAPI_OBSTACLE_AVOIDANCE_STATE_PUBLISHER_HPP_ -#include "rclcpp/rclcpp.hpp" - #include "awapi_awiv_adapter/awapi_autoware_util.hpp" -#include "autoware_api_msgs/msg/obstacle_avoidance_status.hpp" + +#include + +#include namespace autoware_api { diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_pacmod_util.hpp b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_pacmod_util.hpp index 73402992fc7de..29891f6246d36 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_pacmod_util.hpp +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_pacmod_util.hpp @@ -15,12 +15,12 @@ #ifndef AWAPI_AWIV_ADAPTER__AWAPI_PACMOD_UTIL_HPP_ #define AWAPI_AWIV_ADAPTER__AWAPI_PACMOD_UTIL_HPP_ -#include "rclcpp/rclcpp.hpp" +#include -#include "autoware_api_msgs/msg/door_status.hpp" -#include "pacmod_msgs/msg/system_cmd_int.hpp" -#include "pacmod_msgs/msg/system_rpt_int.hpp" -#include "autoware_api_msgs/msg/door_control_command.hpp" +#include +#include +#include +#include namespace autoware_api { diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_stop_reason_aggregator.hpp b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_stop_reason_aggregator.hpp index 60ae64c87bd1b..8487be93b1725 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_stop_reason_aggregator.hpp +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_stop_reason_aggregator.hpp @@ -15,11 +15,11 @@ #ifndef AWAPI_AWIV_ADAPTER__AWAPI_STOP_REASON_AGGREGATOR_HPP_ #define AWAPI_AWIV_ADAPTER__AWAPI_STOP_REASON_AGGREGATOR_HPP_ -#include +#include "awapi_awiv_adapter/awapi_autoware_util.hpp" -#include "rclcpp/rclcpp.hpp" +#include -#include "awapi_awiv_adapter/awapi_autoware_util.hpp" +#include namespace autoware_api { diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_v2x_aggregator.hpp b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_v2x_aggregator.hpp index cc568a442ece5..663b819af02bd 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_v2x_aggregator.hpp +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_v2x_aggregator.hpp @@ -15,14 +15,15 @@ #ifndef AWAPI_AWIV_ADAPTER__AWAPI_V2X_AGGREGATOR_HPP_ #define AWAPI_AWIV_ADAPTER__AWAPI_V2X_AGGREGATOR_HPP_ +#include + +#include +#include + #include #include #include -#include "rclcpp/rclcpp.hpp" -#include "autoware_v2x_msgs/msg/infrastructure_command_array.hpp" -#include "autoware_v2x_msgs/msg/virtual_traffic_light_state_array.hpp" - namespace autoware_api { using Command = autoware_v2x_msgs::msg::InfrastructureCommand; diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_vehicle_state_publisher.hpp b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_vehicle_state_publisher.hpp index 7a0bdf5c9668c..1b2ca906bfaeb 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_vehicle_state_publisher.hpp +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_vehicle_state_publisher.hpp @@ -15,12 +15,13 @@ #ifndef AWAPI_AWIV_ADAPTER__AWAPI_VEHICLE_STATE_PUBLISHER_HPP_ #define AWAPI_AWIV_ADAPTER__AWAPI_VEHICLE_STATE_PUBLISHER_HPP_ -#include +#include "awapi_awiv_adapter/awapi_autoware_util.hpp" -#include "rclcpp/rclcpp.hpp" +#include -#include "awapi_awiv_adapter/awapi_autoware_util.hpp" -#include "autoware_api_msgs/msg/awapi_vehicle_status.hpp" +#include + +#include namespace autoware_api { diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/diagnostics_filter.hpp b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/diagnostics_filter.hpp index e317321d6136b..87e84861b49ee 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/diagnostics_filter.hpp +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/diagnostics_filter.hpp @@ -15,12 +15,12 @@ #ifndef AWAPI_AWIV_ADAPTER__DIAGNOSTICS_FILTER_HPP_ #define AWAPI_AWIV_ADAPTER__DIAGNOSTICS_FILTER_HPP_ +#include + #include #include #include -#include "diagnostic_msgs/msg/diagnostic_status.hpp" - namespace diagnostics_filter { inline std::string splitStringByLastSlash(const std::string & str) diff --git a/awapi/awapi_awiv_adapter/launch/awapi_relay_container.launch.py b/awapi/awapi_awiv_adapter/launch/awapi_relay_container.launch.py index d1440f1967d2a..1819259ca2e12 100644 --- a/awapi/awapi_awiv_adapter/launch/awapi_relay_container.launch.py +++ b/awapi/awapi_awiv_adapter/launch/awapi_relay_container.launch.py @@ -24,358 +24,402 @@ def generate_launch_description(): relay_components = [] - relay_components.append(ComposableNode( - package='topic_tools', - plugin='topic_tools::RelayNode', - name='route_relay', - namespace='awapi', - parameters=[{ - 'input_topic': LaunchConfiguration('input_route'), - 'output_topic': LaunchConfiguration('get_route'), - 'type': 'autoware_planning_msgs/msg/Route', - }], - extra_arguments=[{ - 'use_intra_process_comms': LaunchConfiguration('use_intra_process') - }], - )) + relay_components.append( + ComposableNode( + package="topic_tools", + plugin="topic_tools::RelayNode", + name="route_relay", + namespace="awapi", + parameters=[ + { + "input_topic": LaunchConfiguration("input_route"), + "output_topic": LaunchConfiguration("get_route"), + "type": "autoware_planning_msgs/msg/Route", + } + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + ) - relay_components.append(ComposableNode( - package='topic_tools', - plugin='topic_tools::RelayNode', - name='predict_object_relay', - namespace='awapi', - parameters=[{ - 'input_topic': LaunchConfiguration('input_object'), - 'output_topic': LaunchConfiguration('get_predicted_object'), - 'type': 'autoware_perception_msgs/msg/DynamicObjectArray', - }], - extra_arguments=[{ - 'use_intra_process_comms': LaunchConfiguration('use_intra_process') - }], - )) + relay_components.append( + ComposableNode( + package="topic_tools", + plugin="topic_tools::RelayNode", + name="predict_object_relay", + namespace="awapi", + parameters=[ + { + "input_topic": LaunchConfiguration("input_object"), + "output_topic": LaunchConfiguration("get_predicted_object"), + "type": "autoware_perception_msgs/msg/DynamicObjectArray", + } + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + ) - relay_components.append(ComposableNode( - package='topic_tools', - plugin='topic_tools::RelayNode', - name='nearest_traffic_light_relay', - namespace='awapi', - parameters=[{ - 'input_topic': LaunchConfiguration('input_nearest_traffic_light_state'), - 'output_topic': LaunchConfiguration('get_nearest_traffic_light_status'), - 'type': 'autoware_perception_msgs/msg/LookingTrafficLightState', - }], - extra_arguments=[{ - 'use_intra_process_comms': LaunchConfiguration('use_intra_process') - }], - )) + relay_components.append( + ComposableNode( + package="topic_tools", + plugin="topic_tools::RelayNode", + name="nearest_traffic_light_relay", + namespace="awapi", + parameters=[ + { + "input_topic": LaunchConfiguration("input_nearest_traffic_light_state"), + "output_topic": LaunchConfiguration("get_nearest_traffic_light_status"), + "type": "autoware_perception_msgs/msg/LookingTrafficLightState", + } + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + ) - relay_components.append(ComposableNode( - package='topic_tools', - plugin='topic_tools::RelayNode', - name='ready_module_relay', - namespace='awapi', - parameters=[{ - 'input_topic': LaunchConfiguration('input_path_change_ready'), - 'output_topic': LaunchConfiguration('get_path_change_ready'), - 'type': 'autoware_planning_msgs/msg/PathChangeModule', - }], - extra_arguments=[{ - 'use_intra_process_comms': LaunchConfiguration('use_intra_process') - }], - )) + relay_components.append( + ComposableNode( + package="topic_tools", + plugin="topic_tools::RelayNode", + name="ready_module_relay", + namespace="awapi", + parameters=[ + { + "input_topic": LaunchConfiguration("input_path_change_ready"), + "output_topic": LaunchConfiguration("get_path_change_ready"), + "type": "autoware_planning_msgs/msg/PathChangeModule", + } + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + ) - relay_components.append(ComposableNode( - package='topic_tools', - plugin='topic_tools::RelayNode', - name='force_available_relay', - namespace='awapi', - parameters=[{ - 'input_topic': LaunchConfiguration('input_path_change_force_available'), - 'output_topic': LaunchConfiguration('get_path_change_force_available'), - 'type': 'autoware_planning_msgs/msg/PathChangeModuleArray', - }], - extra_arguments=[{ - 'use_intra_process_comms': LaunchConfiguration('use_intra_process') - }], - )) + relay_components.append( + ComposableNode( + package="topic_tools", + plugin="topic_tools::RelayNode", + name="force_available_relay", + namespace="awapi", + parameters=[ + { + "input_topic": LaunchConfiguration("input_path_change_force_available"), + "output_topic": LaunchConfiguration("get_path_change_force_available"), + "type": "autoware_planning_msgs/msg/PathChangeModuleArray", + } + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + ) - relay_components.append(ComposableNode( - package='topic_tools', - plugin='topic_tools::RelayNode', - name='running_modules_relay', - namespace='awapi', - parameters=[{ - 'input_topic': LaunchConfiguration('input_path_change_running'), - 'output_topic': LaunchConfiguration('get_path_change_running'), - 'type': 'autoware_planning_msgs/msg/PathChangeModuleArray', - }], - extra_arguments=[{ - 'use_intra_process_comms': LaunchConfiguration('use_intra_process') - }], - )) + relay_components.append( + ComposableNode( + package="topic_tools", + plugin="topic_tools::RelayNode", + name="running_modules_relay", + namespace="awapi", + parameters=[ + { + "input_topic": LaunchConfiguration("input_path_change_running"), + "output_topic": LaunchConfiguration("get_path_change_running"), + "type": "autoware_planning_msgs/msg/PathChangeModuleArray", + } + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + ) - relay_components.append(ComposableNode( - package='topic_tools', - plugin='topic_tools::RelayNode', - name='autoware_engage_relay', - namespace='awapi', - parameters=[{ - 'input_topic': LaunchConfiguration('set_engage'), - 'output_topic': LaunchConfiguration('output_autoware_engage'), - 'type': 'autoware_vehicle_msgs/msg/Engage', - }], - extra_arguments=[{ - 'use_intra_process_comms': LaunchConfiguration('use_intra_process') - }], - )) + relay_components.append( + ComposableNode( + package="topic_tools", + plugin="topic_tools::RelayNode", + name="autoware_engage_relay", + namespace="awapi", + parameters=[ + { + "input_topic": LaunchConfiguration("set_engage"), + "output_topic": LaunchConfiguration("output_autoware_engage"), + "type": "autoware_vehicle_msgs/msg/Engage", + } + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + ) - relay_components.append(ComposableNode( - package='topic_tools', - plugin='topic_tools::RelayNode', - name='vehicle_engage_relay', - namespace='awapi', - parameters=[{ - 'input_topic': LaunchConfiguration('set_engage'), - 'output_topic': LaunchConfiguration('output_vehicle_engage'), - 'type': 'autoware_vehicle_msgs/msg/Engage', - }], - extra_arguments=[{ - 'use_intra_process_comms': LaunchConfiguration('use_intra_process') - }], - )) + relay_components.append( + ComposableNode( + package="topic_tools", + plugin="topic_tools::RelayNode", + name="vehicle_engage_relay", + namespace="awapi", + parameters=[ + { + "input_topic": LaunchConfiguration("set_engage"), + "output_topic": LaunchConfiguration("output_vehicle_engage"), + "type": "autoware_vehicle_msgs/msg/Engage", + } + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + ) - relay_components.append(ComposableNode( - package='topic_tools', - plugin='topic_tools::RelayNode', - name='put_route_relay', - namespace='awapi', - parameters=[{ - 'input_topic': LaunchConfiguration('set_route'), - 'output_topic': LaunchConfiguration('output_route'), - 'type': 'autoware_planning_msgs/msg/Route', - 'durability': 'transient_local', - }], - extra_arguments=[{ - 'use_intra_process_comms': LaunchConfiguration('use_intra_process') - }], - )) + relay_components.append( + ComposableNode( + package="topic_tools", + plugin="topic_tools::RelayNode", + name="put_route_relay", + namespace="awapi", + parameters=[ + { + "input_topic": LaunchConfiguration("set_route"), + "output_topic": LaunchConfiguration("output_route"), + "type": "autoware_planning_msgs/msg/Route", + "durability": "transient_local", + } + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + ) - relay_components.append(ComposableNode( - package='topic_tools', - plugin='topic_tools::RelayNode', - name='put_goal_relay', - namespace='awapi', - parameters=[{ - 'input_topic': LaunchConfiguration('set_goal'), - 'output_topic': LaunchConfiguration('output_goal'), - 'type': 'geometry_msgs/msg/PoseStamped', - }], - extra_arguments=[{ - 'use_intra_process_comms': LaunchConfiguration('use_intra_process') - }], - )) + relay_components.append( + ComposableNode( + package="topic_tools", + plugin="topic_tools::RelayNode", + name="put_goal_relay", + namespace="awapi", + parameters=[ + { + "input_topic": LaunchConfiguration("set_goal"), + "output_topic": LaunchConfiguration("output_goal"), + "type": "geometry_msgs/msg/PoseStamped", + } + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + ) - relay_components.append(ComposableNode( - package='topic_tools', - plugin='topic_tools::RelayNode', - name='lane_change_approval_relay', - namespace='awapi', - parameters=[{ - 'input_topic': LaunchConfiguration('set_lane_change_approval'), - 'output_topic': LaunchConfiguration('output_lane_change_approval'), - 'type': 'autoware_planning_msgs/msg/LaneChangeCommand', - }], - extra_arguments=[{ - 'use_intra_process_comms': LaunchConfiguration('use_intra_process') - }], - )) + relay_components.append( + ComposableNode( + package="topic_tools", + plugin="topic_tools::RelayNode", + name="lane_change_approval_relay", + namespace="awapi", + parameters=[ + { + "input_topic": LaunchConfiguration("set_lane_change_approval"), + "output_topic": LaunchConfiguration("output_lane_change_approval"), + "type": "autoware_planning_msgs/msg/LaneChangeCommand", + } + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + ) - relay_components.append(ComposableNode( - package='topic_tools', - plugin='topic_tools::RelayNode', - name='force_lane_change_relay', - namespace='awapi', - parameters=[{ - 'input_topic': LaunchConfiguration('set_force_lane_change'), - 'output_topic': LaunchConfiguration('output_force_lane_change'), - 'type': 'autoware_planning_msgs/msg/LaneChangeCommand', - }], - extra_arguments=[{ - 'use_intra_process_comms': LaunchConfiguration('use_intra_process') - }], - )) + relay_components.append( + ComposableNode( + package="topic_tools", + plugin="topic_tools::RelayNode", + name="force_lane_change_relay", + namespace="awapi", + parameters=[ + { + "input_topic": LaunchConfiguration("set_force_lane_change"), + "output_topic": LaunchConfiguration("output_force_lane_change"), + "type": "autoware_planning_msgs/msg/LaneChangeCommand", + } + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + ) - relay_components.append(ComposableNode( - package='topic_tools', - plugin='topic_tools::RelayNode', - name='external_approval_relay', - namespace='awapi', - parameters=[{ - 'input_topic': LaunchConfiguration('set_path_change_approval'), - 'output_topic': LaunchConfiguration('output_path_change_approval'), - 'type': 'autoware_planning_msgs/msg/Approval', - }], - extra_arguments=[{ - 'use_intra_process_comms': LaunchConfiguration('use_intra_process') - }], - )) + relay_components.append( + ComposableNode( + package="topic_tools", + plugin="topic_tools::RelayNode", + name="external_approval_relay", + namespace="awapi", + parameters=[ + { + "input_topic": LaunchConfiguration("set_path_change_approval"), + "output_topic": LaunchConfiguration("output_path_change_approval"), + "type": "autoware_planning_msgs/msg/Approval", + } + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + ) - relay_components.append(ComposableNode( - package='topic_tools', - plugin='topic_tools::RelayNode', - name='force_approval_relay', - namespace='awapi', - parameters=[{ - 'input_topic': LaunchConfiguration('set_path_change_force'), - 'output_topic': LaunchConfiguration('output_path_change_force'), - 'type': 'autoware_planning_msgs/msg/PathChangeModule', - }], - extra_arguments=[{ - 'use_intra_process_comms': LaunchConfiguration('use_intra_process') - }], - )) + relay_components.append( + ComposableNode( + package="topic_tools", + plugin="topic_tools::RelayNode", + name="force_approval_relay", + namespace="awapi", + parameters=[ + { + "input_topic": LaunchConfiguration("set_path_change_force"), + "output_topic": LaunchConfiguration("output_path_change_force"), + "type": "autoware_planning_msgs/msg/PathChangeModule", + } + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + ) - relay_components.append(ComposableNode( - package='topic_tools', - plugin='topic_tools::RelayNode', - name='obstacle_avoid_approval_relay', - namespace='awapi', - parameters=[{ - 'input_topic': LaunchConfiguration('set_obstacle_avoid_approval'), - 'output_topic': LaunchConfiguration('output_obstacle_avoid_approval'), - 'type': 'autoware_planning_msgs/msg/EnableAvoidance', - }], - extra_arguments=[{ - 'use_intra_process_comms': LaunchConfiguration('use_intra_process') - }], - )) + relay_components.append( + ComposableNode( + package="topic_tools", + plugin="topic_tools::RelayNode", + name="obstacle_avoid_approval_relay", + namespace="awapi", + parameters=[ + { + "input_topic": LaunchConfiguration("set_obstacle_avoid_approval"), + "output_topic": LaunchConfiguration("output_obstacle_avoid_approval"), + "type": "autoware_planning_msgs/msg/EnableAvoidance", + } + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + ) - relay_components.append(ComposableNode( - package='topic_tools', - plugin='topic_tools::RelayNode', - name='traffic_light_relay', - namespace='awapi', - parameters=[{ - 'input_topic': LaunchConfiguration('input_traffic_light_state'), - 'output_topic': LaunchConfiguration('get_traffic_light_status'), - 'type': 'autoware_perception_msgs/msg/TrafficLightStateArray', - }], - extra_arguments=[{ - 'use_intra_process_comms': LaunchConfiguration('use_intra_process') - }], - )) + relay_components.append( + ComposableNode( + package="topic_tools", + plugin="topic_tools::RelayNode", + name="traffic_light_relay", + namespace="awapi", + parameters=[ + { + "input_topic": LaunchConfiguration("input_traffic_light_state"), + "output_topic": LaunchConfiguration("get_traffic_light_status"), + "type": "autoware_perception_msgs/msg/TrafficLightStateArray", + } + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + ) - relay_components.append(ComposableNode( - package='topic_tools', - plugin='topic_tools::RelayNode', - name='overwrite_traffic_light_state_relay', - namespace='awapi', - parameters=[{ - 'input_topic': LaunchConfiguration('set_overwrite_traffic_light_state'), - 'output_topic': LaunchConfiguration('output_overwrite_traffic_light_state'), - 'type': 'autoware_perception_msgs/msg/TrafficLightStateArray', - }], - extra_arguments=[{ - 'use_intra_process_comms': LaunchConfiguration('use_intra_process') - }], - )) + relay_components.append( + ComposableNode( + package="topic_tools", + plugin="topic_tools::RelayNode", + name="overwrite_traffic_light_state_relay", + namespace="awapi", + parameters=[ + { + "input_topic": LaunchConfiguration("set_overwrite_traffic_light_state"), + "output_topic": LaunchConfiguration("output_overwrite_traffic_light_state"), + "type": "autoware_perception_msgs/msg/TrafficLightStateArray", + } + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + ) - relay_components.append(ComposableNode( - package='topic_tools', - plugin='topic_tools::RelayNode', - name='speed_exceeded_relay', - namespace='awapi', - parameters=[{ - 'input_topic': LaunchConfiguration('input_stop_speed_exceeded'), - 'output_topic': LaunchConfiguration('get_stop_speed_exceeded'), - 'type': 'autoware_planning_msgs/msg/StopSpeedExceeded', - }], - extra_arguments=[{ - 'use_intra_process_comms': LaunchConfiguration('use_intra_process') - }], - )) + relay_components.append( + ComposableNode( + package="topic_tools", + plugin="topic_tools::RelayNode", + name="speed_exceeded_relay", + namespace="awapi", + parameters=[ + { + "input_topic": LaunchConfiguration("input_stop_speed_exceeded"), + "output_topic": LaunchConfiguration("get_stop_speed_exceeded"), + "type": "autoware_planning_msgs/msg/StopSpeedExceeded", + } + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + ) - relay_components.append(ComposableNode( - package='topic_tools', - plugin='topic_tools::RelayNode', - name='crosswalk_status_relay', - namespace='awapi', - parameters=[{ - 'input_topic': LaunchConfiguration('set_crosswalk_status'), - 'output_topic': LaunchConfiguration('input_external_crosswalk_status'), - 'type': 'autoware_api_msgs/msg/CrosswalkStatus', - }], - extra_arguments=[{ - 'use_intra_process_comms': LaunchConfiguration('use_intra_process') - }], - )) + relay_components.append( + ComposableNode( + package="topic_tools", + plugin="topic_tools::RelayNode", + name="crosswalk_status_relay", + namespace="awapi", + parameters=[ + { + "input_topic": LaunchConfiguration("set_crosswalk_status"), + "output_topic": LaunchConfiguration("input_external_crosswalk_status"), + "type": "autoware_api_msgs/msg/CrosswalkStatus", + } + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + ) - relay_components.append(ComposableNode( - package='topic_tools', - plugin='topic_tools::RelayNode', - name='intersection_status_relay', - namespace='awapi', - parameters=[{ - 'input_topic': LaunchConfiguration('set_intersection_status'), - 'output_topic': LaunchConfiguration('input_external_intersection_status'), - 'type': 'autoware_api_msgs/msg/IntersectionStatus', - }], - extra_arguments=[{ - 'use_intra_process_comms': LaunchConfiguration('use_intra_process') - }], - )) + relay_components.append( + ComposableNode( + package="topic_tools", + plugin="topic_tools::RelayNode", + name="intersection_status_relay", + namespace="awapi", + parameters=[ + { + "input_topic": LaunchConfiguration("set_intersection_status"), + "output_topic": LaunchConfiguration("input_external_intersection_status"), + "type": "autoware_api_msgs/msg/IntersectionStatus", + } + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + ) - relay_components.append(ComposableNode( - package='topic_tools', - plugin='topic_tools::RelayNode', - name='expand_stop_range_relay', - namespace='awapi', - parameters=[{ - 'input_topic': LaunchConfiguration('set_expand_stop_range'), - 'output_topic': LaunchConfiguration('input_expand_stop_range'), - 'type': 'autoware_planning_msgs/msg/ExpandStopRange', - }], - extra_arguments=[{ - 'use_intra_process_comms': LaunchConfiguration('use_intra_process') - }], - )) + relay_components.append( + ComposableNode( + package="topic_tools", + plugin="topic_tools::RelayNode", + name="expand_stop_range_relay", + namespace="awapi", + parameters=[ + { + "input_topic": LaunchConfiguration("set_expand_stop_range"), + "output_topic": LaunchConfiguration("input_expand_stop_range"), + "type": "autoware_planning_msgs/msg/ExpandStopRange", + } + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + ) - relay_components.append(ComposableNode( - package='topic_tools', - plugin='topic_tools::RelayNode', - name='pose_initialization_request_relay', - namespace='awapi', - parameters=[{ - 'input_topic': LaunchConfiguration('set_pose_initialization_request'), - 'output_topic': LaunchConfiguration('input_pose_initialization_request'), - 'type': 'autoware_localization_msgs/msg/PoseInitializationRequest', - }], - extra_arguments=[{ - 'use_intra_process_comms': LaunchConfiguration('use_intra_process') - }], - )) + relay_components.append( + ComposableNode( + package="topic_tools", + plugin="topic_tools::RelayNode", + name="pose_initialization_request_relay", + namespace="awapi", + parameters=[ + { + "input_topic": LaunchConfiguration("set_pose_initialization_request"), + "output_topic": LaunchConfiguration("input_pose_initialization_request"), + "type": "autoware_localization_msgs/msg/PoseInitializationRequest", + } + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + ) container = ComposableNodeContainer( - name='awapi_relay_container', - namespace='awapi', - package='rclcpp_components', - executable=LaunchConfiguration('container_executable'), + name="awapi_relay_container", + namespace="awapi", + package="rclcpp_components", + executable=LaunchConfiguration("container_executable"), composable_node_descriptions=relay_components, - output='screen', + output="screen", ) set_container_executable = SetLaunchConfiguration( - 'container_executable', - 'component_container', - condition=UnlessCondition(LaunchConfiguration('use_multithread')) + "container_executable", + "component_container", + condition=UnlessCondition(LaunchConfiguration("use_multithread")), ) set_container_mt_executable = SetLaunchConfiguration( - 'container_executable', - 'component_container_mt', - condition=IfCondition(LaunchConfiguration('use_multithread')) + "container_executable", + "component_container_mt", + condition=IfCondition(LaunchConfiguration("use_multithread")), ) - return launch.LaunchDescription([set_container_executable, - set_container_mt_executable] + - [container]) + return launch.LaunchDescription( + [set_container_executable, set_container_mt_executable] + [container] + ) diff --git a/awapi/awapi_awiv_adapter/package.xml b/awapi/awapi_awiv_adapter/package.xml index 1488089ab5386..86ecdbc7a4b53 100644 --- a/awapi/awapi_awiv_adapter/package.xml +++ b/awapi/awapi_awiv_adapter/package.xml @@ -28,7 +28,7 @@ topic_tools ament_lint_auto - ament_lint_common + autoware_lint_common ament_cmake diff --git a/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp b/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp index 37e93e588ef10..393b98a9f1a72 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp @@ -12,13 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "awapi_awiv_adapter/awapi_autoware_state_publisher.hpp" + +#include "awapi_awiv_adapter/diagnostics_filter.hpp" + #include #include #include -#include "awapi_awiv_adapter/awapi_autoware_state_publisher.hpp" -#include "awapi_awiv_adapter/diagnostics_filter.hpp" - namespace autoware_api { AutowareIvAutowareStatePublisher::AutowareIvAutowareStatePublisher(rclcpp::Node & node) @@ -27,8 +28,8 @@ AutowareIvAutowareStatePublisher::AutowareIvAutowareStatePublisher(rclcpp::Node arrived_goal_(false) { // publisher - pub_state_ = node.create_publisher( - "output/autoware_status", 1); + pub_state_ = + node.create_publisher("output/autoware_status", 1); } void AutowareIvAutowareStatePublisher::statePublisher(const AutowareInfo & aw_info) @@ -60,8 +61,7 @@ void AutowareIvAutowareStatePublisher::getAutowareStateInfo( autoware_api_msgs::msg::AwapiAutowareStatus * status) { if (!autoware_state_ptr) { - RCLCPP_DEBUG_STREAM_THROTTLE( - logger_, *clock_, 5000 /* ms */, "autoware_state is nullptr"); + RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *clock_, 5000 /* ms */, "autoware_state is nullptr"); return; } @@ -107,10 +107,9 @@ void AutowareIvAutowareStatePublisher::getEmergencyStateInfo( // get emergency using autoware_system_msgs::msg::EmergencyState; - status->emergency_stopped = - (emergency_state_ptr->state.state == EmergencyState::MRM_OPERATING) || - (emergency_state_ptr->state.state == EmergencyState::MRM_SUCCEEDED) || - (emergency_state_ptr->state.state == EmergencyState::MRM_FAILED); + status->emergency_stopped = (emergency_state_ptr->state.state == EmergencyState::MRM_OPERATING) || + (emergency_state_ptr->state.state == EmergencyState::MRM_SUCCEEDED) || + (emergency_state_ptr->state.state == EmergencyState::MRM_FAILED); } void AutowareIvAutowareStatePublisher::getCurrentMaxVelInfo( @@ -183,8 +182,7 @@ void AutowareIvAutowareStatePublisher::getDiagInfo( { if (!aw_info.diagnostic_ptr) { RCLCPP_DEBUG_STREAM_THROTTLE( - logger_, *clock_, 5000 /* ms */, - "[AutowareIvAutowareStatePublisher] diagnostics is nullptr"); + logger_, *clock_, 5000 /* ms */, "[AutowareIvAutowareStatePublisher] diagnostics is nullptr"); return; } @@ -216,8 +214,7 @@ void AutowareIvAutowareStatePublisher::getErrorDiagInfo( if (!aw_info.diagnostic_ptr) { RCLCPP_DEBUG_STREAM_THROTTLE( - logger_, *clock_, 5000 /* ms */, - "[AutowareIvAutowareStatePublisher] diagnostics is nullptr"); + logger_, *clock_, 5000 /* ms */, "[AutowareIvAutowareStatePublisher] diagnostics is nullptr"); return; } @@ -282,15 +279,13 @@ bool AutowareIvAutowareStatePublisher::isGoal( arrived_goal_ = true; } else if ( // NOLINT prev_state_ == autoware_system_msgs::msg::AutowareState::DRIVING && - aw_state == autoware_system_msgs::msg::AutowareState::WAITING_FOR_ROUTE) - { + aw_state == autoware_system_msgs::msg::AutowareState::WAITING_FOR_ROUTE) { arrived_goal_ = true; } if ( aw_state == autoware_system_msgs::msg::AutowareState::WAITING_FOR_ENGAGE || - aw_state == autoware_system_msgs::msg::AutowareState::DRIVING) - { + aw_state == autoware_system_msgs::msg::AutowareState::DRIVING) { // cancel goal state arrived_goal_ = false; } diff --git a/awapi/awapi_awiv_adapter/src/awapi_autoware_util.cpp b/awapi/awapi_awiv_adapter/src/awapi_autoware_util.cpp index 4710651ca06d7..9b796adbc4dfe 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_autoware_util.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_autoware_util.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #include "awapi_awiv_adapter/awapi_autoware_util.hpp" +#include + namespace autoware_api { double lowpass_filter(const double current_value, const double prev_value, const double gain) @@ -37,13 +37,17 @@ bool calcClosestIndex( const double dist = calcDist2d(getPose(traj, i).position, pose.position); /* check distance threshold */ - if (dist > dist_thr) {continue;} + if (dist > dist_thr) { + continue; + } /* check angle threshold */ double yaw_i = tf2::getYaw(getPose(traj, i).orientation); double yaw_diff = normalizeEulerAngle(yaw_pose - yaw_i); - if (std::fabs(yaw_diff) > angle_thr) {continue;} + if (std::fabs(yaw_diff) > angle_thr) { + continue; + } if (dist < dist_min) { dist_min = dist; @@ -92,8 +96,7 @@ double calcDistanceAlongTrajectory( size_t stop_idx; if ( !calcClosestIndex(trajectory, current_pose, self_idx) || - !calcClosestIndex(trajectory, target_pose, stop_idx)) - { + !calcClosestIndex(trajectory, target_pose, stop_idx)) { return std::numeric_limits::max(); } const double dist_to_stop_pose = calcArcLengthFromWayPoint(trajectory, self_idx, stop_idx); diff --git a/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp b/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp index 165ea308516cb..b9bd7d256bc7a 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "awapi_awiv_adapter/awapi_awiv_adapter_core.hpp" + #include #include #include -#include "awapi_awiv_adapter/awapi_awiv_adapter_core.hpp" - namespace autoware_api { using std::placeholders::_1; @@ -30,8 +30,7 @@ AutowareIvAdapter::AutowareIvAdapter() stop_reason_timeout_ = this->declare_parameter("stop_reason_timeout", 0.5); stop_reason_thresh_dist_ = this->declare_parameter("stop_reason_thresh_dist", 100.0); const double default_max_velocity = waitForParam( - this, declare_parameter("node/max_velocity", ""), - declare_parameter("param/max_velocity", "")); + this, declare_parameter("node/max_velocity", ""), declare_parameter("param/max_velocity", "")); const bool em_stop_param = waitForParam( this, declare_parameter("node/emergency_stop", ""), declare_parameter("param/emergency_stop", "")); @@ -40,12 +39,9 @@ AutowareIvAdapter::AutowareIvAdapter() // setup instance vehicle_state_publisher_ = std::make_unique(*this); autoware_state_publisher_ = std::make_unique(*this); - stop_reason_aggregator_ = - std::make_unique( - *this, stop_reason_timeout_, - stop_reason_thresh_dist_); - v2x_aggregator_ = - std::make_unique(*this); + stop_reason_aggregator_ = std::make_unique( + *this, stop_reason_timeout_, stop_reason_thresh_dist_); + v2x_aggregator_ = std::make_unique(*this); lane_change_state_publisher_ = std::make_unique(*this); obstacle_avoidance_state_publisher_ = std::make_unique(*this); @@ -88,8 +84,7 @@ AutowareIvAdapter::AutowareIvAdapter() "input/gate_mode", durable_qos, std::bind(&AutowareIvAdapter::callbackGateMode, this, _1)); sub_emergency_ = this->create_subscription( "input/emergency_state", 1, std::bind(&AutowareIvAdapter::callbackEmergencyState, this, _1)); - sub_hazard_status_ = - this->create_subscription( + sub_hazard_status_ = this->create_subscription( "input/hazard_status", 1, std::bind(&AutowareIvAdapter::callbackHazardStatus, this, _1)); sub_stop_reason_ = this->create_subscription( "input/stop_reason", 100, std::bind(&AutowareIvAdapter::callbackStopReason, this, _1)); @@ -103,8 +98,8 @@ AutowareIvAdapter::AutowareIvAdapter() "input/global_rpt", 1, std::bind(&AutowareIvAdapter::callbackGlobalRpt, this, _1)); sub_lane_change_available_ = this->create_subscription( - "input/lane_change_available", 1, - std::bind(&AutowareIvAdapter::callbackLaneChangeAvailable, this, _1)); + "input/lane_change_available", 1, + std::bind(&AutowareIvAdapter::callbackLaneChangeAvailable, this, _1)); sub_lane_change_ready_ = this->create_subscription( "input/lane_change_ready", 1, std::bind(&AutowareIvAdapter::callbackLaneChangeReady, this, _1)); sub_lane_change_candidate_ = this->create_subscription( @@ -112,12 +107,12 @@ AutowareIvAdapter::AutowareIvAdapter() std::bind(&AutowareIvAdapter::callbackLaneChangeCandidatePath, this, _1)); sub_obstacle_avoid_ready_ = this->create_subscription( - "input/obstacle_avoid_ready", durable_qos, - std::bind(&AutowareIvAdapter::callbackLaneObstacleAvoidReady, this, _1)); + "input/obstacle_avoid_ready", durable_qos, + std::bind(&AutowareIvAdapter::callbackLaneObstacleAvoidReady, this, _1)); sub_obstacle_avoid_candidate_ = this->create_subscription( - "input/obstacle_avoid_candidate_path", durable_qos, - std::bind(&AutowareIvAdapter::callbackLaneObstacleAvoidCandidatePath, this, _1)); + "input/obstacle_avoid_candidate_path", durable_qos, + std::bind(&AutowareIvAdapter::callbackLaneObstacleAvoidCandidatePath, this, _1)); sub_max_velocity_ = this->create_subscription( "input/max_velocity", 1, std::bind(&AutowareIvAdapter::callbackMaxVelocity, this, _1)); sub_current_max_velocity_ = this->create_subscription( @@ -125,8 +120,7 @@ AutowareIvAdapter::AutowareIvAdapter() std::bind(&AutowareIvAdapter::callbackCurrentMaxVelocity, this, _1)); sub_temporary_stop_ = this->create_subscription( "input/temporary_stop", 1, std::bind(&AutowareIvAdapter::callbackTemporaryStop, this, _1)); - sub_autoware_traj_ = - this->create_subscription( + sub_autoware_traj_ = this->create_subscription( "input/autoware_trajectory", 1, std::bind(&AutowareIvAdapter::callbackAutowareTrajectory, this, _1)); sub_door_control_ = this->create_subscription( diff --git a/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_node.cpp b/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_node.cpp index 7ea57331dfeaa..c3bc000bd2e4c 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_node.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_node.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "awapi_awiv_adapter/awapi_awiv_adapter_core.hpp" -#include "rclcpp/rclcpp.hpp" +#include -#include "awapi_awiv_adapter/awapi_awiv_adapter_core.hpp" +#include int main(int argc, char ** argv) { diff --git a/awapi/awapi_awiv_adapter/src/awapi_lane_change_state_publisher.cpp b/awapi/awapi_awiv_adapter/src/awapi_lane_change_state_publisher.cpp index 880d3a6f16905..7ea31c11150a1 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_lane_change_state_publisher.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_lane_change_state_publisher.cpp @@ -21,8 +21,8 @@ AutowareIvLaneChangeStatePublisher::AutowareIvLaneChangeStatePublisher(rclcpp::N clock_(node.get_clock()) { // publisher - pub_state_ = node.create_publisher( - "output/lane_change_status", 1); + pub_state_ = + node.create_publisher("output/lane_change_status", 1); } void AutowareIvLaneChangeStatePublisher::statePublisher(const AutowareInfo & aw_info) @@ -61,8 +61,7 @@ void AutowareIvLaneChangeStatePublisher::getLaneChangeReadyInfo( autoware_api_msgs::msg::LaneChangeStatus * status) { if (!ready_ptr) { - RCLCPP_DEBUG_STREAM_THROTTLE( - logger_, *clock_, 5000 /* ms */, "lane change ready is nullptr"); + RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *clock_, 5000 /* ms */, "lane change ready is nullptr"); return; } @@ -76,8 +75,7 @@ void AutowareIvLaneChangeStatePublisher::getCandidatePathInfo( { if (!path_ptr) { RCLCPP_DEBUG_STREAM_THROTTLE( - logger_, *clock_, 5000 /* ms */, - "lane_change_candidate_path is nullptr"); + logger_, *clock_, 5000 /* ms */, "lane_change_candidate_path is nullptr"); return; } diff --git a/awapi/awapi_awiv_adapter/src/awapi_max_velocity_publisher.cpp b/awapi/awapi_awiv_adapter/src/awapi_max_velocity_publisher.cpp index b9a64583bfd98..632f5fd846cad 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_max_velocity_publisher.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_max_velocity_publisher.cpp @@ -21,17 +21,16 @@ AutowareIvMaxVelocityPublisher::AutowareIvMaxVelocityPublisher( : default_max_velocity_(default_max_velocity) { // publisher - pub_state_ = node.create_publisher( - "output/max_velocity", 1); + pub_state_ = + node.create_publisher("output/max_velocity", 1); } void AutowareIvMaxVelocityPublisher::statePublisher(const AutowareInfo & aw_info) { autoware_planning_msgs::msg::VelocityLimit max_velocity; if (calcMaxVelocity( - aw_info.max_velocity_ptr, - aw_info.temporary_stop_ptr, - &max_velocity.max_velocity)) // publish info + aw_info.max_velocity_ptr, aw_info.temporary_stop_ptr, + &max_velocity.max_velocity)) // publish info { pub_state_->publish(max_velocity); } diff --git a/awapi/awapi_awiv_adapter/src/awapi_obstacle_avoidance_state_publisher.cpp b/awapi/awapi_awiv_adapter/src/awapi_obstacle_avoidance_state_publisher.cpp index 4679cdd7f6ac9..ed55bad5752d1 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_obstacle_avoidance_state_publisher.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_obstacle_avoidance_state_publisher.cpp @@ -61,8 +61,7 @@ void AutowareIvObstacleAvoidanceStatePublisher::getCandidatePathInfo( { if (!path_ptr) { RCLCPP_DEBUG_STREAM_THROTTLE( - logger_, *clock_, 5000 /* ms */, - "obstacle_avoidance_candidate_path is nullptr"); + logger_, *clock_, 5000 /* ms */, "obstacle_avoidance_candidate_path is nullptr"); return; } diff --git a/awapi/awapi_awiv_adapter/src/awapi_pacmod_util.cpp b/awapi/awapi_awiv_adapter/src/awapi_pacmod_util.cpp index eb89dbd16ca97..95aef96962631 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_pacmod_util.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_pacmod_util.cpp @@ -32,16 +32,11 @@ autoware_api_msgs::msg::DoorStatus getDoorStatusMsg( door_status.status = DoorStatus::UNKNOWN; - if ( - msg_ptr->command == SystemRptInt::DOOR_CLOSE && - msg_ptr->output == SystemRptInt::DOOR_OPEN) - { + if (msg_ptr->command == SystemRptInt::DOOR_CLOSE && msg_ptr->output == SystemRptInt::DOOR_OPEN) { // do not used (command & output are always the same value) door_status.status = DoorStatus::DOOR_CLOSING; } else if ( // NOLINT - msg_ptr->command == SystemRptInt::DOOR_OPEN && - msg_ptr->output == SystemRptInt::DOOR_CLOSE) - { + msg_ptr->command == SystemRptInt::DOOR_OPEN && msg_ptr->output == SystemRptInt::DOOR_CLOSE) { // do not used (command & output are always the same value) door_status.status = DoorStatus::DOOR_OPENING; } else if (msg_ptr->output == SystemRptInt::DOOR_CLOSE) { @@ -75,7 +70,9 @@ pacmod_msgs::msg::SystemCmdInt createDoorCommand( door_cmd.enable = true; door_cmd.command = SystemCmdInt::DOOR_NEUTRAL; - if (!msg_ptr) {return door_cmd;} + if (!msg_ptr) { + return door_cmd; + } if (msg_ptr->open) { door_cmd.command = SystemCmdInt::DOOR_OPEN; diff --git a/awapi/awapi_awiv_adapter/src/awapi_stop_reason_aggregator.cpp b/awapi/awapi_awiv_adapter/src/awapi_stop_reason_aggregator.cpp index 3e63ac54c2b07..c5c9994d2d6d6 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_stop_reason_aggregator.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_stop_reason_aggregator.cpp @@ -12,16 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "awapi_awiv_adapter/awapi_stop_reason_aggregator.hpp" + #include #include -#include "awapi_awiv_adapter/awapi_stop_reason_aggregator.hpp" - namespace autoware_api { AutowareIvStopReasonAggregator::AutowareIvStopReasonAggregator( - rclcpp::Node & node, - const double timeout, const double thresh_dist_to_stop_pose) + rclcpp::Node & node, const double timeout, const double thresh_dist_to_stop_pose) : logger_(node.get_logger().get_child("awapi_awiv_stop_reason_aggregator")), clock_(node.get_clock()), timeout_(timeout), @@ -90,8 +89,7 @@ void AutowareIvStopReasonAggregator::applyTimeOut() for (int i = stop_reason_array_vec_.size() - 1; i >= 0; i--) { if ( (current_time - rclcpp::Time(stop_reason_array_vec_.at(i).header.stamp)).seconds() > - timeout_) - { + timeout_) { remove_idx.emplace_back(i); } } diff --git a/awapi/awapi_awiv_adapter/src/awapi_v2x_aggregator.cpp b/awapi/awapi_awiv_adapter/src/awapi_v2x_aggregator.cpp index 4944bf6e5bb6f..174ee63aeae21 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_v2x_aggregator.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_v2x_aggregator.cpp @@ -19,23 +19,15 @@ namespace autoware_api { - namespace { -std::string createKey(const Command & command) -{ - return command.type + "-" + command.id; -} +std::string createKey(const Command & command) { return command.type + "-" + command.id; } -std::string createKey(const State & state) -{ - return state.type + "-" + state.id; -} +std::string createKey(const State & state) { return state.type + "-" + state.id; } } // namespace AutowareIvV2XAggregator::AutowareIvV2XAggregator(rclcpp::Node & node) -: logger_(node.get_logger().get_child("awapi_awiv_v2x_aggregator")), - clock_(node.get_clock()) +: logger_(node.get_logger().get_child("awapi_awiv_v2x_aggregator")), clock_(node.get_clock()) { } diff --git a/awapi/awapi_awiv_adapter/src/awapi_vehicle_state_publisher.cpp b/awapi/awapi_awiv_adapter/src/awapi_vehicle_state_publisher.cpp index bb2bf6e8fb472..de8ac705bc963 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_vehicle_state_publisher.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_vehicle_state_publisher.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "awapi_awiv_adapter/awapi_vehicle_state_publisher.hpp" + #include #include -#include "awapi_awiv_adapter/awapi_vehicle_state_publisher.hpp" - namespace autoware_api { AutowareIvVehicleStatePublisher::AutowareIvVehicleStatePublisher(rclcpp::Node & node) @@ -99,7 +99,7 @@ void AutowareIvVehicleStatePublisher::getSteerInfo( const double ds = steer_ptr->data - previous_steer_ptr_->data; const double dt = std::max( (rclcpp::Time(steer_ptr->header.stamp) - rclcpp::Time(previous_steer_ptr_->header.stamp)) - .seconds(), + .seconds(), 1e-03); const double steer_vel = ds / dt; @@ -159,7 +159,7 @@ void AutowareIvVehicleStatePublisher::getTwistInfo( const double dv = twist_ptr->twist.linear.x - previous_twist_ptr_->twist.linear.x; const double dt = std::max( (rclcpp::Time(twist_ptr->header.stamp) - rclcpp::Time(previous_twist_ptr_->header.stamp)) - .seconds(), + .seconds(), 1e-03); const double accel = dv / dt; From a84216a042d36132e87263bd7d8b36a365b9301c Mon Sep 17 00:00:00 2001 From: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Date: Thu, 4 Nov 2021 19:32:37 +0900 Subject: [PATCH 59/71] Add COLCON_IGNORE (#500) Signed-off-by: Kenji Miyake Signed-off-by: tanaka3 --- awapi/awapi_awiv_adapter/COLCON_IGNORE | 0 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 awapi/awapi_awiv_adapter/COLCON_IGNORE diff --git a/awapi/awapi_awiv_adapter/COLCON_IGNORE b/awapi/awapi_awiv_adapter/COLCON_IGNORE new file mode 100644 index 0000000000000..e69de29bb2d1d From 797da2590da4d768398cd0dce23e21db6fd80dfc Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" <43976882+isamu-takagi@users.noreply.github.com> Date: Wed, 10 Nov 2021 19:03:48 +0900 Subject: [PATCH 60/71] Port awapi awiv adapter to .auto (#554) * Remove COLCON_IGNORE * Change AutowareState * Change EmergencyState * Change HazardStatus * Split file for convert function * Change Path * Change Trajectory * Change ControlMode * Change Shift * Change TurnSignal and move converter * Change Steering * Change VehicleCommand * Fix EmergencyState * Fix for pre-commit * Move autoware state converter * Fix control comand * Change TwistStamped * Remove unused header Signed-off-by: tanaka3 --- awapi/awapi_awiv_adapter/COLCON_IGNORE | 0 .../awapi_autoware_state_publisher.hpp | 10 +-- .../awapi_autoware_util.hpp | 56 ++++++------- .../awapi_awiv_adapter_core.hpp | 81 +++++++++++-------- .../awapi_lane_change_state_publisher.hpp | 2 +- ...api_obstacle_avoidance_state_publisher.hpp | 2 +- .../awapi_vehicle_state_publisher.hpp | 17 ++-- .../launch/awapi_awiv_adapter.launch.xml | 12 +-- awapi/awapi_awiv_adapter/package.xml | 7 +- .../src/awapi_autoware_state_publisher.cpp | 48 ++++++----- .../src/awapi_autoware_util.cpp | 6 +- .../src/awapi_awiv_adapter_core.cpp | 76 +++++++++-------- .../src/awapi_lane_change_state_publisher.cpp | 7 +- ...api_obstacle_avoidance_state_publisher.cpp | 7 +- .../src/awapi_vehicle_state_publisher.cpp | 67 ++++++++------- 15 files changed, 228 insertions(+), 170 deletions(-) delete mode 100644 awapi/awapi_awiv_adapter/COLCON_IGNORE diff --git a/awapi/awapi_awiv_adapter/COLCON_IGNORE b/awapi/awapi_awiv_adapter/COLCON_IGNORE deleted file mode 100644 index e69de29bb2d1d..0000000000000 diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.hpp b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.hpp index 60b5389197f50..1cb67baf907e8 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.hpp +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.hpp @@ -43,19 +43,19 @@ class AutowareIvAutowareStatePublisher /* parameter for judging goal now */ bool arrived_goal_; - autoware_system_msgs::msg::AutowareState::_state_type prev_state_; + autoware_auto_system_msgs::msg::AutowareState::_state_type prev_state_; void getAutowareStateInfo( - const autoware_system_msgs::msg::AutowareState::ConstSharedPtr & autoware_state_ptr, + const autoware_auto_system_msgs::msg::AutowareState::ConstSharedPtr & autoware_state_ptr, autoware_api_msgs::msg::AwapiAutowareStatus * status); void getControlModeInfo( - const autoware_vehicle_msgs::msg::ControlMode::ConstSharedPtr & control_mode_ptr, + const autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr & control_mode_ptr, autoware_api_msgs::msg::AwapiAutowareStatus * status); void getGateModeInfo( const autoware_control_msgs::msg::GateMode::ConstSharedPtr & gate_mode_ptr, autoware_api_msgs::msg::AwapiAutowareStatus * status); void getEmergencyStateInfo( - const autoware_system_msgs::msg::EmergencyStateStamped::ConstSharedPtr & emergency_state_ptr, + const autoware_auto_system_msgs::msg::EmergencyState::ConstSharedPtr & emergency_state_ptr, autoware_api_msgs::msg::AwapiAutowareStatus * status); void getCurrentMaxVelInfo( const autoware_planning_msgs::msg::VelocityLimit::ConstSharedPtr & current_max_velocity_ptr, @@ -73,7 +73,7 @@ class AutowareIvAutowareStatePublisher const pacmod_msgs::msg::GlobalRpt::ConstSharedPtr & global_rpt_ptr, autoware_api_msgs::msg::AwapiAutowareStatus * status); - bool isGoal(const autoware_system_msgs::msg::AutowareState::ConstSharedPtr & autoware_state); + bool isGoal(const autoware_auto_system_msgs::msg::AutowareState::ConstSharedPtr & autoware_state); }; } // namespace autoware_api diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.hpp b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.hpp index 0f70b9bf14a84..4e2559937ec60 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.hpp +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.hpp @@ -19,27 +19,28 @@ #include #include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include #include #include -#include #include -#include #include -#include -#include -#include #include #include #include -#include -#include -#include -#include -#include #include #include -#include +#include #include #include #include @@ -56,18 +57,19 @@ namespace autoware_api struct AutowareInfo { std::shared_ptr current_pose_ptr; - autoware_vehicle_msgs::msg::Steering::ConstSharedPtr steer_ptr; - autoware_vehicle_msgs::msg::VehicleCommand::ConstSharedPtr vehicle_cmd_ptr; - autoware_vehicle_msgs::msg::TurnSignal::ConstSharedPtr turn_signal_ptr; - geometry_msgs::msg::TwistStamped::ConstSharedPtr twist_ptr; - autoware_vehicle_msgs::msg::ShiftStamped::ConstSharedPtr gear_ptr; + autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr steer_ptr; + autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr vehicle_cmd_ptr; + autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr turn_indicators_ptr; + autoware_auto_vehicle_msgs::msg::HazardLightsReport::ConstSharedPtr hazard_lights_ptr; + nav_msgs::msg::Odometry::ConstSharedPtr odometry_ptr; + autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr gear_ptr; autoware_vehicle_msgs::msg::BatteryStatus::ConstSharedPtr battery_ptr; sensor_msgs::msg::NavSatFix::ConstSharedPtr nav_sat_ptr; - autoware_system_msgs::msg::AutowareState::ConstSharedPtr autoware_state_ptr; - autoware_vehicle_msgs::msg::ControlMode::ConstSharedPtr control_mode_ptr; + autoware_auto_system_msgs::msg::AutowareState::ConstSharedPtr autoware_state_ptr; + autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr control_mode_ptr; autoware_control_msgs::msg::GateMode::ConstSharedPtr gate_mode_ptr; - autoware_system_msgs::msg::EmergencyStateStamped::ConstSharedPtr emergency_state_ptr; - autoware_system_msgs::msg::HazardStatusStamped::ConstSharedPtr hazard_status_ptr; + autoware_auto_system_msgs::msg::EmergencyState::ConstSharedPtr emergency_state_ptr; + autoware_auto_system_msgs::msg::HazardStatusStamped::ConstSharedPtr hazard_status_ptr; autoware_planning_msgs::msg::StopReasonArray::ConstSharedPtr stop_reason_ptr; autoware_v2x_msgs::msg::InfrastructureCommandArray::ConstSharedPtr v2x_command_ptr; autoware_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr v2x_state_ptr; @@ -75,13 +77,13 @@ struct AutowareInfo pacmod_msgs::msg::GlobalRpt::ConstSharedPtr global_rpt_ptr; autoware_planning_msgs::msg::LaneChangeStatus::ConstSharedPtr lane_change_available_ptr; autoware_planning_msgs::msg::LaneChangeStatus::ConstSharedPtr lane_change_ready_ptr; - autoware_planning_msgs::msg::Path::ConstSharedPtr lane_change_candidate_ptr; + autoware_auto_planning_msgs::msg::Path::ConstSharedPtr lane_change_candidate_ptr; autoware_planning_msgs::msg::IsAvoidancePossible::ConstSharedPtr obstacle_avoid_ready_ptr; - autoware_planning_msgs::msg::Trajectory::ConstSharedPtr obstacle_avoid_candidate_ptr; + autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr obstacle_avoid_candidate_ptr; autoware_api_msgs::msg::VelocityLimit::ConstSharedPtr max_velocity_ptr; autoware_planning_msgs::msg::VelocityLimit::ConstSharedPtr current_max_velocity_ptr; autoware_api_msgs::msg::StopCommand::ConstSharedPtr temporary_stop_ptr; - autoware_planning_msgs::msg::Trajectory::ConstSharedPtr autoware_planning_traj_ptr; + autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr autoware_planning_traj_ptr; pacmod_msgs::msg::SystemRptInt::ConstSharedPtr door_state_ptr; }; @@ -115,11 +117,11 @@ double lowpass_filter(const double current_value, const double prev_value, const namespace planning_util { bool calcClosestIndex( - const autoware_planning_msgs::msg::Trajectory & traj, const geometry_msgs::msg::Pose & pose, + const autoware_auto_planning_msgs::msg::Trajectory & traj, const geometry_msgs::msg::Pose & pose, size_t & output_closest_idx, const double dist_thr = 10.0, const double angle_thr = M_PI / 2.0); inline geometry_msgs::msg::Pose getPose( - const autoware_planning_msgs::msg::Trajectory & traj, const int idx) + const autoware_auto_planning_msgs::msg::Trajectory & traj, const int idx) { return traj.points.at(idx).pose; } @@ -132,11 +134,11 @@ inline double calcDist2d(const geometry_msgs::msg::Point & a, const geometry_msg double normalizeEulerAngle(double euler); double calcArcLengthFromWayPoint( - const autoware_planning_msgs::msg::Trajectory & input_path, const size_t src_idx, + const autoware_auto_planning_msgs::msg::Trajectory & input_path, const size_t src_idx, const size_t dst_idx); double calcDistanceAlongTrajectory( - const autoware_planning_msgs::msg::Trajectory & trajectory, + const autoware_auto_planning_msgs::msg::Trajectory & trajectory, const geometry_msgs::msg::Pose & current_pose, const geometry_msgs::msg::Pose & target_pose); } // namespace planning_util diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.hpp b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.hpp index bf8b6c475fc36..14abb4ead25a1 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.hpp +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.hpp @@ -27,21 +27,22 @@ #include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include -#include #include -#include -#include -#include #include #include -#include -#include -#include -#include -#include #include -#include +#include #include #include @@ -59,18 +60,24 @@ class AutowareIvAdapter : public rclcpp::Node private: // subscriber - rclcpp::Subscription::SharedPtr sub_steer_; - rclcpp::Subscription::SharedPtr sub_vehicle_cmd_; - rclcpp::Subscription::SharedPtr sub_turn_signal_cmd_; - rclcpp::Subscription::SharedPtr sub_twist_; - rclcpp::Subscription::SharedPtr sub_gear_; + rclcpp::Subscription::SharedPtr sub_steer_; + rclcpp::Subscription::SharedPtr + sub_vehicle_cmd_; + rclcpp::Subscription::SharedPtr + sub_turn_indicators_; + rclcpp::Subscription::SharedPtr + sub_hazard_lights_; + rclcpp::Subscription::SharedPtr sub_odometry_; + rclcpp::Subscription::SharedPtr sub_gear_; rclcpp::Subscription::SharedPtr sub_battery_; rclcpp::Subscription::SharedPtr sub_nav_sat_; - rclcpp::Subscription::SharedPtr sub_autoware_state_; - rclcpp::Subscription::SharedPtr sub_control_mode_; + rclcpp::Subscription::SharedPtr + sub_autoware_state_; + rclcpp::Subscription::SharedPtr + sub_control_mode_; rclcpp::Subscription::SharedPtr sub_gate_mode_; - rclcpp::Subscription::SharedPtr sub_emergency_; - rclcpp::Subscription::SharedPtr + rclcpp::Subscription::SharedPtr sub_emergency_; + rclcpp::Subscription::SharedPtr sub_hazard_status_; rclcpp::Subscription::SharedPtr sub_stop_reason_; rclcpp::Subscription::SharedPtr @@ -83,16 +90,17 @@ class AutowareIvAdapter : public rclcpp::Node sub_lane_change_available_; rclcpp::Subscription::SharedPtr sub_lane_change_ready_; - rclcpp::Subscription::SharedPtr sub_lane_change_candidate_; + rclcpp::Subscription::SharedPtr + sub_lane_change_candidate_; rclcpp::Subscription::SharedPtr sub_obstacle_avoid_ready_; - rclcpp::Subscription::SharedPtr + rclcpp::Subscription::SharedPtr sub_obstacle_avoid_candidate_; rclcpp::Subscription::SharedPtr sub_max_velocity_; rclcpp::Subscription::SharedPtr sub_current_max_velocity_; rclcpp::Subscription::SharedPtr sub_temporary_stop_; - rclcpp::Subscription::SharedPtr sub_autoware_traj_; + rclcpp::Subscription::SharedPtr sub_autoware_traj_; rclcpp::Subscription::SharedPtr sub_door_control_; rclcpp::Subscription::SharedPtr sub_door_status_; @@ -111,21 +119,26 @@ class AutowareIvAdapter : public rclcpp::Node tf2_ros::TransformListener tf_listener_; // callback function - void callbackSteer(const autoware_vehicle_msgs::msg::Steering::ConstSharedPtr msg_ptr); - void callbackVehicleCmd(const autoware_vehicle_msgs::msg::VehicleCommand::ConstSharedPtr msg_ptr); - void callbackTurnSignal(const autoware_vehicle_msgs::msg::TurnSignal::ConstSharedPtr msg_ptr); - void callbackTwist(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg_ptr); - void callbackGear(const autoware_vehicle_msgs::msg::ShiftStamped::ConstSharedPtr msg_ptr); + void callbackSteer(const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr msg_ptr); + void callbackVehicleCmd( + const autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr msg_ptr); + void callbackTurnIndicators( + const autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr msg_ptr); + void callbackHazardLights( + const autoware_auto_vehicle_msgs::msg::HazardLightsReport::ConstSharedPtr msg_ptr); + void callbackTwist(const nav_msgs::msg::Odometry::ConstSharedPtr msg_ptr); + void callbackGear(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr msg_ptr); void callbackBattery(const autoware_vehicle_msgs::msg::BatteryStatus::ConstSharedPtr msg_ptr); void callbackNavSat(const sensor_msgs::msg::NavSatFix::ConstSharedPtr msg_ptr); void callbackAutowareState( - const autoware_system_msgs::msg::AutowareState::ConstSharedPtr msg_ptr); - void callbackControlMode(const autoware_vehicle_msgs::msg::ControlMode::ConstSharedPtr msg_ptr); + const autoware_auto_system_msgs::msg::AutowareState::ConstSharedPtr msg_ptr); + void callbackControlMode( + const autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg_ptr); void callbackGateMode(const autoware_control_msgs::msg::GateMode::ConstSharedPtr msg_ptr); void callbackEmergencyState( - const autoware_system_msgs::msg::EmergencyStateStamped::ConstSharedPtr msg_ptr); + const autoware_auto_system_msgs::msg::EmergencyState::ConstSharedPtr msg_ptr); void callbackHazardStatus( - const autoware_system_msgs::msg::HazardStatusStamped::ConstSharedPtr msg_ptr); + const autoware_auto_system_msgs::msg::HazardStatusStamped::ConstSharedPtr msg_ptr); void callbackStopReason( const autoware_planning_msgs::msg::StopReasonArray::ConstSharedPtr msg_ptr); void callbackV2XCommand( @@ -139,17 +152,17 @@ class AutowareIvAdapter : public rclcpp::Node void callbackLaneChangeReady( const autoware_planning_msgs::msg::LaneChangeStatus::ConstSharedPtr msg_ptr); void callbackLaneChangeCandidatePath( - const autoware_planning_msgs::msg::Path::ConstSharedPtr msg_ptr); + const autoware_auto_planning_msgs::msg::Path::ConstSharedPtr msg_ptr); void callbackLaneObstacleAvoidReady( const autoware_planning_msgs::msg::IsAvoidancePossible::ConstSharedPtr msg_ptr); void callbackLaneObstacleAvoidCandidatePath( - const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr msg_ptr); + const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg_ptr); void callbackMaxVelocity(const autoware_api_msgs::msg::VelocityLimit::ConstSharedPtr msg_ptr); void callbackCurrentMaxVelocity( const autoware_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg_ptr); void callbackTemporaryStop(const autoware_api_msgs::msg::StopCommand::ConstSharedPtr msg_ptr); void callbackAutowareTrajectory( - const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr msg_ptr); + const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg_ptr); void callbackDoorControl( const autoware_api_msgs::msg::DoorControlCommand::ConstSharedPtr msg_ptr); void callbackDoorStatus(const pacmod_msgs::msg::SystemRptInt::ConstSharedPtr msg_ptr); diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_lane_change_state_publisher.hpp b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_lane_change_state_publisher.hpp index a379218bbd1f7..f4b2803924405 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_lane_change_state_publisher.hpp +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_lane_change_state_publisher.hpp @@ -43,7 +43,7 @@ class AutowareIvLaneChangeStatePublisher const autoware_planning_msgs::msg::LaneChangeStatus::ConstSharedPtr & ready_ptr, autoware_api_msgs::msg::LaneChangeStatus * status); void getCandidatePathInfo( - const autoware_planning_msgs::msg::Path::ConstSharedPtr & path_ptr, + const autoware_auto_planning_msgs::msg::Path::ConstSharedPtr & path_ptr, autoware_api_msgs::msg::LaneChangeStatus * status); }; diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_obstacle_avoidance_state_publisher.hpp b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_obstacle_avoidance_state_publisher.hpp index 568464c6eb4cc..bb9bc23d71c86 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_obstacle_avoidance_state_publisher.hpp +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_obstacle_avoidance_state_publisher.hpp @@ -40,7 +40,7 @@ class AutowareIvObstacleAvoidanceStatePublisher const autoware_planning_msgs::msg::IsAvoidancePossible::ConstSharedPtr & ready_ptr, autoware_api_msgs::msg::ObstacleAvoidanceStatus * status); void getCandidatePathInfo( - const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr & path_ptr, + const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr & path_ptr, autoware_api_msgs::msg::ObstacleAvoidanceStatus * status); }; diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_vehicle_state_publisher.hpp b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_vehicle_state_publisher.hpp index 1b2ca906bfaeb..6725f26b513a0 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_vehicle_state_publisher.hpp +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_vehicle_state_publisher.hpp @@ -40,19 +40,22 @@ class AutowareIvVehicleStatePublisher const std::shared_ptr & pose_ptr, autoware_api_msgs::msg::AwapiVehicleStatus * status); void getSteerInfo( - const autoware_vehicle_msgs::msg::Steering::ConstSharedPtr & steer_ptr, + const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr & steer_ptr, autoware_api_msgs::msg::AwapiVehicleStatus * status); void getVehicleCmdInfo( - const autoware_vehicle_msgs::msg::VehicleCommand::ConstSharedPtr & vehicle_cmd_ptr, + const autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr & + vehicle_cmd_ptr, autoware_api_msgs::msg::AwapiVehicleStatus * status); void getTurnSignalInfo( - const autoware_vehicle_msgs::msg::TurnSignal::ConstSharedPtr & turn_signal_ptr, + const autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr & + turn_indicators_ptr, + const autoware_auto_vehicle_msgs::msg::HazardLightsReport::ConstSharedPtr & hazard_lights_ptr, autoware_api_msgs::msg::AwapiVehicleStatus * status); void getTwistInfo( - const geometry_msgs::msg::TwistStamped::ConstSharedPtr & twist_ptr, + const nav_msgs::msg::Odometry::ConstSharedPtr & odometry_ptr, autoware_api_msgs::msg::AwapiVehicleStatus * status); void getGearInfo( - const autoware_vehicle_msgs::msg::ShiftStamped::ConstSharedPtr & gear_ptr, + const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr & gear_ptr, autoware_api_msgs::msg::AwapiVehicleStatus * status); void getBatteryInfo( const autoware_vehicle_msgs::msg::BatteryStatus::ConstSharedPtr & battery_ptr, @@ -65,8 +68,8 @@ class AutowareIvVehicleStatePublisher rclcpp::Clock::SharedPtr clock_; // parameters - geometry_msgs::msg::TwistStamped::ConstSharedPtr previous_twist_ptr_; - autoware_vehicle_msgs::msg::Steering::ConstSharedPtr previous_steer_ptr_; + nav_msgs::msg::Odometry::ConstSharedPtr previous_odometry_ptr_; + autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr previous_steer_ptr_; double prev_accel_; double prev_steer_vel_; diff --git a/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml b/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml index 60bb9b52e73e4..ec0f8fb5cb996 100644 --- a/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml +++ b/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml @@ -6,9 +6,10 @@ - - - + + + + @@ -105,8 +106,9 @@ - - + + + diff --git a/awapi/awapi_awiv_adapter/package.xml b/awapi/awapi_awiv_adapter/package.xml index 86ecdbc7a4b53..ded85d12dd671 100644 --- a/awapi/awapi_awiv_adapter/package.xml +++ b/awapi/awapi_awiv_adapter/package.xml @@ -10,13 +10,18 @@ ament_cmake_auto autoware_api_msgs + autoware_auto_control_msgs + autoware_auto_planning_msgs + autoware_auto_system_msgs + autoware_auto_vehicle_msgs autoware_control_msgs + autoware_iv_auto_msgs_converter autoware_planning_msgs - autoware_system_msgs autoware_v2x_msgs autoware_vehicle_msgs diagnostic_msgs geometry_msgs + nav_msgs pacmod_msgs rclcpp sensor_msgs diff --git a/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp b/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp index 393b98a9f1a72..07dc39ee48838 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp @@ -14,6 +14,7 @@ #include "awapi_awiv_adapter/awapi_autoware_state_publisher.hpp" +#include "autoware_iv_auto_msgs_converter/autoware_iv_auto_msgs_converter.hpp" #include "awapi_awiv_adapter/diagnostics_filter.hpp" #include @@ -57,7 +58,7 @@ void AutowareIvAutowareStatePublisher::statePublisher(const AutowareInfo & aw_in } void AutowareIvAutowareStatePublisher::getAutowareStateInfo( - const autoware_system_msgs::msg::AutowareState::ConstSharedPtr & autoware_state_ptr, + const autoware_auto_system_msgs::msg::AutowareState::ConstSharedPtr & autoware_state_ptr, autoware_api_msgs::msg::AwapiAutowareStatus * status) { if (!autoware_state_ptr) { @@ -66,12 +67,13 @@ void AutowareIvAutowareStatePublisher::getAutowareStateInfo( } // get autoware_state - status->autoware_state = autoware_state_ptr->state; + using autoware_iv_auto_msgs_converter::convert; + status->autoware_state = convert(*autoware_state_ptr).state; status->arrived_goal = isGoal(autoware_state_ptr); } void AutowareIvAutowareStatePublisher::getControlModeInfo( - const autoware_vehicle_msgs::msg::ControlMode::ConstSharedPtr & control_mode_ptr, + const autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr & control_mode_ptr, autoware_api_msgs::msg::AwapiAutowareStatus * status) { if (!control_mode_ptr) { @@ -80,7 +82,8 @@ void AutowareIvAutowareStatePublisher::getControlModeInfo( } // get control mode - status->control_mode = control_mode_ptr->data; + using autoware_iv_auto_msgs_converter::convert; + status->control_mode = convert(*control_mode_ptr).data; } void AutowareIvAutowareStatePublisher::getGateModeInfo( @@ -97,7 +100,7 @@ void AutowareIvAutowareStatePublisher::getGateModeInfo( } void AutowareIvAutowareStatePublisher::getEmergencyStateInfo( - const autoware_system_msgs::msg::EmergencyStateStamped::ConstSharedPtr & emergency_state_ptr, + const autoware_auto_system_msgs::msg::EmergencyState::ConstSharedPtr & emergency_state_ptr, autoware_api_msgs::msg::AwapiAutowareStatus * status) { if (!emergency_state_ptr) { @@ -106,10 +109,10 @@ void AutowareIvAutowareStatePublisher::getEmergencyStateInfo( } // get emergency - using autoware_system_msgs::msg::EmergencyState; - status->emergency_stopped = (emergency_state_ptr->state.state == EmergencyState::MRM_OPERATING) || - (emergency_state_ptr->state.state == EmergencyState::MRM_SUCCEEDED) || - (emergency_state_ptr->state.state == EmergencyState::MRM_FAILED); + using autoware_auto_system_msgs::msg::EmergencyState; + status->emergency_stopped = (emergency_state_ptr->state == EmergencyState::MRM_OPERATING) || + (emergency_state_ptr->state == EmergencyState::MRM_SUCCEEDED) || + (emergency_state_ptr->state == EmergencyState::MRM_FAILED); } void AutowareIvAutowareStatePublisher::getCurrentMaxVelInfo( @@ -152,7 +155,8 @@ void AutowareIvAutowareStatePublisher::getHazardStatusInfo( } // get emergency - status->hazard_status = *aw_info.hazard_status_ptr; + using autoware_iv_auto_msgs_converter::convert; + status->hazard_status = convert(*aw_info.hazard_status_ptr); // filter leaf diagnostics status->hazard_status.status.diagnostics_spf = @@ -195,8 +199,8 @@ void AutowareIvAutowareStatePublisher::getDiagInfo( void AutowareIvAutowareStatePublisher::getErrorDiagInfo( const AutowareInfo & aw_info, autoware_api_msgs::msg::AwapiAutowareStatus * status) { - using autoware_system_msgs::msg::AutowareState; - using autoware_vehicle_msgs::msg::ControlMode; + using autoware_auto_system_msgs::msg::AutowareState; + using autoware_auto_vehicle_msgs::msg::ControlModeReport; if (!aw_info.autoware_state_ptr) { RCLCPP_DEBUG_STREAM_THROTTLE( @@ -230,22 +234,22 @@ void AutowareIvAutowareStatePublisher::getErrorDiagInfo( const auto & hazard_status = aw_info.hazard_status_ptr->status; std::vector error_diagnostics; - for (const auto & hazard_diag : hazard_status.diagnostics_spf) { + for (const auto & hazard_diag : hazard_status.diag_single_point_fault) { auto diag = hazard_diag; diag.message = "[Single Point Fault]" + hazard_diag.message; error_diagnostics.push_back(diag); } - for (const auto & hazard_diag : hazard_status.diagnostics_lf) { + for (const auto & hazard_diag : hazard_status.diag_latent_fault) { auto diag = hazard_diag; diag.message = "[Latent Fault]" + hazard_diag.message; error_diagnostics.push_back(diag); } - for (const auto & hazard_diag : hazard_status.diagnostics_sf) { + for (const auto & hazard_diag : hazard_status.diag_safe_fault) { auto diag = hazard_diag; diag.message = "[Safe Fault]" + hazard_diag.message; error_diagnostics.push_back(diag); } - for (const auto & hazard_diag : hazard_status.diagnostics_nf) { + for (const auto & hazard_diag : hazard_status.diag_no_fault) { auto diag = hazard_diag; diag.message = "[No Fault]" + hazard_diag.message; diag.level = DiagnosticStatus::OK; @@ -270,22 +274,22 @@ void AutowareIvAutowareStatePublisher::getGlobalRptInfo( } bool AutowareIvAutowareStatePublisher::isGoal( - const autoware_system_msgs::msg::AutowareState::ConstSharedPtr & autoware_state) + const autoware_auto_system_msgs::msg::AutowareState::ConstSharedPtr & autoware_state) { // rename const auto & aw_state = autoware_state->state; - if (aw_state == autoware_system_msgs::msg::AutowareState::ARRIVAL_GOAL) { + if (aw_state == autoware_auto_system_msgs::msg::AutowareState::ARRIVED_GOAL) { arrived_goal_ = true; } else if ( // NOLINT - prev_state_ == autoware_system_msgs::msg::AutowareState::DRIVING && - aw_state == autoware_system_msgs::msg::AutowareState::WAITING_FOR_ROUTE) { + prev_state_ == autoware_auto_system_msgs::msg::AutowareState::DRIVING && + aw_state == autoware_auto_system_msgs::msg::AutowareState::WAITING_FOR_ROUTE) { arrived_goal_ = true; } if ( - aw_state == autoware_system_msgs::msg::AutowareState::WAITING_FOR_ENGAGE || - aw_state == autoware_system_msgs::msg::AutowareState::DRIVING) { + aw_state == autoware_auto_system_msgs::msg::AutowareState::WAITING_FOR_ENGAGE || + aw_state == autoware_auto_system_msgs::msg::AutowareState::DRIVING) { // cancel goal state arrived_goal_ = false; } diff --git a/awapi/awapi_awiv_adapter/src/awapi_autoware_util.cpp b/awapi/awapi_awiv_adapter/src/awapi_autoware_util.cpp index 9b796adbc4dfe..7d6ccba672996 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_autoware_util.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_autoware_util.cpp @@ -26,7 +26,7 @@ double lowpass_filter(const double current_value, const double prev_value, const namespace planning_util { bool calcClosestIndex( - const autoware_planning_msgs::msg::Trajectory & traj, const geometry_msgs::msg::Pose & pose, + const autoware_auto_planning_msgs::msg::Trajectory & traj, const geometry_msgs::msg::Pose & pose, size_t & output_closest_idx, const double dist_thr, const double angle_thr) { double dist_min = std::numeric_limits::max(); @@ -74,7 +74,7 @@ double normalizeEulerAngle(double euler) } double calcArcLengthFromWayPoint( - const autoware_planning_msgs::msg::Trajectory & input_path, const size_t src_idx, + const autoware_auto_planning_msgs::msg::Trajectory & input_path, const size_t src_idx, const size_t dst_idx) { double length = 0; @@ -89,7 +89,7 @@ double calcArcLengthFromWayPoint( } double calcDistanceAlongTrajectory( - const autoware_planning_msgs::msg::Trajectory & trajectory, + const autoware_auto_planning_msgs::msg::Trajectory & trajectory, const geometry_msgs::msg::Pose & current_pose, const geometry_msgs::msg::Pose & target_pose) { size_t self_idx; diff --git a/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp b/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp index b9bd7d256bc7a..19d7a2409a6e1 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp @@ -62,30 +62,37 @@ AutowareIvAdapter::AutowareIvAdapter() auto durable_qos = rclcpp::QoS{1}.transient_local(); - sub_steer_ = this->create_subscription( + sub_steer_ = this->create_subscription( "input/steer", 1, std::bind(&AutowareIvAdapter::callbackSteer, this, _1)); - sub_vehicle_cmd_ = this->create_subscription( - "input/vehicle_cmd", durable_qos, std::bind(&AutowareIvAdapter::callbackVehicleCmd, this, _1)); - sub_turn_signal_cmd_ = this->create_subscription( - "input/turn_signal", 1, std::bind(&AutowareIvAdapter::callbackTurnSignal, this, _1)); - sub_twist_ = this->create_subscription( - "input/twist", 1, std::bind(&AutowareIvAdapter::callbackTwist, this, _1)); - sub_gear_ = this->create_subscription( + sub_vehicle_cmd_ = + this->create_subscription( + "input/vehicle_cmd", durable_qos, + std::bind(&AutowareIvAdapter::callbackVehicleCmd, this, _1)); + sub_turn_indicators_ = + this->create_subscription( + "input/turn_indicators", 1, std::bind(&AutowareIvAdapter::callbackTurnIndicators, this, _1)); + sub_hazard_lights_ = + this->create_subscription( + "input/hazard_lights", 1, std::bind(&AutowareIvAdapter::callbackHazardLights, this, _1)); + sub_odometry_ = this->create_subscription( + "input/odometry", 1, std::bind(&AutowareIvAdapter::callbackTwist, this, _1)); + sub_gear_ = this->create_subscription( "input/gear", 1, std::bind(&AutowareIvAdapter::callbackGear, this, _1)); sub_battery_ = this->create_subscription( "input/battery", 1, std::bind(&AutowareIvAdapter::callbackBattery, this, _1)); sub_nav_sat_ = this->create_subscription( "input/nav_sat", 1, std::bind(&AutowareIvAdapter::callbackNavSat, this, _1)); - sub_autoware_state_ = this->create_subscription( + sub_autoware_state_ = this->create_subscription( "input/autoware_state", 1, std::bind(&AutowareIvAdapter::callbackAutowareState, this, _1)); - sub_control_mode_ = this->create_subscription( + sub_control_mode_ = this->create_subscription( "input/control_mode", 1, std::bind(&AutowareIvAdapter::callbackControlMode, this, _1)); sub_gate_mode_ = this->create_subscription( "input/gate_mode", durable_qos, std::bind(&AutowareIvAdapter::callbackGateMode, this, _1)); - sub_emergency_ = this->create_subscription( + sub_emergency_ = this->create_subscription( "input/emergency_state", 1, std::bind(&AutowareIvAdapter::callbackEmergencyState, this, _1)); - sub_hazard_status_ = this->create_subscription( - "input/hazard_status", 1, std::bind(&AutowareIvAdapter::callbackHazardStatus, this, _1)); + sub_hazard_status_ = + this->create_subscription( + "input/hazard_status", 1, std::bind(&AutowareIvAdapter::callbackHazardStatus, this, _1)); sub_stop_reason_ = this->create_subscription( "input/stop_reason", 100, std::bind(&AutowareIvAdapter::callbackStopReason, this, _1)); sub_v2x_command_ = this->create_subscription( @@ -102,7 +109,7 @@ AutowareIvAdapter::AutowareIvAdapter() std::bind(&AutowareIvAdapter::callbackLaneChangeAvailable, this, _1)); sub_lane_change_ready_ = this->create_subscription( "input/lane_change_ready", 1, std::bind(&AutowareIvAdapter::callbackLaneChangeReady, this, _1)); - sub_lane_change_candidate_ = this->create_subscription( + sub_lane_change_candidate_ = this->create_subscription( "input/lane_change_candidate_path", 1, std::bind(&AutowareIvAdapter::callbackLaneChangeCandidatePath, this, _1)); sub_obstacle_avoid_ready_ = @@ -110,7 +117,7 @@ AutowareIvAdapter::AutowareIvAdapter() "input/obstacle_avoid_ready", durable_qos, std::bind(&AutowareIvAdapter::callbackLaneObstacleAvoidReady, this, _1)); sub_obstacle_avoid_candidate_ = - this->create_subscription( + this->create_subscription( "input/obstacle_avoid_candidate_path", durable_qos, std::bind(&AutowareIvAdapter::callbackLaneObstacleAvoidCandidatePath, this, _1)); sub_max_velocity_ = this->create_subscription( @@ -120,7 +127,7 @@ AutowareIvAdapter::AutowareIvAdapter() std::bind(&AutowareIvAdapter::callbackCurrentMaxVelocity, this, _1)); sub_temporary_stop_ = this->create_subscription( "input/temporary_stop", 1, std::bind(&AutowareIvAdapter::callbackTemporaryStop, this, _1)); - sub_autoware_traj_ = this->create_subscription( + sub_autoware_traj_ = this->create_subscription( "input/autoware_trajectory", 1, std::bind(&AutowareIvAdapter::callbackAutowareTrajectory, this, _1)); sub_door_control_ = this->create_subscription( @@ -176,31 +183,36 @@ void AutowareIvAdapter::timerCallback() } void AutowareIvAdapter::callbackSteer( - const autoware_vehicle_msgs::msg::Steering::ConstSharedPtr msg_ptr) + const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr msg_ptr) { aw_info_.steer_ptr = msg_ptr; } void AutowareIvAdapter::callbackVehicleCmd( - const autoware_vehicle_msgs::msg::VehicleCommand::ConstSharedPtr msg_ptr) + const autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr msg_ptr) { aw_info_.vehicle_cmd_ptr = msg_ptr; } -void AutowareIvAdapter::callbackTurnSignal( - const autoware_vehicle_msgs::msg::TurnSignal::ConstSharedPtr msg_ptr) +void AutowareIvAdapter::callbackTurnIndicators( + const autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr msg_ptr) { - aw_info_.turn_signal_ptr = msg_ptr; + aw_info_.turn_indicators_ptr = msg_ptr; } -void AutowareIvAdapter::callbackTwist( - const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg_ptr) +void AutowareIvAdapter::callbackHazardLights( + const autoware_auto_vehicle_msgs::msg::HazardLightsReport::ConstSharedPtr msg_ptr) { - aw_info_.twist_ptr = msg_ptr; + aw_info_.hazard_lights_ptr = msg_ptr; +} + +void AutowareIvAdapter::callbackTwist(const nav_msgs::msg::Odometry::ConstSharedPtr msg_ptr) +{ + aw_info_.odometry_ptr = msg_ptr; } void AutowareIvAdapter::callbackGear( - const autoware_vehicle_msgs::msg::ShiftStamped::ConstSharedPtr msg_ptr) + const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr msg_ptr) { aw_info_.gear_ptr = msg_ptr; } @@ -234,12 +246,12 @@ void AutowareIvAdapter::getCurrentPose() } void AutowareIvAdapter::callbackAutowareState( - const autoware_system_msgs::msg::AutowareState::ConstSharedPtr msg_ptr) + const autoware_auto_system_msgs::msg::AutowareState::ConstSharedPtr msg_ptr) { aw_info_.autoware_state_ptr = msg_ptr; } void AutowareIvAdapter::callbackControlMode( - const autoware_vehicle_msgs::msg::ControlMode::ConstSharedPtr msg_ptr) + const autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg_ptr) { aw_info_.control_mode_ptr = msg_ptr; } @@ -251,13 +263,13 @@ void AutowareIvAdapter::callbackGateMode( } void AutowareIvAdapter::callbackEmergencyState( - const autoware_system_msgs::msg::EmergencyStateStamped::ConstSharedPtr msg_ptr) + const autoware_auto_system_msgs::msg::EmergencyState::ConstSharedPtr msg_ptr) { aw_info_.emergency_state_ptr = msg_ptr; } void AutowareIvAdapter::callbackHazardStatus( - const autoware_system_msgs::msg::HazardStatusStamped::ConstSharedPtr msg_ptr) + const autoware_auto_system_msgs::msg::HazardStatusStamped::ConstSharedPtr msg_ptr) { aw_info_.hazard_status_ptr = msg_ptr; } @@ -304,7 +316,7 @@ void AutowareIvAdapter::callbackLaneChangeReady( } void AutowareIvAdapter::callbackLaneChangeCandidatePath( - const autoware_planning_msgs::msg::Path::ConstSharedPtr msg_ptr) + const autoware_auto_planning_msgs::msg::Path::ConstSharedPtr msg_ptr) { aw_info_.lane_change_candidate_ptr = msg_ptr; } @@ -316,7 +328,7 @@ void AutowareIvAdapter::callbackLaneObstacleAvoidReady( } void AutowareIvAdapter::callbackLaneObstacleAvoidCandidatePath( - const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr msg_ptr) + const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg_ptr) { aw_info_.obstacle_avoid_candidate_ptr = msg_ptr; } @@ -349,7 +361,7 @@ void AutowareIvAdapter::callbackTemporaryStop( } void AutowareIvAdapter::callbackAutowareTrajectory( - const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr msg_ptr) + const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg_ptr) { aw_info_.autoware_planning_traj_ptr = msg_ptr; } diff --git a/awapi/awapi_awiv_adapter/src/awapi_lane_change_state_publisher.cpp b/awapi/awapi_awiv_adapter/src/awapi_lane_change_state_publisher.cpp index 7ea31c11150a1..522e992abf584 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_lane_change_state_publisher.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_lane_change_state_publisher.cpp @@ -14,6 +14,8 @@ #include "awapi_awiv_adapter/awapi_lane_change_state_publisher.hpp" +#include "autoware_iv_auto_msgs_converter/autoware_iv_auto_msgs_converter.hpp" + namespace autoware_api { AutowareIvLaneChangeStatePublisher::AutowareIvLaneChangeStatePublisher(rclcpp::Node & node) @@ -70,7 +72,7 @@ void AutowareIvLaneChangeStatePublisher::getLaneChangeReadyInfo( } void AutowareIvLaneChangeStatePublisher::getCandidatePathInfo( - const autoware_planning_msgs::msg::Path::ConstSharedPtr & path_ptr, + const autoware_auto_planning_msgs::msg::Path::ConstSharedPtr & path_ptr, autoware_api_msgs::msg::LaneChangeStatus * status) { if (!path_ptr) { @@ -79,7 +81,8 @@ void AutowareIvLaneChangeStatePublisher::getCandidatePathInfo( return; } - status->candidate_path = *path_ptr; + using autoware_iv_auto_msgs_converter::convert; + status->candidate_path = convert(*path_ptr); } } // namespace autoware_api diff --git a/awapi/awapi_awiv_adapter/src/awapi_obstacle_avoidance_state_publisher.cpp b/awapi/awapi_awiv_adapter/src/awapi_obstacle_avoidance_state_publisher.cpp index ed55bad5752d1..5f6c6e789a747 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_obstacle_avoidance_state_publisher.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_obstacle_avoidance_state_publisher.cpp @@ -14,6 +14,8 @@ #include "awapi_awiv_adapter/awapi_obstacle_avoidance_state_publisher.hpp" +#include "autoware_iv_auto_msgs_converter/autoware_iv_auto_msgs_converter.hpp" + namespace autoware_api { AutowareIvObstacleAvoidanceStatePublisher::AutowareIvObstacleAvoidanceStatePublisher( @@ -56,7 +58,7 @@ void AutowareIvObstacleAvoidanceStatePublisher::getObstacleAvoidReadyInfo( } void AutowareIvObstacleAvoidanceStatePublisher::getCandidatePathInfo( - const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr & path_ptr, + const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr & path_ptr, autoware_api_msgs::msg::ObstacleAvoidanceStatus * status) { if (!path_ptr) { @@ -65,7 +67,8 @@ void AutowareIvObstacleAvoidanceStatePublisher::getCandidatePathInfo( return; } - status->candidate_path = *path_ptr; + using autoware_iv_auto_msgs_converter::convert; + status->candidate_path = convert(*path_ptr); } } // namespace autoware_api diff --git a/awapi/awapi_awiv_adapter/src/awapi_vehicle_state_publisher.cpp b/awapi/awapi_awiv_adapter/src/awapi_vehicle_state_publisher.cpp index de8ac705bc963..d38e4dff171b2 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_vehicle_state_publisher.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_vehicle_state_publisher.cpp @@ -14,6 +14,8 @@ #include "awapi_awiv_adapter/awapi_vehicle_state_publisher.hpp" +#include "autoware_iv_auto_msgs_converter/autoware_iv_auto_msgs_converter.hpp" + #include #include @@ -41,8 +43,8 @@ void AutowareIvVehicleStatePublisher::statePublisher(const AutowareInfo & aw_inf getPoseInfo(aw_info.current_pose_ptr, &status); getSteerInfo(aw_info.steer_ptr, &status); getVehicleCmdInfo(aw_info.vehicle_cmd_ptr, &status); - getTurnSignalInfo(aw_info.turn_signal_ptr, &status); - getTwistInfo(aw_info.twist_ptr, &status); + getTurnSignalInfo(aw_info.turn_indicators_ptr, aw_info.hazard_lights_ptr, &status); + getTwistInfo(aw_info.odometry_ptr, &status); getGearInfo(aw_info.gear_ptr, &status); getBatteryInfo(aw_info.battery_ptr, &status); getGpsInfo(aw_info.nav_sat_ptr, &status); @@ -82,7 +84,7 @@ void AutowareIvVehicleStatePublisher::getPoseInfo( } void AutowareIvVehicleStatePublisher::getSteerInfo( - const autoware_vehicle_msgs::msg::Steering::ConstSharedPtr & steer_ptr, + const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr & steer_ptr, autoware_api_msgs::msg::AwapiVehicleStatus * status) { if (!steer_ptr) { @@ -91,16 +93,15 @@ void AutowareIvVehicleStatePublisher::getSteerInfo( } // get steer - status->steering = steer_ptr->data; + using autoware_iv_auto_msgs_converter::convert; + status->steering = convert(*steer_ptr).data; // get steer vel if (previous_steer_ptr_) { // calculate steer vel from steer - const double ds = steer_ptr->data - previous_steer_ptr_->data; + const double ds = steer_ptr->steering_tire_angle - previous_steer_ptr_->steering_tire_angle; const double dt = std::max( - (rclcpp::Time(steer_ptr->header.stamp) - rclcpp::Time(previous_steer_ptr_->header.stamp)) - .seconds(), - 1e-03); + (rclcpp::Time(steer_ptr->stamp) - rclcpp::Time(previous_steer_ptr_->stamp)).seconds(), 1e-03); const double steer_vel = ds / dt; // apply lowpass filter @@ -112,7 +113,7 @@ void AutowareIvVehicleStatePublisher::getSteerInfo( previous_steer_ptr_ = steer_ptr; } void AutowareIvVehicleStatePublisher::getVehicleCmdInfo( - const autoware_vehicle_msgs::msg::VehicleCommand::ConstSharedPtr & vehicle_cmd_ptr, + const autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr & vehicle_cmd_ptr, autoware_api_msgs::msg::AwapiVehicleStatus * status) { if (!vehicle_cmd_ptr) { @@ -121,44 +122,53 @@ void AutowareIvVehicleStatePublisher::getVehicleCmdInfo( } // get command - status->target_acceleration = vehicle_cmd_ptr->control.acceleration; - status->target_velocity = vehicle_cmd_ptr->control.velocity; - status->target_steering = vehicle_cmd_ptr->control.steering_angle; - status->target_steering_velocity = vehicle_cmd_ptr->control.steering_angle_velocity; + status->target_acceleration = vehicle_cmd_ptr->longitudinal.acceleration; + status->target_velocity = vehicle_cmd_ptr->longitudinal.speed; + status->target_steering = vehicle_cmd_ptr->lateral.steering_tire_angle; + status->target_steering_velocity = vehicle_cmd_ptr->lateral.steering_tire_rotation_rate; } void AutowareIvVehicleStatePublisher::getTurnSignalInfo( - const autoware_vehicle_msgs::msg::TurnSignal::ConstSharedPtr & turn_signal_ptr, + const autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr & turn_indicators_ptr, + const autoware_auto_vehicle_msgs::msg::HazardLightsReport::ConstSharedPtr & hazard_lights_ptr, autoware_api_msgs::msg::AwapiVehicleStatus * status) { - if (!turn_signal_ptr) { - RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *clock_, 5000 /* ms */, "turn signal is nullptr"); + if (!turn_indicators_ptr) { + RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *clock_, 5000 /* ms */, "turn indicators is nullptr"); + return; + } + + if (!hazard_lights_ptr) { + RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *clock_, 5000 /* ms */, "hazard lights is nullptr"); return; } // get turn signal - status->turn_signal = turn_signal_ptr->data; + using autoware_iv_auto_msgs_converter::convert; + status->turn_signal = convert(*turn_indicators_ptr, *hazard_lights_ptr).data; } void AutowareIvVehicleStatePublisher::getTwistInfo( - const geometry_msgs::msg::TwistStamped::ConstSharedPtr & twist_ptr, + const nav_msgs::msg::Odometry::ConstSharedPtr & odometry_ptr, autoware_api_msgs::msg::AwapiVehicleStatus * status) { - if (!twist_ptr) { - RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *clock_, 5000 /* ms */, "twist is nullptr"); + if (!odometry_ptr) { + RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *clock_, 5000 /* ms */, "odometry is nullptr"); return; } // get twist - status->velocity = twist_ptr->twist.linear.x; - status->angular_velocity = twist_ptr->twist.angular.z; + status->velocity = odometry_ptr->twist.twist.linear.x; + status->angular_velocity = odometry_ptr->twist.twist.angular.z; // get accel - if (previous_twist_ptr_) { + if (previous_odometry_ptr_) { // calculate acceleration from velocity - const double dv = twist_ptr->twist.linear.x - previous_twist_ptr_->twist.linear.x; + const double dv = + odometry_ptr->twist.twist.linear.x - previous_odometry_ptr_->twist.twist.linear.x; const double dt = std::max( - (rclcpp::Time(twist_ptr->header.stamp) - rclcpp::Time(previous_twist_ptr_->header.stamp)) + (rclcpp::Time(odometry_ptr->header.stamp) - + rclcpp::Time(previous_odometry_ptr_->header.stamp)) .seconds(), 1e-03); const double accel = dv / dt; @@ -168,11 +178,11 @@ void AutowareIvVehicleStatePublisher::getTwistInfo( prev_accel_ = lowpass_accel; status->acceleration = lowpass_accel; } - previous_twist_ptr_ = twist_ptr; + previous_odometry_ptr_ = odometry_ptr; } void AutowareIvVehicleStatePublisher::getGearInfo( - const autoware_vehicle_msgs::msg::ShiftStamped::ConstSharedPtr & gear_ptr, + const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr & gear_ptr, autoware_api_msgs::msg::AwapiVehicleStatus * status) { if (!gear_ptr) { @@ -181,7 +191,8 @@ void AutowareIvVehicleStatePublisher::getGearInfo( } // get gear (shift) - status->gear = gear_ptr->shift.data; + using autoware_iv_auto_msgs_converter::convert; + status->gear = convert(*gear_ptr).shift.data; } void AutowareIvVehicleStatePublisher::getBatteryInfo( From 0c20051bd2c79b4994b6aaa41ad377268aa5113c Mon Sep 17 00:00:00 2001 From: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Date: Fri, 12 Nov 2021 15:34:24 +0900 Subject: [PATCH 61/71] Use pacmod3 msgs (#610) * Migration to pacmod3 msgs Signed-off-by: wep21 * Fix action for pacmod Signed-off-by: wep21 * Run pre-commit Signed-off-by: wep21 Signed-off-by: tanaka3 --- .../awapi_autoware_state_publisher.hpp | 2 +- .../awapi_awiv_adapter/awapi_autoware_util.hpp | 8 ++++---- .../awapi_awiv_adapter/awapi_awiv_adapter_core.hpp | 12 ++++++------ .../include/awapi_awiv_adapter/awapi_pacmod_util.hpp | 10 +++++----- awapi/awapi_awiv_adapter/package.xml | 2 +- .../src/awapi_autoware_state_publisher.cpp | 2 +- .../src/awapi_awiv_adapter_core.cpp | 11 ++++++----- awapi/awapi_awiv_adapter/src/awapi_pacmod_util.cpp | 12 ++++++------ 8 files changed, 30 insertions(+), 29 deletions(-) diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.hpp b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.hpp index 1cb67baf907e8..f01494fa9a25d 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.hpp +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.hpp @@ -70,7 +70,7 @@ class AutowareIvAutowareStatePublisher void getErrorDiagInfo( const AutowareInfo & aw_info, autoware_api_msgs::msg::AwapiAutowareStatus * status); void getGlobalRptInfo( - const pacmod_msgs::msg::GlobalRpt::ConstSharedPtr & global_rpt_ptr, + const pacmod3_msgs::msg::GlobalRpt::ConstSharedPtr & global_rpt_ptr, autoware_api_msgs::msg::AwapiAutowareStatus * status); bool isGoal(const autoware_auto_system_msgs::msg::AutowareState::ConstSharedPtr & autoware_state); diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.hpp b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.hpp index 4e2559937ec60..cd4460de7c2d5 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.hpp +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.hpp @@ -41,8 +41,8 @@ #include #include #include -#include -#include +#include +#include #include #include @@ -74,7 +74,7 @@ struct AutowareInfo autoware_v2x_msgs::msg::InfrastructureCommandArray::ConstSharedPtr v2x_command_ptr; autoware_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr v2x_state_ptr; diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr diagnostic_ptr; - pacmod_msgs::msg::GlobalRpt::ConstSharedPtr global_rpt_ptr; + pacmod3_msgs::msg::GlobalRpt::ConstSharedPtr global_rpt_ptr; autoware_planning_msgs::msg::LaneChangeStatus::ConstSharedPtr lane_change_available_ptr; autoware_planning_msgs::msg::LaneChangeStatus::ConstSharedPtr lane_change_ready_ptr; autoware_auto_planning_msgs::msg::Path::ConstSharedPtr lane_change_candidate_ptr; @@ -84,7 +84,7 @@ struct AutowareInfo autoware_planning_msgs::msg::VelocityLimit::ConstSharedPtr current_max_velocity_ptr; autoware_api_msgs::msg::StopCommand::ConstSharedPtr temporary_stop_ptr; autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr autoware_planning_traj_ptr; - pacmod_msgs::msg::SystemRptInt::ConstSharedPtr door_state_ptr; + pacmod3_msgs::msg::SystemRptInt::ConstSharedPtr door_state_ptr; }; template diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.hpp b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.hpp index 14abb4ead25a1..f233b42212428 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.hpp +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.hpp @@ -43,7 +43,7 @@ #include #include #include -#include +#include #include #include @@ -85,7 +85,7 @@ class AutowareIvAdapter : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_v2x_state_; rclcpp::Subscription::SharedPtr sub_diagnostics_; - rclcpp::Subscription::SharedPtr sub_global_rpt_; + rclcpp::Subscription::SharedPtr sub_global_rpt_; rclcpp::Subscription::SharedPtr sub_lane_change_available_; rclcpp::Subscription::SharedPtr @@ -102,10 +102,10 @@ class AutowareIvAdapter : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_temporary_stop_; rclcpp::Subscription::SharedPtr sub_autoware_traj_; rclcpp::Subscription::SharedPtr sub_door_control_; - rclcpp::Subscription::SharedPtr sub_door_status_; + rclcpp::Subscription::SharedPtr sub_door_status_; // publisher - rclcpp::Publisher::SharedPtr pub_door_control_; + rclcpp::Publisher::SharedPtr pub_door_control_; rclcpp::Publisher::SharedPtr pub_door_status_; rclcpp::Publisher::SharedPtr pub_v2x_command_; rclcpp::Publisher::SharedPtr @@ -146,7 +146,7 @@ class AutowareIvAdapter : public rclcpp::Node void callbackV2XState( const autoware_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr msg_ptr); void callbackDiagnostics(const diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr msg_ptr); - void callbackGlobalRpt(const pacmod_msgs::msg::GlobalRpt::ConstSharedPtr msg_ptr); + void callbackGlobalRpt(const pacmod3_msgs::msg::GlobalRpt::ConstSharedPtr msg_ptr); void callbackLaneChangeAvailable( const autoware_planning_msgs::msg::LaneChangeStatus::ConstSharedPtr msg_ptr); void callbackLaneChangeReady( @@ -165,7 +165,7 @@ class AutowareIvAdapter : public rclcpp::Node const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg_ptr); void callbackDoorControl( const autoware_api_msgs::msg::DoorControlCommand::ConstSharedPtr msg_ptr); - void callbackDoorStatus(const pacmod_msgs::msg::SystemRptInt::ConstSharedPtr msg_ptr); + void callbackDoorStatus(const pacmod3_msgs::msg::SystemRptInt::ConstSharedPtr msg_ptr); // timer function void timerCallback(); diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_pacmod_util.hpp b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_pacmod_util.hpp index 29891f6246d36..c5bbcf8b5600e 100644 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_pacmod_util.hpp +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_pacmod_util.hpp @@ -19,18 +19,18 @@ #include #include -#include -#include +#include +#include namespace autoware_api { namespace pacmod_util { autoware_api_msgs::msg::DoorStatus getDoorStatusMsg( - const pacmod_msgs::msg::SystemRptInt::ConstSharedPtr & msg_ptr); -pacmod_msgs::msg::SystemCmdInt createClearOverrideDoorCommand( + const pacmod3_msgs::msg::SystemRptInt::ConstSharedPtr & msg_ptr); +pacmod3_msgs::msg::SystemCmdInt createClearOverrideDoorCommand( const rclcpp::Clock::SharedPtr & clock); -pacmod_msgs::msg::SystemCmdInt createDoorCommand( +pacmod3_msgs::msg::SystemCmdInt createDoorCommand( const rclcpp::Clock::SharedPtr & clock, const autoware_api_msgs::msg::DoorControlCommand::ConstSharedPtr & msg_ptr); } // namespace pacmod_util diff --git a/awapi/awapi_awiv_adapter/package.xml b/awapi/awapi_awiv_adapter/package.xml index ded85d12dd671..17e1bb5c27dd3 100644 --- a/awapi/awapi_awiv_adapter/package.xml +++ b/awapi/awapi_awiv_adapter/package.xml @@ -22,7 +22,7 @@ diagnostic_msgs geometry_msgs nav_msgs - pacmod_msgs + pacmod3_msgs rclcpp sensor_msgs std_msgs diff --git a/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp b/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp index 07dc39ee48838..5df89494b0623 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp @@ -261,7 +261,7 @@ void AutowareIvAutowareStatePublisher::getErrorDiagInfo( } void AutowareIvAutowareStatePublisher::getGlobalRptInfo( - const pacmod_msgs::msg::GlobalRpt::ConstSharedPtr & global_rpt_ptr, + const pacmod3_msgs::msg::GlobalRpt::ConstSharedPtr & global_rpt_ptr, autoware_api_msgs::msg::AwapiAutowareStatus * status) { if (!global_rpt_ptr) { diff --git a/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp b/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp index 19d7a2409a6e1..4621e903e208b 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp @@ -50,7 +50,7 @@ AutowareIvAdapter::AutowareIvAdapter() // publisher pub_door_control_ = - this->create_publisher("output/door_control", 1); + this->create_publisher("output/door_control", 1); pub_door_status_ = this->create_publisher("output/door_status", 1); pub_v2x_command_ = this->create_publisher( @@ -101,7 +101,7 @@ AutowareIvAdapter::AutowareIvAdapter() "input/v2x_state", 100, std::bind(&AutowareIvAdapter::callbackV2XState, this, _1)); sub_diagnostics_ = this->create_subscription( "input/diagnostics", 1, std::bind(&AutowareIvAdapter::callbackDiagnostics, this, _1)); - sub_global_rpt_ = this->create_subscription( + sub_global_rpt_ = this->create_subscription( "input/global_rpt", 1, std::bind(&AutowareIvAdapter::callbackGlobalRpt, this, _1)); sub_lane_change_available_ = this->create_subscription( @@ -132,7 +132,7 @@ AutowareIvAdapter::AutowareIvAdapter() std::bind(&AutowareIvAdapter::callbackAutowareTrajectory, this, _1)); sub_door_control_ = this->create_subscription( "input/door_control", 1, std::bind(&AutowareIvAdapter::callbackDoorControl, this, _1)); - sub_door_status_ = this->create_subscription( + sub_door_status_ = this->create_subscription( "input/door_status", 1, std::bind(&AutowareIvAdapter::callbackDoorStatus, this, _1)); // timer @@ -298,7 +298,8 @@ void AutowareIvAdapter::callbackDiagnostics( aw_info_.diagnostic_ptr = msg_ptr; } -void AutowareIvAdapter::callbackGlobalRpt(const pacmod_msgs::msg::GlobalRpt::ConstSharedPtr msg_ptr) +void AutowareIvAdapter::callbackGlobalRpt( + const pacmod3_msgs::msg::GlobalRpt::ConstSharedPtr msg_ptr) { aw_info_.global_rpt_ptr = msg_ptr; } @@ -375,7 +376,7 @@ void AutowareIvAdapter::callbackDoorControl( } void AutowareIvAdapter::callbackDoorStatus( - const pacmod_msgs::msg::SystemRptInt::ConstSharedPtr msg_ptr) + const pacmod3_msgs::msg::SystemRptInt::ConstSharedPtr msg_ptr) { aw_info_.door_state_ptr = msg_ptr; } diff --git a/awapi/awapi_awiv_adapter/src/awapi_pacmod_util.cpp b/awapi/awapi_awiv_adapter/src/awapi_pacmod_util.cpp index 95aef96962631..fd219d888b9de 100644 --- a/awapi/awapi_awiv_adapter/src/awapi_pacmod_util.cpp +++ b/awapi/awapi_awiv_adapter/src/awapi_pacmod_util.cpp @@ -19,10 +19,10 @@ namespace autoware_api namespace pacmod_util { autoware_api_msgs::msg::DoorStatus getDoorStatusMsg( - const pacmod_msgs::msg::SystemRptInt::ConstSharedPtr & msg_ptr) + const pacmod3_msgs::msg::SystemRptInt::ConstSharedPtr & msg_ptr) { using autoware_api_msgs::msg::DoorStatus; - using pacmod_msgs::msg::SystemRptInt; + using pacmod3_msgs::msg::SystemRptInt; DoorStatus door_status; if (!msg_ptr) { @@ -48,21 +48,21 @@ autoware_api_msgs::msg::DoorStatus getDoorStatusMsg( return door_status; } -pacmod_msgs::msg::SystemCmdInt createClearOverrideDoorCommand( +pacmod3_msgs::msg::SystemCmdInt createClearOverrideDoorCommand( const rclcpp::Clock::SharedPtr & clock) { - pacmod_msgs::msg::SystemCmdInt door_cmd; + pacmod3_msgs::msg::SystemCmdInt door_cmd; door_cmd.header.frame_id = "base_link"; door_cmd.header.stamp = clock->now(); door_cmd.clear_override = true; return door_cmd; } -pacmod_msgs::msg::SystemCmdInt createDoorCommand( +pacmod3_msgs::msg::SystemCmdInt createDoorCommand( const rclcpp::Clock::SharedPtr & clock, const autoware_api_msgs::msg::DoorControlCommand::ConstSharedPtr & msg_ptr) { - using pacmod_msgs::msg::SystemCmdInt; + using pacmod3_msgs::msg::SystemCmdInt; SystemCmdInt door_cmd; door_cmd.header.frame_id = "base_link"; From 4dc8c04804216c570f7038e2fc12d0a964363983 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Mon, 15 Nov 2021 14:10:17 +0900 Subject: [PATCH 62/71] fix package.xml (#630) Co-authored-by: Takayuki Murooka Signed-off-by: tanaka3 --- awapi/awapi_awiv_adapter/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/awapi/awapi_awiv_adapter/package.xml b/awapi/awapi_awiv_adapter/package.xml index 17e1bb5c27dd3..b4158d6581de7 100644 --- a/awapi/awapi_awiv_adapter/package.xml +++ b/awapi/awapi_awiv_adapter/package.xml @@ -15,6 +15,7 @@ autoware_auto_system_msgs autoware_auto_vehicle_msgs autoware_control_msgs + autoware_external_api_msgs autoware_iv_auto_msgs_converter autoware_planning_msgs autoware_v2x_msgs From 6d77c5fb3e83e7615c789595e0b33204b95feea6 Mon Sep 17 00:00:00 2001 From: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> Date: Tue, 16 Nov 2021 11:49:27 +0900 Subject: [PATCH 63/71] adapt to actuation cmd/status as control msg (#646) * adapt to actuation cmd/status as control msg * fix readme * fix topics * fix remaing topics * as to pacmod interface * fix vehicle status * add header to twist * revert gyro_odometer_change * revert twist topic change * revert unchanged package Signed-off-by: tanaka3 --- awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml b/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml index ec0f8fb5cb996..777346c5097d1 100644 --- a/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml +++ b/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml @@ -5,7 +5,7 @@ - + From 28b6935a5b75c8bc90ad1050efbc97c657b0ba2e Mon Sep 17 00:00:00 2001 From: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> Date: Wed, 17 Nov 2021 20:34:53 +0900 Subject: [PATCH 64/71] fix awapi topics (#685) Signed-off-by: tanaka3 --- .../launch/awapi_awiv_adapter.launch.xml | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml b/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml index 777346c5097d1..9554f8c5fb4ff 100644 --- a/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml +++ b/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml @@ -6,11 +6,11 @@ - - - + + + - + From 0249a6d0d81df5a544267f9d0e8078c598807719 Mon Sep 17 00:00:00 2001 From: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> Date: Thu, 18 Nov 2021 14:58:30 +0900 Subject: [PATCH 65/71] fix mixed route topic (#695) Signed-off-by: tanaka3 --- .../awapi_awiv_adapter/launch/awapi_relay_container.launch.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/awapi/awapi_awiv_adapter/launch/awapi_relay_container.launch.py b/awapi/awapi_awiv_adapter/launch/awapi_relay_container.launch.py index 1819259ca2e12..9e845912375ab 100644 --- a/awapi/awapi_awiv_adapter/launch/awapi_relay_container.launch.py +++ b/awapi/awapi_awiv_adapter/launch/awapi_relay_container.launch.py @@ -34,7 +34,7 @@ def generate_launch_description(): { "input_topic": LaunchConfiguration("input_route"), "output_topic": LaunchConfiguration("get_route"), - "type": "autoware_planning_msgs/msg/Route", + "type": "autoware_auto_planning_msgs/msg/HADMapRoute", } ], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], @@ -170,7 +170,7 @@ def generate_launch_description(): { "input_topic": LaunchConfiguration("set_route"), "output_topic": LaunchConfiguration("output_route"), - "type": "autoware_planning_msgs/msg/Route", + "type": "autoware_auto_planning_msgs/msg/HADMapRoute", "durability": "transient_local", } ], From a891e565ba5c1c9bcd7d034e69363df5ca2917bf Mon Sep 17 00:00:00 2001 From: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> Date: Thu, 18 Nov 2021 18:38:42 +0900 Subject: [PATCH 66/71] Fix/psim topics emergency handler awapi (#702) * fix emergency handler * fix awapi * remove unused topic * remove duplecated vehicle cmd Signed-off-by: tanaka3 --- awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml b/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml index 9554f8c5fb4ff..04123b76fe334 100644 --- a/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml +++ b/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml @@ -22,7 +22,7 @@ - + @@ -37,7 +37,7 @@ - + From de4dc168ffd7f2ebd2ab79592c3b8ccf8907d495 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" <43976882+isamu-takagi@users.noreply.github.com> Date: Thu, 18 Nov 2021 19:00:02 +0900 Subject: [PATCH 67/71] Fix topic type of awapi relay (#701) * Fix topic type of awapi relay * Fix Signed-off-by: tanaka3 --- .../launch/awapi_relay_container.launch.py | 12 ++++++------ awapi/awapi_awiv_adapter/package.xml | 1 + 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/awapi/awapi_awiv_adapter/launch/awapi_relay_container.launch.py b/awapi/awapi_awiv_adapter/launch/awapi_relay_container.launch.py index 9e845912375ab..b132447327e06 100644 --- a/awapi/awapi_awiv_adapter/launch/awapi_relay_container.launch.py +++ b/awapi/awapi_awiv_adapter/launch/awapi_relay_container.launch.py @@ -51,7 +51,7 @@ def generate_launch_description(): { "input_topic": LaunchConfiguration("input_object"), "output_topic": LaunchConfiguration("get_predicted_object"), - "type": "autoware_perception_msgs/msg/DynamicObjectArray", + "type": "autoware_auto_perception_msgs/msg/PredictedObjects", } ], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], @@ -68,7 +68,7 @@ def generate_launch_description(): { "input_topic": LaunchConfiguration("input_nearest_traffic_light_state"), "output_topic": LaunchConfiguration("get_nearest_traffic_light_status"), - "type": "autoware_perception_msgs/msg/LookingTrafficLightState", + "type": "autoware_auto_perception_msgs/msg/LookingTrafficSignal", } ], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], @@ -136,7 +136,7 @@ def generate_launch_description(): { "input_topic": LaunchConfiguration("set_engage"), "output_topic": LaunchConfiguration("output_autoware_engage"), - "type": "autoware_vehicle_msgs/msg/Engage", + "type": "autoware_auto_vehicle_msgs/msg/Engage", } ], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], @@ -153,7 +153,7 @@ def generate_launch_description(): { "input_topic": LaunchConfiguration("set_engage"), "output_topic": LaunchConfiguration("output_vehicle_engage"), - "type": "autoware_vehicle_msgs/msg/Engage", + "type": "autoware_auto_vehicle_msgs/msg/Engage", } ], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], @@ -290,7 +290,7 @@ def generate_launch_description(): { "input_topic": LaunchConfiguration("input_traffic_light_state"), "output_topic": LaunchConfiguration("get_traffic_light_status"), - "type": "autoware_perception_msgs/msg/TrafficLightStateArray", + "type": "autoware_auto_perception_msgs/msg/TrafficSignalArray", } ], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], @@ -307,7 +307,7 @@ def generate_launch_description(): { "input_topic": LaunchConfiguration("set_overwrite_traffic_light_state"), "output_topic": LaunchConfiguration("output_overwrite_traffic_light_state"), - "type": "autoware_perception_msgs/msg/TrafficLightStateArray", + "type": "autoware_auto_perception_msgs/msg/TrafficSignalArray", } ], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], diff --git a/awapi/awapi_awiv_adapter/package.xml b/awapi/awapi_awiv_adapter/package.xml index b4158d6581de7..93aa82c06b4c8 100644 --- a/awapi/awapi_awiv_adapter/package.xml +++ b/awapi/awapi_awiv_adapter/package.xml @@ -30,6 +30,7 @@ tf2 tf2_geometry_msgs + autoware_auto_perception_msgs autoware_perception_msgs topic_tools From 700cab59a78921f55c8f542a824ca75af92b6f12 Mon Sep 17 00:00:00 2001 From: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> Date: Fri, 19 Nov 2021 15:38:33 +0900 Subject: [PATCH 68/71] fix door cmd topic (#711) * fix door cmd topic * fix topic Signed-off-by: tanaka3 --- awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml b/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml index 04123b76fe334..e42a458fa78f2 100644 --- a/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml +++ b/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml @@ -58,7 +58,7 @@ - + From 59f1d05155d946f375e1a5ca1224ec0c13230c26 Mon Sep 17 00:00:00 2001 From: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Date: Thu, 25 Nov 2021 13:37:37 +0900 Subject: [PATCH 69/71] Update traffic light topic name (#729) * Update traffic light topic name Signed-off-by: wep21 * Update traffic light topic name in perception Signed-off-by: wep21 Signed-off-by: tanaka3 --- awapi/awapi_awiv_adapter/Readme.md | 48 ++++++++++--------- .../launch/awapi_awiv_adapter.launch.xml | 12 ++--- .../launch/awapi_relay_container.launch.py | 18 +++---- 3 files changed, 40 insertions(+), 38 deletions(-) diff --git a/awapi/awapi_awiv_adapter/Readme.md b/awapi/awapi_awiv_adapter/Readme.md index ee487398f4b78..7c0904a820a08 100644 --- a/awapi/awapi_awiv_adapter/Readme.md +++ b/awapi/awapi_awiv_adapter/Readme.md @@ -104,33 +104,33 @@ | | bool | obstacle_avoidance_ready | True when obstacle avoidance is ready | | | | autoware_planning_msgs/Trajectory | candidate_path | according to autoware_planning_msgs/Trajectory | Msg type is different from lane change candidate path | -### /awapi/traffic_light/get/status +### /awapi/traffic_light/get/traffic_signals - get recognition result of traffic light -- MessageType: autoware_api_msgs/TrafficLightStateArray +- MessageType: autoware_auto_perception_msgs/msg/TrafficSignalArray -| ✓ | type | name | unit | note | -| --- | :--------------------------------------- | :--- | :--- | :--- | -| | autoware_api_msgs/TrafficLightStateArray | | | | +| ✓ | type | name | unit | note | +| --- | :--------------------------------------------------- | :--- | :--- | :--- | +| | autoware_auto_perception_msgs/msg/TrafficSignalArray | | | | -### /awapi/traffic_light/get/nearest_traffic_light_status +### /awapi/traffic_light/get/nearest_traffic_signal - get recognition result of nearest traffic light -- MessageType: autoware_perception_msgs/LookingTrafficLightState +- MessageType: autoware_auto_perception_msgs/LookingTrafficSignal -| | type | name | unit | note | -| --- | :-------------------------------------------------- | :--------- | :--- | :------------------------------------------------------------ | -| | std_msgs/Header | header | | | -| | autoware_perception_msgs/TrafficLightStateWithJudge | perception | | traffic light information from autoware perception module | -| | autoware_perception_msgs/TrafficLightStateWithJudge | external | | traffic light information from external tool/module | -| | autoware_perception_msgs/TrafficLightStateWithJudge | final | | traffic light information used by the planning module finally | +| | type | name | unit | note | +| --- | :--------------------------------------------------- | :--------- | :--- | :------------------------------------------------------------ | +| | std_msgs/Header | header | | | +| | autoware_auto_perception_msgs/TrafficSignalWithJudge | perception | | traffic light information from autoware perception module | +| | autoware_auto_perception_msgs/TrafficSignalWithJudge | external | | traffic light information from external tool/module | +| | autoware_auto_perception_msgs/TrafficSignalWithJudge | final | | traffic light information used by the planning module finally | -- The contents of TrafficLightStateWithJudge.msg is following. +- The contents of TrafficSignalWithJudge.msg is following. -| | type | name | unit | note | -| --- | :----------------------------------------- | :---- | :------------------- | :------------------------------------------------------------- | -| | autoware_perception_msgs/TrafficLightState | state | | traffic light color/arrow | -| | uint8 | judge | 0:NONE, 1:STOP, 2:GO | go/stop judgment based on the color/arrow of the traffic light | +| | type | name | unit | note | +| --- | :------------------------------------------ | :----- | :------------------- | :------------------------------------------------------------- | +| | autoware_auto_perception_msgs/TrafficSignal | signal | | traffic light color/arrow | +| | uint8 | judge | 0:NONE, 1:STOP, 2:GO | go/stop judgment based on the color/arrow of the traffic light | ### /awapi/vehicle/get/door @@ -245,14 +245,14 @@ | ✓ | type | name | unit | note | | --- | :--- | :--- | :--- | :--- | -### /awapi/traffic_light/put/traffic_light_status +### /awapi/traffic_light/put/traffic_signals - Overwrite the recognition result of traffic light -- MessageType: autoware_perception_msgs/TrafficLightStateArray +- MessageType: autoware_auto_perception_msgs/TrafficSignalArray -| ✓ | type | name | unit | note | -| --- | :---------------------------------------------- | :--- | :--- | :--- | -| | autoware_perception_msgs/TrafficLightStateArray | | | | +| ✓ | type | name | unit | note | +| --- | :----------------------------------------------- | :--- | :--- | :--- | +| | autoware_auto_perception_msgs/TrafficSignalArray | | | | ### /awapi/vehicle/put/door @@ -295,3 +295,5 @@ | ✓ | type | name | unit | note | | --- | :------------------------------------- | :--- | :--- | :--- | | | autoware_planning_msgs/ExpandStopRange | | | | + +--- diff --git a/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml b/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml index e42a458fa78f2..4446491b11131 100644 --- a/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml +++ b/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml @@ -33,8 +33,8 @@ - - + + @@ -61,7 +61,7 @@ - + @@ -79,7 +79,7 @@ - + @@ -90,8 +90,8 @@ - - + + diff --git a/awapi/awapi_awiv_adapter/launch/awapi_relay_container.launch.py b/awapi/awapi_awiv_adapter/launch/awapi_relay_container.launch.py index b132447327e06..07e9b982c8fa9 100644 --- a/awapi/awapi_awiv_adapter/launch/awapi_relay_container.launch.py +++ b/awapi/awapi_awiv_adapter/launch/awapi_relay_container.launch.py @@ -62,12 +62,12 @@ def generate_launch_description(): ComposableNode( package="topic_tools", plugin="topic_tools::RelayNode", - name="nearest_traffic_light_relay", + name="nearest_traffic_signal_relay", namespace="awapi", parameters=[ { - "input_topic": LaunchConfiguration("input_nearest_traffic_light_state"), - "output_topic": LaunchConfiguration("get_nearest_traffic_light_status"), + "input_topic": LaunchConfiguration("input_nearest_traffic_signal"), + "output_topic": LaunchConfiguration("get_nearest_traffic_signal"), "type": "autoware_auto_perception_msgs/msg/LookingTrafficSignal", } ], @@ -284,12 +284,12 @@ def generate_launch_description(): ComposableNode( package="topic_tools", plugin="topic_tools::RelayNode", - name="traffic_light_relay", + name="traffic_signal_relay", namespace="awapi", parameters=[ { - "input_topic": LaunchConfiguration("input_traffic_light_state"), - "output_topic": LaunchConfiguration("get_traffic_light_status"), + "input_topic": LaunchConfiguration("input_traffic_signals"), + "output_topic": LaunchConfiguration("get_traffic_signals"), "type": "autoware_auto_perception_msgs/msg/TrafficSignalArray", } ], @@ -301,12 +301,12 @@ def generate_launch_description(): ComposableNode( package="topic_tools", plugin="topic_tools::RelayNode", - name="overwrite_traffic_light_state_relay", + name="overwrite_traffic_signals_relay", namespace="awapi", parameters=[ { - "input_topic": LaunchConfiguration("set_overwrite_traffic_light_state"), - "output_topic": LaunchConfiguration("output_overwrite_traffic_light_state"), + "input_topic": LaunchConfiguration("set_overwrite_traffic_signals"), + "output_topic": LaunchConfiguration("output_overwrite_traffic_signals"), "type": "autoware_auto_perception_msgs/msg/TrafficSignalArray", } ], From 83a781c69ed8378a7b7acda116d94fb09a3d14ef Mon Sep 17 00:00:00 2001 From: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Date: Fri, 26 Nov 2021 12:39:34 +0900 Subject: [PATCH 70/71] Update twist topic name (#736) Signed-off-by: wep21 Signed-off-by: tanaka3 --- awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml b/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml index 4446491b11131..5038612f87644 100644 --- a/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml +++ b/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml @@ -9,7 +9,7 @@ - + From 7655026f8d8c744e2673480a0d3ff81ead61cf34 Mon Sep 17 00:00:00 2001 From: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> Date: Fri, 3 Dec 2021 13:56:28 +0900 Subject: [PATCH 71/71] fix: refactor readme Co-authored-by: Takeshi Miura <57553950+1222-takeshi@users.noreply.github.com> --- awapi/awapi_awiv_adapter/Readme.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/awapi/awapi_awiv_adapter/Readme.md b/awapi/awapi_awiv_adapter/Readme.md index 7c0904a820a08..de876912a370a 100644 --- a/awapi/awapi_awiv_adapter/Readme.md +++ b/awapi/awapi_awiv_adapter/Readme.md @@ -1,4 +1,4 @@ -# AWAPI_AWIV_ADAPTER +# awapi_awiv_adapter ✓: confirmed, (blank): not confirmed