Skip to content

Commit

Permalink
fix(crosswalk_traffic_light_estimator): package rename
Browse files Browse the repository at this point in the history
Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
  • Loading branch information
satoshi-ota committed Jul 1, 2022
1 parent b4703dc commit 9daaafb
Show file tree
Hide file tree
Showing 9 changed files with 36 additions and 33 deletions.
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
cmake_minimum_required(VERSION 3.5)
project(traffic_light_estimator)
project(crosswalk_traffic_light_estimator)

find_package(autoware_cmake REQUIRED)
autoware_package()
Expand All @@ -10,13 +10,13 @@ autoware_package()

include_directories()

ament_auto_add_library(traffic_light_estimator SHARED
ament_auto_add_library(crosswalk_traffic_light_estimator SHARED
src/node.cpp
)

rclcpp_components_register_node(traffic_light_estimator
PLUGIN "traffic_light::TrafficLightEstimatorNode"
EXECUTABLE traffic_light_estimator_node
rclcpp_components_register_node(crosswalk_traffic_light_estimator
PLUGIN "traffic_light::CrosswalkTrafficLightEstimatorNode"
EXECUTABLE crosswalk_traffic_light_estimator_node
)

if(BUILD_TESTING)
Expand Down
Original file line number Diff line number Diff line change
@@ -1,8 +1,8 @@
# traffic_light_estimator
# crosswalk_traffic_light_estimator

## Purpose

`traffic_light_estimator` is a module that estimates pedestrian traffic signals from HDMap and detected vehicle traffic signals.
`crosswalk_traffic_light_estimator` is a module that estimates pedestrian traffic signals from HDMap and detected vehicle traffic signals.

## Inputs / Outputs

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -12,8 +12,8 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef TRAFFIC_LIGHT_ESTIMATOR__NODE_HPP_
#define TRAFFIC_LIGHT_ESTIMATOR__NODE_HPP_
#ifndef CROSSWALK_TRAFFIC_LIGHT_ESTIMATOR__NODE_HPP_
#define CROSSWALK_TRAFFIC_LIGHT_ESTIMATOR__NODE_HPP_

#include <lanelet2_extension/regulatory_elements/autoware_traffic_light.hpp>
#include <lanelet2_extension/utility/query.hpp>
Expand Down Expand Up @@ -50,10 +50,10 @@ using tier4_autoware_utils::DebugPublisher;
using tier4_autoware_utils::StopWatch;
using tier4_debug_msgs::msg::Float64Stamped;

class TrafficLightEstimatorNode : public rclcpp::Node
class CrosswalkTrafficLightEstimatorNode : public rclcpp::Node
{
public:
explicit TrafficLightEstimatorNode(const rclcpp::NodeOptions & options);
explicit CrosswalkTrafficLightEstimatorNode(const rclcpp::NodeOptions & options);

private:
rclcpp::Subscription<HADMapBin>::SharedPtr sub_map_;
Expand Down Expand Up @@ -104,4 +104,4 @@ class TrafficLightEstimatorNode : public rclcpp::Node

} // namespace traffic_light

#endif // TRAFFIC_LIGHT_ESTIMATOR__NODE_HPP_
#endif // CROSSWALK_TRAFFIC_LIGHT_ESTIMATOR__NODE_HPP_
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
<?xml version="1.0"?>
<launch>
<node pkg="traffic_light_estimator" exec="traffic_light_estimator_node" name="traffic_light_estimator" output="screen">
<node pkg="crosswalk_traffic_light_estimator" exec="crosswalk_traffic_light_estimator_node" name="crosswalk_traffic_light_estimator" output="screen">
<remap from="~/input/vector_map" to="/map/vector_map"/>
<remap from="~/input/route" to="/planning/mission_planning/route"/>
<remap from="~/input/classified/traffic_signals" to="classified/traffic_signals"/>
Expand Down
Original file line number Diff line number Diff line change
@@ -1,8 +1,8 @@
<?xml version="1.0"?>
<package format="2">
<name>traffic_light_estimator</name>
<name>crosswalk_traffic_light_estimator</name>
<version>0.1.0</version>
<description>The traffic_light_estimator package</description>
<description>The crosswalk_traffic_light_estimator package</description>

<maintainer email="satoshi.ota@tier4.jp">Satoshi Ota</maintainer>
<license>Apache License 2.0</license>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,7 @@
// 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 "traffic_light_estimator/node.hpp"
#include "crosswalk_traffic_light_estimator/node.hpp"

#include <lanelet2_extension/utility/message_conversion.hpp>

Expand Down Expand Up @@ -75,31 +75,32 @@ bool hasMergeLane(

} // namespace

TrafficLightEstimatorNode::TrafficLightEstimatorNode(const rclcpp::NodeOptions & options)
: Node("traffic_light_estimator", options)
CrosswalkTrafficLightEstimatorNode::CrosswalkTrafficLightEstimatorNode(
const rclcpp::NodeOptions & options)
: Node("crosswalk_traffic_light_estimator", options)
{
using std::placeholders::_1;

use_last_detect_color_ = this->declare_parameter("use_last_detect_color", true);

sub_map_ = create_subscription<HADMapBin>(
"~/input/vector_map", rclcpp::QoS{1}.transient_local(),
std::bind(&TrafficLightEstimatorNode::onMap, this, _1));
std::bind(&CrosswalkTrafficLightEstimatorNode::onMap, this, _1));
sub_route_ = create_subscription<HADMapRoute>(
"~/input/route", rclcpp::QoS{1}.transient_local(),
std::bind(&TrafficLightEstimatorNode::onRoute, this, _1));
std::bind(&CrosswalkTrafficLightEstimatorNode::onRoute, this, _1));
sub_traffic_light_array_ = create_subscription<TrafficSignalArray>(
"~/input/classified/traffic_signals", rclcpp::QoS{1},
std::bind(&TrafficLightEstimatorNode::onTrafficLightArray, this, _1));
std::bind(&CrosswalkTrafficLightEstimatorNode::onTrafficLightArray, this, _1));

