Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

use autoware auto msgs #27

Draft
wants to merge 2 commits into
base: ros2
Choose a base branch
from
Draft

use autoware auto msgs #27

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
b171004
use autoware auto msgs
yukkysaito Nov 15, 2021
d58b784
fix bug
tkimura4 Nov 16, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 2 additions & 2 deletions velodyne_pointcloud/include/velodyne_pointcloud/func.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,7 @@

#include <pcl/point_cloud.h>

#include <geometry_msgs/msg/twist_stamped.hpp>
#include <autoware_auto_vehicle_msgs/msg/velocity_report.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

#include <velodyne_pointcloud/point_types.h>
Expand All @@ -44,7 +44,7 @@ pcl::PointCloud<velodyne_pointcloud::PointXYZIRADT>::Ptr extractInvalidNearPoint

pcl::PointCloud<velodyne_pointcloud::PointXYZIRADT>::Ptr interpolate(
const pcl::PointCloud<velodyne_pointcloud::PointXYZIRADT>::ConstPtr & input_pointcloud,
const std::deque<geometry_msgs::msg::TwistStamped> & twist_queue,
const std::deque<autoware_auto_vehicle_msgs::msg::VelocityReport> & velocity_report_queue,
const tf2::Transform & tf2_base_link_to_sensor);

