diff --git a/autoware/core_planning/waypoint_planner/include/waypoint_planner/velocity_set/velocity_set_info.h b/autoware/core_planning/waypoint_planner/include/waypoint_planner/velocity_set/velocity_set_info.h index a42bcc5..3cdfcd7 100644 --- a/autoware/core_planning/waypoint_planner/include/waypoint_planner/velocity_set/velocity_set_info.h +++ b/autoware/core_planning/waypoint_planner/include/waypoint_planner/velocity_set/velocity_set_info.h @@ -43,6 +43,8 @@ class VelocitySetInfo double velocity_change_limit_; // (m/s) double temporal_waypoints_size_; // (meter) int wpidx_detectionResultByOtherNodes_; // waypoints index@finalwaypoints + // TODO : light_brake + std_msgs::Int32 light_status_; // ROS param double remove_points_upto_; @@ -65,9 +67,15 @@ class VelocitySetInfo void localizerPoseCallback(const geometry_msgs::PoseStampedConstPtr &msg); void detectionCallback(const std_msgs::Int32 &msg); + //light brake + void light_brakeCallback(const std_msgs::Int32 &msg); void clearPoints(); - + //light brake + std_msgs::Int32 getLightStatus() const + { + return light_status_; + } int getDetectionResultByOtherNodes() const { return wpidx_detectionResultByOtherNodes_; diff --git a/autoware/core_planning/waypoint_planner/src/velocity_set/velocity_set_info.cpp b/autoware/core_planning/waypoint_planner/src/velocity_set/velocity_set_info.cpp index d84e9e1..3eeac4d 100644 --- a/autoware/core_planning/waypoint_planner/src/velocity_set/velocity_set_info.cpp +++ b/autoware/core_planning/waypoint_planner/src/velocity_set/velocity_set_info.cpp @@ -129,3 +129,11 @@ void VelocitySetInfo::localizerPoseCallback(const geometry_msgs::PoseStampedCons health_checker_ptr_->CHECK_RATE("topic_rate_localizer_pose_slow", 8, 5, 1, "topic localizer_pose subscribe rate slow."); localizer_pose_ = *msg; } + +/** + * @brief light dectection + */ +void VelocitySetInfo::light_brakeCallback(const std_msgs::Int32 &msg) +{ + light_status_ = msg; +} \ No newline at end of file