Skip to content

Commit

Permalink
Added linters to turn_signal_decider (autowarefoundation#155)
Browse files Browse the repository at this point in the history
* Added linters to turn_signal_decider

* Removed duplicate dependencies

* Fix linting error
  • Loading branch information
esteve authored Dec 16, 2020
1 parent 859a002 commit 71462ca
Show file tree
Hide file tree
Showing 9 changed files with 62 additions and 36 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,8 @@ ament_auto_find_build_dependencies()
### Compile options
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(-Wall -Wextra -Wpedantic -Wno-unused-parameter -Werror)
Expand All @@ -16,4 +18,9 @@ find_package(Eigen3 REQUIRED)

ament_auto_add_executable(turn_signal_decider src/turn_signal_decider_node.cpp src/turn_signal_decider_core.cpp src/data_manager.cpp src/frenet_coordinate.cpp)

if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
ament_lint_auto_find_test_dependencies()
endif()

ament_auto_package()
Original file line number Diff line number Diff line change
Expand Up @@ -11,20 +11,20 @@
// 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 TURN_SIGNAL_DECIDER__DATA_MANAGER_HPP_
#define TURN_SIGNAL_DECIDER__DATA_MANAGER_HPP_

#include "rclcpp/rclcpp.hpp"

#include "geometry_msgs/msg/pose_stamped.hpp"
#include "tf2_ros/transform_listener.h"
#include <memory>

#include "autoware_lanelet2_msgs/msg/map_bin.hpp"
#include "autoware_planning_msgs/msg/path_with_lane_id.hpp"
#include "autoware_vehicle_msgs/msg/turn_signal.hpp"

#include "geometry_msgs/msg/pose_stamped.hpp"
#include "lanelet2_core/LaneletMap.h"
#include "lanelet2_routing/RoutingGraph.h"
#include "lanelet2_traffic_rules/TrafficRulesFactory.h"
#include "rclcpp/rclcpp.hpp"
#include "tf2_ros/transform_listener.h"

namespace turn_signal_decider
{
Expand Down Expand Up @@ -73,3 +73,4 @@ class DataManager
bool isDataReady() const;
};
} // namespace turn_signal_decider
#endif // TURN_SIGNAL_DECIDER__DATA_MANAGER_HPP_
Original file line number Diff line number Diff line change
Expand Up @@ -11,11 +11,13 @@
// 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 TURN_SIGNAL_DECIDER__FRENET_COORDINATE_HPP_
#define TURN_SIGNAL_DECIDER__FRENET_COORDINATE_HPP_

#include <vector>

#include "autoware_planning_msgs/msg/path_with_lane_id.hpp"
#include "geometry_msgs/msg/point.hpp"
#include <vector>

namespace turn_signal_decider
{
Expand All @@ -34,3 +36,4 @@ bool convertToFrenetCoordinate3d(
const std::vector<geometry_msgs::msg::Point> & linestring,
const geometry_msgs::msg::Point & search_point_geom, FrenetCoordinate3d * frenet_coordinate);
} // namespace turn_signal_decider
#endif // TURN_SIGNAL_DECIDER__FRENET_COORDINATE_HPP_
Original file line number Diff line number Diff line change
Expand Up @@ -11,19 +11,21 @@
// 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 TURN_SIGNAL_DECIDER__TURN_SIGNAL_DECIDER_HPP_
#define TURN_SIGNAL_DECIDER__TURN_SIGNAL_DECIDER_HPP_

#include "rclcpp/rclcpp.hpp"
#include <string>

#include "autoware_vehicle_msgs/msg/turn_signal.hpp"
#include "rclcpp/rclcpp.hpp"
#include "turn_signal_decider/data_manager.hpp"
#include "turn_signal_decider/frenet_coordinate.hpp"

namespace turn_signal_decider
{
struct TurnSignalParameters
{
double lane_change_search_distance; // TODO: change this to time based threshold
double lane_change_search_distance; // TODO(mitsudome-r): change this to time based threshold
double intersection_search_distance;
};

Expand Down Expand Up @@ -63,3 +65,4 @@ class TurnSignalDecider : public std::enable_shared_from_this<TurnSignalDecider>
TurnSignalDecider(const std::string & node_name, const rclcpp::NodeOptions & node_options);
};
} // namespace turn_signal_decider
#endif // TURN_SIGNAL_DECIDER__TURN_SIGNAL_DECIDER_HPP_
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,9 @@
<depend>std_msgs</depend>
<depend>tf2_ros</depend>

<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>

<export>
<build_type>ament_cmake</build_type>
</export>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -11,10 +11,11 @@
// 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 "turn_signal_decider/turn_signal_decider.hpp"
#include <memory>

#include "lanelet2_extension/utility/message_conversion.hpp"
#include "lanelet2_extension/utility/utilities.hpp"
#include "turn_signal_decider/turn_signal_decider.hpp"

using autoware_planning_msgs::msg::PathWithLaneId;