pcl::PointCloud<velodyne_pointcloud::PointXYZIRADT>::Ptr sortRingNumber(
Expand Down
10 changes: 5 additions & 5 deletions velodyne_pointcloud/include/velodyne_pointcloud/interpolate.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -21,15 +21,15 @@
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/transform_listener.h>

#include <geometry_msgs/msg/twist_stamped.hpp>
#include <autoware_auto_vehicle_msgs/msg/velocity_report.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <visualization_msgs/msg/marker_array.hpp>

#include <velodyne_pointcloud/pointcloudXYZIRADT.h>

namespace velodyne_pointcloud
{
class Interpolate : public rclcpp::Node
class Interpolate : public rclcpp::Node
{
public:
Interpolate(const rclcpp::NodeOptions & options);
Expand All @@ -41,20 +41,20 @@ class Interpolate : public rclcpp::Node

void processPoints(
const sensor_msgs::msg::PointCloud2::SharedPtr points_xyziradt);
void processTwist(const geometry_msgs::msg::TwistStamped::SharedPtr twist_msg);
void processVelocityReport(const autoware_auto_vehicle_msgs::msg::VelocityReport::SharedPtr velocity_report_msg);
bool getTransform(
const std::string & target_frame, const std::string & source_frame,
tf2::Transform * tf2_transform_ptr);

rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr velodyne_points_ex_sub_;
rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr twist_sub_;
rclcpp::Subscription<autoware_auto_vehicle_msgs::msg::VelocityReport>::SharedPtr velocity_report_sub_;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr velodyne_points_interpolate_pub_;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr velodyne_points_interpolate_ex_pub_;

tf2::BufferCore tf2_buffer_;
tf2_ros::TransformListener tf2_listener_;

std::deque<geometry_msgs::msg::TwistStamped> twist_queue_;
std::deque<autoware_auto_vehicle_msgs::msg::VelocityReport> velocity_report_queue_;

std::string base_link_frame_;
};
Expand Down
1 change: 1 addition & 0 deletions velodyne_pointcloud/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,7 @@

<depend>angles</depend>
<depend>diagnostic_updater</depend>
<depend>autoware_auto_vehicle_msgs</depend>
<depend>geometry_msgs</depend>
<depend>libpcl-all-dev</depend>
<depend>message_filters</depend>
Expand Down
32 changes: 18 additions & 14 deletions velodyne_pointcloud/src/conversions/func.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -161,16 +161,16 @@ pcl::PointCloud<velodyne_pointcloud::PointXYZIRADT>::Ptr extractInvalidNearPoint

pcl::PointCloud<velodyne_pointcloud::PointXYZIRADT>::Ptr interpolate(
const pcl::PointCloud<velodyne_pointcloud::PointXYZIRADT>::ConstPtr & input_pointcloud,
const std::deque<geometry_msgs::msg::TwistStamped> & twist_queue,
const std::deque<autoware_auto_vehicle_msgs::msg::VelocityReport> & velocity_report_queue,
const tf2::Transform & tf2_base_link_to_sensor)
{
pcl::PointCloud<velodyne_pointcloud::PointXYZIRADT>::Ptr output_pointcloud(
new pcl::PointCloud<velodyne_pointcloud::PointXYZIRADT>);
output_pointcloud->reserve(input_pointcloud->points.size());

if (input_pointcloud->points.empty() || twist_queue.empty()) {
if (input_pointcloud->points.empty() || velocity_report_queue.empty()) {
auto ros_clock = rclcpp::Clock(RCL_ROS_TIME);
RCLCPP_WARN_STREAM_THROTTLE(rclcpp::get_logger("velodyne_interpolate"), ros_clock, 10000 /* ms */, "input_pointcloud->points or twist_queue is empty.");
RCLCPP_WARN_STREAM_THROTTLE(rclcpp::get_logger("velodyne_interpolate"), ros_clock, 10000 /* ms */, "input_pointcloud->points or velocity_report_queue is empty.");
*output_pointcloud = *input_pointcloud;
return output_pointcloud;
}
Expand All @@ -179,25 +179,29 @@ pcl::PointCloud<velodyne_pointcloud::PointXYZIRADT>::Ptr interpolate(
float x = 0, y = 0;
double prev_time_stamp = input_pointcloud->points.front().time_stamp;

auto twist_it = std::lower_bound(
std::begin(twist_queue), std::end(twist_queue),
auto velocity_report_it = std::lower_bound(
std::begin(velocity_report_queue), std::end(velocity_report_queue),
rclcpp::Time(input_pointcloud->points.front().time_stamp, RCL_ROS_TIME),
[](const geometry_msgs::msg::TwistStamped & x, rclcpp::Time t) { return rclcpp::Time(x.header.stamp) < t; });
twist_it = twist_it == std::end(twist_queue) ? std::end(twist_queue) - 1 : twist_it;
[](const autoware_auto_vehicle_msgs::msg::VelocityReport & x, rclcpp::Time t) {
return rclcpp::Time(x.header.stamp) < t;
});
velocity_report_it = velocity_report_it == std::end(velocity_report_queue) ? std::end(velocity_report_queue) - 1 : velocity_report_it;

const tf2::Transform tf2_base_link_to_sensor_inv = tf2_base_link_to_sensor.inverse();
for (const auto & p : input_pointcloud->points) {
for (; (twist_it != std::end(twist_queue) - 1 && p.time_stamp > rclcpp::Time(twist_it->header.stamp).seconds());
++twist_it) {
// std::cout << std::fixed << p.time_stamp << " " << rclcpp::Time(twist_it->header.stamp).seconds() << std::endl;
for (;
(velocity_report_it != std::end(velocity_report_queue) - 1 &&
p.time_stamp > rclcpp::Time(velocity_report_it->header.stamp).seconds());
++velocity_report_it) {
// std::cout << std::fixed << p.time_stamp << " " << rclcpp::Time(velocity_report_it->stamp).seconds() << std::endl;
}

float v = twist_it->twist.linear.x;
float w = twist_it->twist.angular.z;
float v = velocity_report_it->longitudinal_velocity;
float w = velocity_report_it->heading_rate;

if (std::fabs(p.time_stamp - rclcpp::Time(twist_it->header.stamp).seconds()) > 0.1) {
if (std::fabs(p.time_stamp - rclcpp::Time(velocity_report_it->header.stamp).seconds()) > 0.1) {
auto ros_clock = rclcpp::Clock(RCL_ROS_TIME);
RCLCPP_WARN_STREAM_THROTTLE(rclcpp::get_logger("velodyne_interpolate"), ros_clock, 10000 /* ms */, "Twist time_stamp is too late. Cloud not interpolate.");
RCLCPP_WARN_STREAM_THROTTLE(rclcpp::get_logger("velodyne_interpolate"), ros_clock, 10000 /* ms */, "velocity_report time_stamp is too late. Cloud not interpolate.");
v = 0;
w = 0;
}
Expand Down
25 changes: 15 additions & 10 deletions velodyne_pointcloud/src/conversions/interpolate.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -29,22 +29,27 @@ Interpolate::Interpolate(const rclcpp::NodeOptions & options)
this->create_publisher<sensor_msgs::msg::PointCloud2>("velodyne_points_interpolate_ex", rclcpp::SensorDataQoS());

// subscribe
twist_sub_ = this->create_subscription<geometry_msgs::msg::TwistStamped>(
"/vehicle/status/twist", 10, std::bind(&Interpolate::processTwist, this, std::placeholders::_1));
velocity_report_sub_ = this->create_subscription<autoware_auto_vehicle_msgs::msg::VelocityReport>(
"/vehicle/status/velocity_status", 10,
std::bind(&Interpolate::processVelocityReport, this, std::placeholders::_1));
velodyne_points_ex_sub_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
"velodyne_points_ex", rclcpp::SensorDataQoS(), std::bind(&Interpolate::processPoints,this, std::placeholders::_1));
}

void Interpolate::processTwist(const geometry_msgs::msg::TwistStamped::SharedPtr twist_msg)
void Interpolate::processVelocityReport(const autoware_auto_vehicle_msgs::msg::VelocityReport::SharedPtr velocity_report_msg)
{
twist_queue_.push_back(*twist_msg);
velocity_report_queue_.push_back(*velocity_report_msg);

while (!twist_queue_.empty()) {
while (!velocity_report_queue_.empty()) {
//for replay rosbag
if (rclcpp::Time(twist_queue_.front().header.stamp) > rclcpp::Time(twist_msg->header.stamp)) {
twist_queue_.pop_front();
} else if (rclcpp::Time(twist_queue_.front().header.stamp) < rclcpp::Time(twist_msg->header.stamp) - rclcpp::Duration::from_seconds(1.0)) {
twist_queue_.pop_front();
if (
rclcpp::Time(velocity_report_queue_.front().header.stamp) >
rclcpp::Time(velocity_report_msg->header.stamp)) {
velocity_report_queue_.pop_front();
} else if (
rclcpp::Time(velocity_report_queue_.front().header.stamp) <
rclcpp::Time(velocity_report_msg->header.stamp) - rclcpp::Duration::from_seconds(1.0)) {
velocity_report_queue_.pop_front();
} else {
break;
}
Expand All @@ -68,7 +73,7 @@ void Interpolate::processPoints(
new pcl::PointCloud<velodyne_pointcloud::PointXYZIRADT>);
tf2::Transform tf2_base_link_to_sensor;
getTransform(points_xyziradt->header.frame_id, base_link_frame_, &tf2_base_link_to_sensor);
interpolate_points_xyziradt = interpolate(points_xyziradt, twist_queue_, tf2_base_link_to_sensor);
interpolate_points_xyziradt = interpolate(points_xyziradt, velocity_report_queue_, tf2_base_link_to_sensor);

if (velodyne_points_interpolate_pub_->get_subscription_count() > 0) {
const auto interpolate_points_xyzir = convert(interpolate_points_xyziradt);
Expand Down