pub_traffic_light_array_ =
this->create_publisher<TrafficSignalArray>("~/output/traffic_signals", rclcpp::QoS{1});
pub_processing_time_ = std::make_shared<DebugPublisher>(this, "~/debug");
}

void TrafficLightEstimatorNode::onMap(const HADMapBin::ConstSharedPtr msg)
void CrosswalkTrafficLightEstimatorNode::onMap(const HADMapBin::ConstSharedPtr msg)
{
RCLCPP_INFO(get_logger(), "[TrafficLightEstimatorNode]: Start loading lanelet");
RCLCPP_INFO(get_logger(), "[CrosswalkTrafficLightEstimatorNode]: Start loading lanelet");
lanelet_map_ptr_ = std::make_shared<lanelet::LaneletMap>();
lanelet::utils::conversion::fromBinMsg(
*msg, lanelet_map_ptr_, &traffic_rules_ptr_, &routing_graph_ptr_);
Expand All @@ -114,10 +115,10 @@ void TrafficLightEstimatorNode::onMap(const HADMapBin::ConstSharedPtr msg)
lanelet::routing::RoutingGraphContainer overall_graphs({vehicle_graph, pedestrian_graph});
overall_graphs_ptr_ =
std::make_shared<const lanelet::routing::RoutingGraphContainer>(overall_graphs);
RCLCPP_INFO(get_logger(), "[TrafficLightEstimatorNode]: Map is loaded");
RCLCPP_INFO(get_logger(), "[CrosswalkTrafficLightEstimatorNode]: Map is loaded");
}

void TrafficLightEstimatorNode::onRoute(const HADMapRoute::ConstSharedPtr msg)
void CrosswalkTrafficLightEstimatorNode::onRoute(const HADMapRoute::ConstSharedPtr msg)
{
if (lanelet_map_ptr_ == nullptr) {
RCLCPP_WARN(get_logger(), "cannot set traffic light in route because don't receive map");
Expand Down Expand Up @@ -148,7 +149,8 @@ void TrafficLightEstimatorNode::onRoute(const HADMapRoute::ConstSharedPtr msg)
}
}