Expand Down Expand Up @@ -83,10 +84,8 @@ void DataManager::onVehiclePoseUpdate()
if (node_ != nullptr) {
try {
const auto current_time = node_->now();
const auto transform =
tf_buffer_->lookupTransform(
"map", "base_link", current_time, rclcpp::Duration::from_seconds(
0.1));
const auto transform = tf_buffer_->lookupTransform(
"map", "base_link", current_time, rclcpp::Duration::from_seconds(0.1));
vehicle_pose_.pose.position.x = transform.transform.translation.x;
vehicle_pose_.pose.position.y = transform.transform.translation.y;
vehicle_pose_.pose.position.z = transform.transform.translation.z;
Expand All @@ -103,7 +102,7 @@ void DataManager::onVehiclePoseUpdate()
RCLCPP_WARN_STREAM_THROTTLE(
node_->get_logger(), *node_->get_clock(), std::chrono::milliseconds(5000).count(),
ex.what());
} else { // if tf suddenly stops comming, then there must be something wrong.
} else { // if tf suddenly stops comming, then there must be something wrong.
RCLCPP_ERROR_STREAM_THROTTLE(
node_->get_logger(), *node_->get_clock(), std::chrono::milliseconds(5000).count(),
ex.what());
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,12 @@
// 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 <limits>
#include <vector>

#include "turn_signal_decider/frenet_coordinate.hpp"

#include "Eigen/Dense"

namespace
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,12 @@
// 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 <limits>
#include <memory>
#include <string>
#include <utility>

#include "turn_signal_decider/turn_signal_decider.hpp"

using autoware_planning_msgs::msg::PathWithLaneId;
Expand All @@ -29,8 +35,7 @@ double getDistance3d(const geometry_msgs::msg::Point & p1, const geometry_msgs::
namespace turn_signal_decider
{
TurnSignalDecider::TurnSignalDecider(
const std::string & node_name,
const rclcpp::NodeOptions & node_options)
const std::string & node_name, const rclcpp::NodeOptions & node_options)
: rclcpp::Node(node_name, node_options), data_(this)
{
// setup data manager
Expand All @@ -49,20 +54,18 @@ TurnSignalDecider::TurnSignalDecider(
"input/path_with_lane_id", rclcpp::QoS{1},
std::bind(&DataManager::onPathWithLaneId, &data_, _1));
map_subscription_ = this->create_subscription<autoware_lanelet2_msgs::msg::MapBin>(
"input/vector_map", rclcpp::QoS{1}.transient_local(), std::bind(&DataManager::onLaneletMap, &data_, _1));
"input/vector_map", rclcpp::QoS{1}.transient_local(),
std::bind(&DataManager::onLaneletMap, &data_, _1));

// get ROS parameters
parameters_.lane_change_search_distance = this->declare_parameter(
"lane_change_search_distance",
double(30));
parameters_.intersection_search_distance = this->declare_parameter(
"intersection_search_distance",
double(30));
parameters_.lane_change_search_distance =
this->declare_parameter("lane_change_search_distance", static_cast<double>(30));
parameters_.intersection_search_distance =
this->declare_parameter("intersection_search_distance", static_cast<double>(30));

// set publishers
turn_signal_publisher_ = this->create_publisher<TurnSignal>(
"output/turn_signal_cmd",
rclcpp::QoS{1});
turn_signal_publisher_ =
this->create_publisher<TurnSignal>("output/turn_signal_cmd", rclcpp::QoS{1});

constexpr double turn_signal_update_period = 0.1;
auto turn_signal_timer_callback = std::bind(&TurnSignalDecider::onTurnSignalTimer, this);
Expand Down Expand Up @@ -155,8 +158,8 @@ lanelet::routing::RelationType TurnSignalDecider::getRelation(

bool TurnSignalDecider::isChangingLane(
const autoware_planning_msgs::msg::PathWithLaneId & path,
const FrenetCoordinate3d & vehicle_pose_frenet,
TurnSignal * signal_state_ptr, double * distance_ptr) const
const FrenetCoordinate3d & vehicle_pose_frenet, TurnSignal * signal_state_ptr,
double * distance_ptr) const
{
if (signal_state_ptr == nullptr || distance_ptr == nullptr) {
RCLCPP_ERROR(this->get_logger(), "Given argument is nullptr.");
Expand Down Expand Up @@ -210,8 +213,8 @@ bool TurnSignalDecider::isChangingLane(

bool TurnSignalDecider::isTurning(
const autoware_planning_msgs::msg::PathWithLaneId & path,
const FrenetCoordinate3d & vehicle_pose_frenet,
TurnSignal * signal_state_ptr, double * distance_ptr) const
const FrenetCoordinate3d & vehicle_pose_frenet, TurnSignal * signal_state_ptr,
double * distance_ptr) const
{
if (signal_state_ptr == nullptr || distance_ptr == nullptr) {
RCLCPP_ERROR(this->get_logger(), "Given argument is nullptr.");
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -11,16 +11,18 @@
// 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 <memory>

#include "turn_signal_decider/turn_signal_decider.hpp"

int main(int argc, char * argv[])
{
rclcpp::init(argc, argv);

rclcpp::NodeOptions node_options;
auto decider = std::make_shared<turn_signal_decider::TurnSignalDecider>(
"turn_signal_decider",
node_options);
auto decider =
std::make_shared<turn_signal_decider::TurnSignalDecider>("turn_signal_decider", node_options);

rclcpp::spin(decider);

Expand Down

0 comments on commit 71462ca

Please sign in to comment.