Skip to content

Commit

Permalink
Feature road_publisher (#28)
Browse files Browse the repository at this point in the history
* Feature road_publisher

* Add packages

* Create .rosinstall

* Update CI

* Fix bug

* Use cross product

* Add pose callback

* Add subscriber

* Fix frame

* Create header file
  • Loading branch information
ToshikiNakamura0412 authored Jul 23, 2024
1 parent 969bdf1 commit 497bdef
Show file tree
Hide file tree
Showing 6 changed files with 299 additions and 7 deletions.
3 changes: 2 additions & 1 deletion .github/workflows/main.yml
Original file line number Diff line number Diff line change
Expand Up @@ -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
7 changes: 7 additions & 0 deletions .rosinstall
Original file line number Diff line number Diff line change
@@ -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
14 changes: 8 additions & 6 deletions CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -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
Expand Down Expand Up @@ -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)
Expand Down
126 changes: 126 additions & 0 deletions include/localmap_creator/virtual_road_projector.h
Original file line number Diff line number Diff line change
@@ -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 <geometry_msgs/PoseWithCovarianceStamped.h>
#include <nav_msgs/OccupancyGrid.h>
#include <optional>
#include <ros/ros.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/transform_listener.h>

#include <Eigen/Dense>

#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<amsl_navigation_msgs::Road> road_;
std::optional<geometry_msgs::PoseWithCovarianceStamped> 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
6 changes: 6 additions & 0 deletions package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -13,17 +13,23 @@
<buildtool_depend>catkin</buildtool_depend>
<depend>roscpp</depend>
<depend>rospy</depend>
<depend>geometry_msgs</depend>
<depend>nav_msgs</depend>
<depend>std_msgs</depend>
<depend>laser_geometry</depend>
<depend>image_geometry</depend>
<depend>image_transport</depend>
<depend>tf</depend>
<depend>tf2</depend>
<depend>tf2_geometry_msgs</depend>
<depend>tf2_ros</depend>
<depend>tf_conversions</depend>
<depend>cv_bridge</depend>
<depend>libpcl-all-dev</depend>
<depend>pcl_ros</depend>
<depend>eigen</depend>

<build_depend>amsl_navigation_msgs</build_depend>

<!-- The export tag contains other, unspecified, tags -->
<export>
Expand Down
150 changes: 150 additions & 0 deletions src/virtual_road_projector.cpp
Original file line number Diff line number Diff line change
@@ -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<std::string>("global_frame_id", param_.global_frame_id, std::string("map"));
private_nh_.param<int>("allowable_num_of_not_received", param_.allowable_num_of_not_received, 5);
private_nh_.param<float>("robot_radius", param_.robot_radius, 0.3);
private_nh_.param<float>("wall_thickness", param_.wall_thickness, 0.05);

map_pub_ = nh_.advertise<nav_msgs::OccupancyGrid>("/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<int>(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;
}

0 comments on commit 497bdef

Please sign in to comment.