void TrafficLightEstimatorNode::onTrafficLightArray(const TrafficSignalArray::ConstSharedPtr msg)
void CrosswalkTrafficLightEstimatorNode::onTrafficLightArray(
const TrafficSignalArray::ConstSharedPtr msg)
{
StopWatch<std::chrono::milliseconds> stop_watch;
stop_watch.tic("Total");
Expand All @@ -175,7 +177,7 @@ void TrafficLightEstimatorNode::onTrafficLightArray(const TrafficSignalArray::Co
return;
}

void TrafficLightEstimatorNode::updateLastDetectedSignal(
void CrosswalkTrafficLightEstimatorNode::updateLastDetectedSignal(
const lanelet::Id & id, const uint8_t color)
{
if (color == TrafficLight::UNKNOWN) {
Expand All @@ -189,7 +191,7 @@ void TrafficLightEstimatorNode::updateLastDetectedSignal(
last_detect_color_.at(id) = color;
}

void TrafficLightEstimatorNode::setCrosswalkTrafficSignal(
void CrosswalkTrafficLightEstimatorNode::setCrosswalkTrafficSignal(
const lanelet::ConstLanelet & crosswalk, const uint8_t color, TrafficSignalArray & msg) const
{
const auto tl_reg_elems = crosswalk.regulatoryElementsAs<const lanelet::TrafficLight>();
Expand All @@ -211,7 +213,7 @@ void TrafficLightEstimatorNode::setCrosswalkTrafficSignal(
}
}

lanelet::ConstLanelets TrafficLightEstimatorNode::getGreenLanelets(
lanelet::ConstLanelets CrosswalkTrafficLightEstimatorNode::getGreenLanelets(
const lanelet::ConstLanelets & lanelets,
const std::unordered_map<lanelet::Id, TrafficSignal> & traffic_light_id_map)
{
Expand Down Expand Up @@ -247,7 +249,7 @@ lanelet::ConstLanelets TrafficLightEstimatorNode::getGreenLanelets(
return green_lanelets;
}

uint8_t TrafficLightEstimatorNode::estimateCrosswalkTrafficSignal(
uint8_t CrosswalkTrafficLightEstimatorNode::estimateCrosswalkTrafficSignal(
const lanelet::ConstLanelet & crosswalk, const lanelet::ConstLanelets & green_lanelets) const
{
bool has_left_green_lane = false;
Expand Down Expand Up @@ -283,7 +285,7 @@ uint8_t TrafficLightEstimatorNode::estimateCrosswalkTrafficSignal(
: TrafficLight::UNKNOWN;
}

uint8_t TrafficLightEstimatorNode::getHighestConfidenceTrafficSignal(
uint8_t CrosswalkTrafficLightEstimatorNode::getHighestConfidenceTrafficSignal(
const lanelet::ConstLineStringsOrPolygons3d & traffic_lights,
const std::unordered_map<lanelet::Id, TrafficSignal> & traffic_light_id_map) const
{
Expand Down Expand Up @@ -318,7 +320,8 @@ uint8_t TrafficLightEstimatorNode::getHighestConfidenceTrafficSignal(
return ret;
}

uint8_t TrafficLightEstimatorNode::getLastDetectedTrafficSignal(const lanelet::Id & id) const
uint8_t CrosswalkTrafficLightEstimatorNode::getLastDetectedTrafficSignal(
const lanelet::Id & id) const
{
if (last_detect_color_.count(id) == 0) {
return TrafficLight::UNKNOWN;
Expand All @@ -330,4 +333,4 @@ uint8_t TrafficLightEstimatorNode::getLastDetectedTrafficSignal(const lanelet::I

#include <rclcpp_components/register_node_macro.hpp>

RCLCPP_COMPONENTS_REGISTER_NODE(traffic_light::TrafficLightEstimatorNode)
RCLCPP_COMPONENTS_REGISTER_NODE(traffic_light::CrosswalkTrafficLightEstimatorNode)

0 comments on commit 9daaafb

Please sign in to comment.