From 497bdef834faa09dc74ca23db40e01fcaf16ed6c Mon Sep 17 00:00:00 2001 From: Toshiki Nakamura <82020865+ToshikiNakamura0412@users.noreply.github.com> Date: Tue, 23 Jul 2024 11:57:26 +0900 Subject: [PATCH] Feature road_publisher (#28) * Feature road_publisher * Add packages * Create .rosinstall * Update CI * Fix bug * Use cross product * Add pose callback * Add subscriber * Fix frame * Create header file --- .github/workflows/main.yml | 3 +- .rosinstall | 7 + CMakeLists.txt | 14 +- .../localmap_creator/virtual_road_projector.h | 126 +++++++++++++++ package.xml | 6 + src/virtual_road_projector.cpp | 150 ++++++++++++++++++ 6 files changed, 299 insertions(+), 7 deletions(-) create mode 100644 .rosinstall create mode 100644 include/localmap_creator/virtual_road_projector.h create mode 100644 src/virtual_road_projector.cpp diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index cecb0fd..85fa192 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -15,10 +15,11 @@ jobs: - name: Checkout uses: actions/checkout@v3 - name: Setup ROS - uses: ros-tooling/setup-ros@v0.6 + uses: ros-tooling/setup-ros@v0.7 with: required-ros-distributions: noetic - name: Build uses: ros-tooling/action-ros-ci@v0.3 with: target-ros1-distro: noetic + vcs-repo-file-url: .rosinstall diff --git a/.rosinstall b/.rosinstall new file mode 100644 index 0000000..a832d91 --- /dev/null +++ b/.rosinstall @@ -0,0 +1,7 @@ +# amsl repositories +- git: + local-name: amsl_navigation_managers + uri: https://github.com/amslabtech/amsl_navigation_managers.git + version: master + +# other repositories diff --git a/CMakeLists.txt b/CMakeLists.txt index 433837f..ec2311b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,32 +1,32 @@ cmake_minimum_required(VERSION 3.5) project(localmap_creator) -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) -endif() +add_compile_options(-std=c++17 -O2 -g) -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages find_package(catkin REQUIRED COMPONENTS roscpp rospy + geometry_msgs + nav_msgs std_msgs tf tf_conversions tf2 tf2_geometry_msgs + tf2_ros cv_bridge image_transport image_geometry laser_geometry pcl_ros + amsl_navigation_msgs ) ## System dependencies are found with CMake's conventions # find_package(Boost REQUIRED COMPONENTS system) find_package(OpenCV REQUIRED) find_package(PCL 1.8 REQUIRED) +find_package(Eigen3 REQUIRED COMPONENTS system) ## Uncomment this if the package has a setup.py. This macro ensures @@ -264,6 +264,8 @@ target_link_libraries(pc_extractor ${catkin_LIBRARIES} ${PCL_LIBRARIES} ) +add_executable(virtual_road_projector src/virtual_road_projector.cpp) +target_link_libraries(virtual_road_projector ${catkin_LIBRARIES}) if(CATKIN_ENABLE_TESTING) find_package(rostest REQUIRED) diff --git a/include/localmap_creator/virtual_road_projector.h b/include/localmap_creator/virtual_road_projector.h new file mode 100644 index 0000000..a0d38bc --- /dev/null +++ b/include/localmap_creator/virtual_road_projector.h @@ -0,0 +1,126 @@ +/** + * @file virtual_road_projector.h + * @author amsl + * @brief Virtual road projector class + * @copyright Copyright (c) 2024 + */ + +#ifndef LOCALMAP_CREATOR_VIRTUAL_ROAD_PROJECTOR_H +#define LOCALMAP_CREATOR_VIRTUAL_ROAD_PROJECTOR_H + +#include +#include +#include +#include +#include +#include + +#include + +#include "amsl_navigation_msgs/Road.h" + +/** + * @struct Param + * @brief Parameters for VirtualRoadProjector + */ +struct Param +{ + std::string global_frame_id; + int allowable_num_of_not_received; + float robot_radius; + float wall_thickness; +}; + +/** + * @class VirtualRoadProjector + * @brief Virtual road projector class + */ +class VirtualRoadProjector +{ +public: + /** + * @brief Construct a new Virtual Road Projector object + */ + VirtualRoadProjector(void); + +private: + /** + * @brief Callback function for map + * @param msg OccupancyGrid message + */ + void map_callback(const nav_msgs::OccupancyGridConstPtr &msg); + + /** + * @brief Callback function for pose + * @param msg PoseWithCovarianceStamped message + */ + void pose_callback(const geometry_msgs::PoseWithCovarianceStampedConstPtr &msg) { pose_ = *msg; } + + /** + * @brief Callback function for road information + * @param msg Road message + */ + void road_info_callback(const amsl_navigation_msgs::RoadConstPtr &msg); + + /** + * @brief Project road to map + * @param map OccupancyGrid message + * @param road Road message + * @return nav_msgs::OccupancyGrid Projected map + */ + nav_msgs::OccupancyGrid project_road(const nav_msgs::OccupancyGrid &map, amsl_navigation_msgs::Road road); + + /** + * @brief Check if the point is inside the road + * @param road Road message + * @param point Point message + * @return true if the point is inside the road + * @return false if the point is outside the road + */ + bool inside_road(const amsl_navigation_msgs::Road &road, const geometry_msgs::Point &point); + + /** + * @brief Convert index to point + * @param map OccupancyGrid message + * @param index Index + * @return geometry_msgs::Point Point message + */ + geometry_msgs::Point index_to_point(const nav_msgs::OccupancyGrid &map, const int index); + + /** + * @brief Check if the point is edge of the road + * @param point Point message + * @param road Road message + * @return true if the point is edge of the road + * @return false if the point is not edge of the road + */ + bool is_edge_of_road(const geometry_msgs::Point &point, const amsl_navigation_msgs::Road &road); + + /** + * @brief Calculate distance to the path + * @param edge_point0 Edge point 0 + * @param edge_point1 Edge point 1 + * @param target_point Target point + * @return float Distance to the path + */ + float calc_dist_to_path( + const geometry_msgs::Point &edge_point0, const geometry_msgs::Point &edge_point1, + const geometry_msgs::Point &target_point); + + Param param_; + bool road_updated_ = false; + int count_of_not_received_road_ = 0; + std::optional road_; + std::optional pose_; + + ros::NodeHandle nh_; + ros::NodeHandle private_nh_; + ros::Publisher map_pub_; + ros::Subscriber map_sub_; + ros::Subscriber pose_sub_; + ros::Subscriber road_sub_; + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; +}; + +#endif // LOCALMAP_CREATOR_VIRTUAL_ROAD_PROJECTOR_H diff --git a/package.xml b/package.xml index 7ed7db5..ca363e5 100644 --- a/package.xml +++ b/package.xml @@ -13,6 +13,8 @@ catkin roscpp rospy + geometry_msgs + nav_msgs std_msgs laser_geometry image_geometry @@ -20,10 +22,14 @@ tf tf2 tf2_geometry_msgs + tf2_ros tf_conversions cv_bridge libpcl-all-dev pcl_ros + eigen + + amsl_navigation_msgs diff --git a/src/virtual_road_projector.cpp b/src/virtual_road_projector.cpp new file mode 100644 index 0000000..09ee2f2 --- /dev/null +++ b/src/virtual_road_projector.cpp @@ -0,0 +1,150 @@ +/** + * @file virtual_road_projector.cpp + * @author amsl + * @brief Virtual road projector class + * @copyright Copyright (c) 2024 + */ + +#include "localmap_creator/virtual_road_projector.h" + +VirtualRoadProjector::VirtualRoadProjector(void) : private_nh_("~"), tf_listener_(tf_buffer_) +{ + private_nh_.param("global_frame_id", param_.global_frame_id, std::string("map")); + private_nh_.param("allowable_num_of_not_received", param_.allowable_num_of_not_received, 5); + private_nh_.param("robot_radius", param_.robot_radius, 0.3); + private_nh_.param("wall_thickness", param_.wall_thickness, 0.05); + + map_pub_ = nh_.advertise("/local_map/projected", 1); + map_sub_ = nh_.subscribe( + "/local_map", 1, &VirtualRoadProjector::map_callback, this, ros::TransportHints().reliable().tcpNoDelay()); + pose_sub_ = nh_.subscribe( + "/localized_pose", 1, &VirtualRoadProjector::pose_callback, this, ros::TransportHints().reliable().tcpNoDelay()); + road_sub_ = nh_.subscribe( + "/node_edge_map/road", 1, &VirtualRoadProjector::road_info_callback, this, + ros::TransportHints().reliable().tcpNoDelay()); +} + +void VirtualRoadProjector::map_callback(const nav_msgs::OccupancyGridConstPtr &msg) +{ + if (!road_updated_) + count_of_not_received_road_++; + if (param_.allowable_num_of_not_received < count_of_not_received_road_) + road_.reset(); + + if (!road_.has_value()) + map_pub_.publish(*msg); + else + map_pub_.publish(project_road(*msg, road_.value())); + + road_updated_ = false; +} + +void VirtualRoadProjector::road_info_callback(const amsl_navigation_msgs::RoadConstPtr &msg) +{ + road_ = *msg; + road_updated_ = true; + count_of_not_received_road_ = 0; +} + +nav_msgs::OccupancyGrid +VirtualRoadProjector::project_road(const nav_msgs::OccupancyGrid &map, amsl_navigation_msgs::Road road) +{ + nav_msgs::OccupancyGrid projected_map = map; + + // transform road points to global frame + geometry_msgs::TransformStamped transform_stamped; + while (ros::ok()) + { + try + { + transform_stamped = tf_buffer_.lookupTransform(map.header.frame_id, param_.global_frame_id, ros::Time(0)); + break; + } + catch (tf2::TransformException &ex) + { + ROS_WARN("%s", ex.what()); + ros::Duration(0.5).sleep(); + } + } + tf2::doTransform(road.point0, road.point0, transform_stamped); + tf2::doTransform(road.point1, road.point1, transform_stamped); + + if (pose_.has_value()) + { + tf2::doTransform(pose_.value().pose.pose.position, pose_.value().pose.pose.position, transform_stamped); + if (!inside_road(road, pose_.value().pose.pose.position)) + return map; + } + + // project road to map + for (int i = 0; i < projected_map.data.size(); i++) + { + const geometry_msgs::Point point = index_to_point(projected_map, i); + if (is_edge_of_road(point, road)) + { + if (hypot(point.x, point.y) <= param_.robot_radius) + return map; + projected_map.data[i] = 100; + } + } + + return projected_map; +} + +bool VirtualRoadProjector::inside_road(const amsl_navigation_msgs::Road &road, const geometry_msgs::Point &point) +{ + const Eigen::Vector3d reference_vector(road.point1.x - road.point0.x, road.point1.y - road.point0.y, 0.0); + const Eigen::Vector3d target_vector(point.x - road.point0.x, point.y - road.point0.y, 0.0); + const float dist_to_path = calc_dist_to_path(road.point0, road.point1, point); + if (reference_vector.cross(target_vector).z() < 0.0) + return dist_to_path < road.distance_to_right; + else + return dist_to_path < road.width - road.distance_to_right; +} + +geometry_msgs::Point VirtualRoadProjector::index_to_point(const nav_msgs::OccupancyGrid &map, const int index) +{ + geometry_msgs::Point point; + const int grid_index_x = index % map.info.width; + const int grid_index_y = static_cast(index / map.info.width); + point.x = grid_index_x * map.info.resolution + map.info.origin.position.x + map.info.resolution / 2.0; + point.y = grid_index_y * map.info.resolution + map.info.origin.position.y + map.info.resolution / 2.0; + + return point; +} + +bool VirtualRoadProjector::is_edge_of_road(const geometry_msgs::Point &point, const amsl_navigation_msgs::Road &road) +{ + const Eigen::Vector3d reference_vector(road.point1.x - road.point0.x, road.point1.y - road.point0.y, 0.0); + const Eigen::Vector3d target_vector(point.x - road.point0.x, point.y - road.point0.y, 0.0); + const float dist_to_path = calc_dist_to_path(road.point0, road.point1, point); + if (reference_vector.cross(target_vector).z() < 0.0) + { + return road.distance_to_right <= dist_to_path && dist_to_path < road.distance_to_right + param_.wall_thickness; + } + else + { + const float distance_to_left = road.width - road.distance_to_right; + return distance_to_left <= dist_to_path && dist_to_path < distance_to_left + param_.wall_thickness; + } +} + +float VirtualRoadProjector::calc_dist_to_path( + const geometry_msgs::Point &edge_point0, const geometry_msgs::Point &edge_point1, + const geometry_msgs::Point &target_point) +{ + const float a = edge_point1.y - edge_point0.y; + const float b = -(edge_point1.x - edge_point0.x); + const float c = -a * edge_point0.x - b * edge_point0.y; + + return fabs(a * target_point.x + b * target_point.y + c) / (hypot(a, b) + DBL_EPSILON); +} + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "road_projector"); + VirtualRoadProjector road_projector; + ros::spin(); + + return 0; +}