From b20e6b2fab8de2efec55fb050be6a241139e8277 Mon Sep 17 00:00:00 2001 From: Mateusz Przybyla Date: Wed, 19 Jul 2017 21:07:27 +0200 Subject: [PATCH] Removed scan_rectifier in favor of using laser_geometry in scans_merger --- CMakeLists.txt | 20 +- include/obstacle_detector/scan_rectifier.h | 93 --------- include/obstacle_detector/scans_merger.h | 18 +- nodelet_plugins.xml | 8 - package.xml | 1 + src/nodelets/scan_rectifier_nodelet.cpp | 67 ------ src/nodelets/scans_merger_nodelet.cpp | 2 +- src/nodes/scan_rectifier_node.cpp | 55 ----- src/nodes/scans_merger_node.cpp | 2 +- src/scan_rectifier.cpp | 229 --------------------- src/scans_merger.cpp | 177 ++++++---------- 11 files changed, 85 insertions(+), 587 deletions(-) delete mode 100644 include/obstacle_detector/scan_rectifier.h delete mode 100644 src/nodelets/scan_rectifier_nodelet.cpp delete mode 100644 src/nodes/scan_rectifier_node.cpp delete mode 100644 src/scan_rectifier.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 6356a7f..e10ed96 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,7 +3,7 @@ project(obstacle_detector) set(CMAKE_CXX_FLAGS "-std=c++11 -fpermissive ${CMAKE_CXX_FLAGS} -Wfatal-errors\ ") -find_package(catkin REQUIRED COMPONENTS roscpp roslaunch nodelet rviz std_msgs nav_msgs geometry_msgs sensor_msgs std_srvs tf message_generation) +find_package(catkin REQUIRED COMPONENTS roscpp roslaunch nodelet rviz std_msgs nav_msgs geometry_msgs sensor_msgs std_srvs tf laser_geometry message_generation) find_package(Armadillo REQUIRED) find_package(Boost 1.54.0 REQUIRED system) @@ -12,8 +12,8 @@ generate_messages(DEPENDENCIES std_msgs geometry_msgs) catkin_package( INCLUDE_DIRS include - LIBRARIES scan_rectifier scans_merger obstacle_extractor obstacle_tracker obstacle_publisher ${PROJECT_NAME}_nodelets ${PROJECT_NAME}_gui - CATKIN_DEPENDS roscpp nodelet rviz std_msgs nav_msgs geometry_msgs sensor_msgs std_srvs tf message_runtime + LIBRARIES scans_merger obstacle_extractor obstacle_tracker obstacle_publisher ${PROJECT_NAME}_nodelets ${PROJECT_NAME}_gui + CATKIN_DEPENDS roscpp nodelet rviz std_msgs nav_msgs geometry_msgs sensor_msgs std_srvs tf laser_geometry message_runtime ) include_directories(include ${catkin_INCLUDE_DIRS} ${ARMADILLO_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) @@ -21,10 +21,6 @@ include_directories(include ${catkin_INCLUDE_DIRS} ${ARMADILLO_INCLUDE_DIRS} ${B # # Build libs # -add_library(scan_rectifier src/scan_rectifier.cpp) -target_link_libraries(scan_rectifier ${catkin_LIBRARIES}) -add_dependencies(scan_rectifier ${catkin_EXPORTED_TARGETS}) - add_library(scans_merger src/scans_merger.cpp) target_link_libraries(scans_merger ${catkin_LIBRARIES}) add_dependencies(scans_merger ${catkin_EXPORTED_TARGETS}) @@ -44,9 +40,6 @@ add_dependencies(obstacle_publisher ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME} # # Build nodes # -add_executable(scan_rectifier_node src/nodes/scan_rectifier_node.cpp) -target_link_libraries(scan_rectifier_node scan_rectifier) - add_executable(scans_merger_node src/nodes/scans_merger_node.cpp) target_link_libraries(scans_merger_node scans_merger) @@ -63,12 +56,11 @@ target_link_libraries(obstacle_publisher_node obstacle_publisher) # Build nodelets # add_library(${PROJECT_NAME}_nodelets - src/nodelets/scan_rectifier_nodelet.cpp src/nodelets/scans_merger_nodelet.cpp src/nodelets/obstacle_extractor_nodelet.cpp src/nodelets/obstacle_tracker_nodelet.cpp src/nodelets/obstacle_publisher_nodelet.cpp) -target_link_libraries(${PROJECT_NAME}_nodelets scan_rectifier scans_merger obstacle_extractor obstacle_tracker obstacle_publisher) +target_link_libraries(${PROJECT_NAME}_nodelets scans_merger obstacle_extractor obstacle_tracker obstacle_publisher) # # Build rviz plugins @@ -102,7 +94,7 @@ add_dependencies(${PROJECT_NAME}_gui ${catkin_EXPORTED_TARGETS}) # # Install libraries # -install(TARGETS scan_rectifier scans_merger obstacle_extractor obstacle_tracker obstacle_publisher ${PROJECT_NAME}_nodelets ${PROJECT_NAME}_gui +install(TARGETS scans_merger obstacle_extractor obstacle_tracker obstacle_publisher ${PROJECT_NAME}_nodelets ${PROJECT_NAME}_gui ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) @@ -110,7 +102,7 @@ install(TARGETS scan_rectifier scans_merger obstacle_extractor obstacle_tracker # # Install nodes # -install(TARGETS scan_rectifier_node scans_merger_node obstacle_extractor_node obstacle_tracker_node obstacle_publisher_node +install(TARGETS scans_merger_node obstacle_extractor_node obstacle_tracker_node obstacle_publisher_node RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) # diff --git a/include/obstacle_detector/scan_rectifier.h b/include/obstacle_detector/scan_rectifier.h deleted file mode 100644 index 3627279..0000000 --- a/include/obstacle_detector/scan_rectifier.h +++ /dev/null @@ -1,93 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Poznan University of Technology - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Poznan University of Technology nor the names - * of its contributors may be used to endorse or promote products - * derived from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/* - * Author: Mateusz Przybyla - */ - -#pragma once - -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -namespace obstacle_detector -{ - -class ScanRectifier -{ -public: - ScanRectifier(ros::NodeHandle& nh, ros::NodeHandle& nh_local); - -private: - bool updateParams(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res); - void scanCallback(const sensor_msgs::LaserScan::ConstPtr scan); - void odomCallback(const nav_msgs::Odometry::ConstPtr odom_msg); - - void initialize() { std_srvs::Empty empt; updateParams(empt.request, empt.response); } - - ros::NodeHandle nh_; - ros::NodeHandle nh_local_; - - ros::Subscriber scan_sub_; - ros::Subscriber odom_sub_; - ros::Publisher scan_pub_; - ros::ServiceServer params_srv_; - - tf::TransformListener tf_ls_; - laser_geometry::LaserProjection projector_; - - boost::circular_buffer odoms_; - geometry_msgs::Pose2D scanner_in_base_tf_; - - std::vector ranges_; - std::vector points_; - - // Parameters - bool p_active_; - - double p_scan_rate_; - double p_odom_rate_; - - int p_odom2scan_ratio_; - - std::string p_robot_frame_; - std::string p_scanner_frame_; -}; - -} // namespace obstacle_detector diff --git a/include/obstacle_detector/scans_merger.h b/include/obstacle_detector/scans_merger.h index edad6c4..d22887b 100644 --- a/include/obstacle_detector/scans_merger.h +++ b/include/obstacle_detector/scans_merger.h @@ -41,6 +41,7 @@ #include #include #include +#include namespace obstacle_detector { @@ -57,9 +58,7 @@ class ScansMerger void initialize() { std_srvs::Empty empt; updateParams(empt.request, empt.response); } - void publishScan(); - void publishPCL(); - void publishAll(); + void publishMessages(); ros::NodeHandle nh_; ros::NodeHandle nh_local_; @@ -71,18 +70,16 @@ class ScansMerger ros::Publisher scan_pub_; ros::Publisher pcl_pub_; + tf::TransformListener tf_ls_; + laser_geometry::LaserProjection projector_; + bool front_scan_received_; bool rear_scan_received_; bool front_scan_error_; bool rear_scan_error_; - ros::Time stamp_; - - geometry_msgs::Pose2D front_tf_; - geometry_msgs::Pose2D rear_tf_; - - std::vector ranges_; - std::vector points_; + sensor_msgs::PointCloud front_pcl_; + sensor_msgs::PointCloud rear_pcl_; // Parameters bool p_active_; @@ -98,6 +95,7 @@ class ScansMerger double p_max_y_range_; double p_min_y_range_; + std::string p_fixed_frame_id_; std::string p_target_frame_id_; std::string p_front_scan_frame_id_; std::string p_rear_scan_frame_id_; diff --git a/nodelet_plugins.xml b/nodelet_plugins.xml index b9c17e2..63550af 100644 --- a/nodelet_plugins.xml +++ b/nodelet_plugins.xml @@ -1,13 +1,5 @@ - - - Scan Rectifier Nodelet - - - diff --git a/package.xml b/package.xml index f774997..bf18d37 100644 --- a/package.xml +++ b/package.xml @@ -20,6 +20,7 @@ nav_msgs sensor_msgs geometry_msgs + laser_geometry std_srvs diff --git a/src/nodelets/scan_rectifier_nodelet.cpp b/src/nodelets/scan_rectifier_nodelet.cpp deleted file mode 100644 index 1745d43..0000000 --- a/src/nodelets/scan_rectifier_nodelet.cpp +++ /dev/null @@ -1,67 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Poznan University of Technology - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Poznan University of Technology nor the names - * of its contributors may be used to endorse or promote products - * derived from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/* - * Author: Mateusz Przybyla - */ - -#include -#include - -#include "obstacle_detector/scan_rectifier.h" - -namespace obstacle_detector -{ - -class ScanRectifierNodelet : public nodelet::Nodelet -{ -public: - virtual void onInit() { - NODELET_INFO("[Scan Rectifier]: Initializing Nodelet"); - ros::NodeHandle nh = getNodeHandle(); - ros::NodeHandle nh_local = getPrivateNodeHandle(); - - try { - scan_rectifier_ = std::shared_ptr(new ScanRectifier(nh, nh_local)); - } - catch(std::string s) { - NODELET_ERROR_STREAM(s); - } - } - -private: - std::shared_ptr scan_rectifier_; -}; - -} // namespace obstacle_detector - -#include -PLUGINLIB_EXPORT_CLASS(obstacle_detector::ScanRectifierNodelet, nodelet::Nodelet) diff --git a/src/nodelets/scans_merger_nodelet.cpp b/src/nodelets/scans_merger_nodelet.cpp index f7e786d..37dddbb 100644 --- a/src/nodelets/scans_merger_nodelet.cpp +++ b/src/nodelets/scans_merger_nodelet.cpp @@ -52,7 +52,7 @@ class ScansMergerNodelet : public nodelet::Nodelet try { scans_merger_ = std::shared_ptr(new ScansMerger(nh, nh_local)); } - catch (std::string s) { + catch (const char* s) { NODELET_FATAL_STREAM(s); } } diff --git a/src/nodes/scan_rectifier_node.cpp b/src/nodes/scan_rectifier_node.cpp deleted file mode 100644 index b57d529..0000000 --- a/src/nodes/scan_rectifier_node.cpp +++ /dev/null @@ -1,55 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Poznan University of Technology - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Poznan University of Technology nor the names - * of its contributors may be used to endorse or promote products - * derived from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/* - * Author: Mateusz Przybyla - */ - -#include "obstacle_detector/scan_rectifier.h" - -using namespace obstacle_detector; - -int main(int argc, char** argv) { - ros::init(argc, argv, "scan_rectifier", ros::init_options::NoRosout); - ros::NodeHandle nh(""); - ros::NodeHandle nh_local("~"); - - try { - ROS_INFO("[Scan Rectifier]: Initializing Node"); - ScanRectifier sr(nh, nh_local); - ros::spin(); - } - catch(const char* s) { - ROS_ERROR_STREAM(s); - } - - return 0; -} diff --git a/src/nodes/scans_merger_node.cpp b/src/nodes/scans_merger_node.cpp index 60e71cd..16f3da7 100644 --- a/src/nodes/scans_merger_node.cpp +++ b/src/nodes/scans_merger_node.cpp @@ -47,7 +47,7 @@ int main(int argc, char** argv) { ScansMerger sm(nh, nh_local); ros::spin(); } - catch (std::string s) { + catch (const char* s) { ROS_FATAL_STREAM(s); } diff --git a/src/scan_rectifier.cpp b/src/scan_rectifier.cpp deleted file mode 100644 index aefdab3..0000000 --- a/src/scan_rectifier.cpp +++ /dev/null @@ -1,229 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Poznan University of Technology - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Poznan University of Technology nor the names - * of its contributors may be used to endorse or promote products - * derived from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/* - * Author: Mateusz Przybyla - */ - -#include "obstacle_detector/scan_rectifier.h" - -using namespace obstacle_detector; -using namespace std; - -ScanRectifier::ScanRectifier(ros::NodeHandle& nh, ros::NodeHandle& nh_local) : nh_(nh), nh_local_(nh_local) { - p_active_ = false; - params_srv_ = nh_local_.advertiseService("params", &ScanRectifier::updateParams, this); - -// ROS_INFO_STREAM("[Scan Rectifier]: Waiting for first " << p_odom2scan_ratio_ << " odometry messages."); -// boost::shared_ptr first_odom; -// for (int i = 0; i < p_odom2scan_ratio_; ++i) { -// first_odom = ros::topic::waitForMessage("odom", nh_); -// odoms_.push_back(*first_odom); -// } -// ROS_INFO_STREAM("[Scan Rectifier]: Acquired first " << p_odom2scan_ratio_ << " odometry messages."); - -// ROS_INFO_STREAM("[Scan Rectifier]: Waiting for first odometry message."); -// boost::shared_ptr first_odom; -// first_odom = ros::topic::waitForMessage("odom", nh_); -// odoms_.push_back(*first_odom); -// ROS_INFO_STREAM("[Scan Rectifier]: Acquired first odometry message."); - -// ROS_INFO_STREAM("[Scan Rectifier]: Waiting for first scan message."); -// boost::shared_ptr first_scan; -// first_scan = ros::topic::waitForMessage("scan", nh_); -// scan_prototype_ = *first_scan; -// num_ranges_ = scan_prototype_.ranges.size(); -// scan_prototype_.ranges.clear(); -// ROS_INFO_STREAM("[Scan Rectifier]: Acquired first laser scan."); - - initialize(); -} - -bool ScanRectifier::updateParams(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res) { - bool prev_active = p_active_; - - nh_local_.param("active", p_active_, true); - - nh_local_.param("scan_rate", p_scan_rate_, 10.0); - nh_local_.param("odom_rate", p_odom_rate_, 100.0); - - p_odom2scan_ratio_ = static_cast(p_odom_rate_ / p_scan_rate_); - odoms_.set_capacity(p_odom2scan_ratio_); - - nh_local_.param("robot_frame", p_robot_frame_, string("robot")); - nh_local_.param("scanner_frame", p_scanner_frame_, string("scanner")); - - try { - ROS_INFO_STREAM("[Scan Rectifier]: Waiting for transformation: " << p_robot_frame_ << " -> " << p_scanner_frame_); - tf::StampedTransform transform; - ros::Time now = ros::Time::now(); - tf_ls_.waitForTransform(p_robot_frame_, p_scanner_frame_, now, ros::Duration(10.0)); - tf_ls_.lookupTransform(p_robot_frame_, p_scanner_frame_, now, transform); - - scanner_in_base_tf_.x = transform.getOrigin().getX(); - scanner_in_base_tf_.y = transform.getOrigin().getY(); - scanner_in_base_tf_.theta = tf::getYaw(transform.getRotation()); - ROS_INFO_STREAM("[Scan Rectifier]: Acquired transformation " << p_robot_frame_ << " -> " << p_scanner_frame_); - } - catch (tf::TransformException ex) { - throw ex.what(); - } - - if (p_active_ != prev_active) { - if (p_active_) { - scan_sub_ = nh_.subscribe("scan", 10, &ScanRectifier::scanCallback, this); - odom_sub_ = nh_.subscribe("odom", 10, &ScanRectifier::odomCallback, this); - scan_pub_ = nh_.advertise("rect_scan", 10); - } - else { - scan_sub_.shutdown(); - odom_sub_.shutdown(); - scan_pub_.shutdown(); - } - } - - return true; -} - -void ScanRectifier::scanCallback(const sensor_msgs::LaserScan::ConstPtr scan_msg) { -// if (odoms_.size() < odoms_.capacity()) -// return; - -// points_.clear(); - -// double t = 0.0; -// double theta = 0.0; -// double phi = scan_msg->angle_min; - -// double dt = scan_msg->time_increment; -// double dphi = scan_msg->angle_increment; -// double dt_offset = scan_msg->header.stamp.toSec() - odoms_[0].header.stamp.toSec(); - -// int idx_offset = 0; -// if (dt_offset > 0.0) -// idx_offset = static_cast(dt_offset * p_odom_rate_); - -// // Temporary velocities in robot coordinate frame -// double u, v, w; -// u = v = w = 0.0; - -// // Corrections of point coordinates resulting from motion prediction -// double x_cor, y_cor; -// x_cor = y_cor = 0.0; - -// geometry_msgs::Point point; -// for (const float r : scan_msg->ranges) { -// if (r > scan_msg->range_min && r < scan_msg->range_max) { -// int cur_odom_idx = static_cast(p_odom2scan_ratio_ * t / scan_msg->scan_time) + idx_offset; -// if (cur_odom_idx >= odoms_.size()) -// cur_odom_idx = odoms_.size() - 1; - -// u = odoms_[cur_odom_idx].twist.twist.linear.x; -// v = odoms_[cur_odom_idx].twist.twist.linear.y; -// w = odoms_[cur_odom_idx].twist.twist.angular.z; - -// w = 0.0; -// v = 0.0; - -// double v_s_x = u - scanner_in_base_tf_.y * w; -// double v_s_y = v + scanner_in_base_tf_.x * w; - -// // TODO: Rotate velocities into scanner frame - -// if (w != 0.0) { -// x_cor = (v_s_x * sin(theta) + v_s_y * (cos(theta) - 1.0)) / w; -// y_cor = (v_s_x * (1.0 - cos(theta)) + v_s_y * sin(theta)) / w; -// } -// else { -// x_cor = v_s_x * t; -// y_cor = v_s_y * t; -// } - -// point.x = r * cos(phi + theta) + x_cor; -// point.y = r * sin(phi + theta) + y_cor; - -// points_.push_back(point); -// } - -// t += dt; -// phi += dphi; -// theta += w * dt; -// } - - if (!tf_ls_.waitForTransform(scan_msg->header.frame_id, p_robot_frame_, - scan_msg->header.stamp + ros::Duration().fromSec(scan_msg->ranges.size() * scan_msg->time_increment), ros::Duration(1.0))) - return; - - sensor_msgs::PointCloud cloud; - projector_.transformLaserScanToPointCloud(p_robot_frame_, *scan_msg, cloud, tf_ls_); - - // Do something with cloud. - - // Prepare and publish scan - sensor_msgs::LaserScanPtr rect_scan_msg(new sensor_msgs::LaserScan); - - ranges_.assign(scan_msg->ranges.size(), 10.0f * scan_msg->range_max); - - for (auto& point : cloud.points) { - float angle = atan2(point.y, point.x); - float range = sqrt(pow(point.x, 2.0) + pow(point.y, 2.0)); -\ - int idx = static_cast(scan_msg->ranges.size() * (angle - scan_msg->angle_min) / (scan_msg->angle_max - scan_msg->angle_min)); - - if (ranges_[idx] > range) - ranges_[idx] = range; - } - - for (int jdx = 0; jdx < scan_msg->ranges.size(); ++jdx) - if (ranges_[jdx] < scan_msg->range_min || ranges_[jdx] > scan_msg->range_max) - ranges_[jdx] = nan(""); - - rect_scan_msg->header.frame_id = p_scanner_frame_; - rect_scan_msg->header.stamp = scan_msg->header.stamp; - - rect_scan_msg->angle_min = scan_msg->angle_min; - rect_scan_msg->angle_max = scan_msg->angle_max; - rect_scan_msg->angle_increment = scan_msg->angle_increment; - - rect_scan_msg->range_min = scan_msg->range_min; - rect_scan_msg->range_max = scan_msg->range_max; - - rect_scan_msg->scan_time = scan_msg->scan_time; - rect_scan_msg->time_increment = scan_msg->time_increment; - - rect_scan_msg-> ranges = ranges_; - - scan_pub_.publish(rect_scan_msg); -} - -void ScanRectifier::odomCallback(const nav_msgs::Odometry::ConstPtr odom_msg) { - odoms_.push_back(*odom_msg); -} diff --git a/src/scans_merger.cpp b/src/scans_merger.cpp index f1d1b13..4739235 100644 --- a/src/scans_merger.cpp +++ b/src/scans_merger.cpp @@ -34,7 +34,6 @@ */ #include "obstacle_detector/scans_merger.h" -#include "obstacle_detector/utilities/math_utilities.h" using namespace obstacle_detector; using namespace std; @@ -69,33 +68,10 @@ bool ScansMerger::updateParams(std_srvs::Empty::Request &req, std_srvs::Empty::R nh_local_.param("max_y_range", p_max_y_range_, 10.0); nh_local_.param("min_y_range", p_min_y_range_, -10.0); + nh_local_.param("fixed_frame_id", p_fixed_frame_id_, "map"); + nh_local_.param("target_frame_id", p_target_frame_id_, "scanner_base"); nh_local_.param("front_scan_frame_id", p_front_scan_frame_id_, "front_scanner"); nh_local_.param("rear_scan_frame_id", p_rear_scan_frame_id_, "rear_scanner"); - nh_local_.param("target_frame_id", p_target_frame_id_, "scanner_base"); - - try { - ROS_INFO_STREAM("[Scans Merger]: Waiting for transformations."); - tf::TransformListener tf; - tf::StampedTransform transform; - - tf.waitForTransform(p_target_frame_id_, p_front_scan_frame_id_, ros::Time(0), ros::Duration(10.0)); - tf.lookupTransform(p_target_frame_id_, p_front_scan_frame_id_, ros::Time(0), transform); - - front_tf_.x = transform.getOrigin().getX(); - front_tf_.y = transform.getOrigin().getY(); - front_tf_.theta = tf::getYaw(transform.getRotation()); - - tf.waitForTransform(p_target_frame_id_, p_rear_scan_frame_id_, ros::Time(0), ros::Duration(10.0)); - tf.lookupTransform(p_target_frame_id_, p_rear_scan_frame_id_, ros::Time(0), transform); - - rear_tf_.x = transform.getOrigin().getX(); - rear_tf_.y = transform.getOrigin().getY(); - rear_tf_.theta = tf::getYaw(transform.getRotation()); - ROS_INFO_STREAM("[Scans Merger]: Acquired transformations."); - } - catch (tf::TransformException ex) { - throw std::string("[Scans Merger]: ") + std::string(ex.what()); - } if (p_active_ != prev_active) { if (p_active_) { @@ -116,124 +92,107 @@ bool ScansMerger::updateParams(std_srvs::Empty::Request &req, std_srvs::Empty::R } void ScansMerger::frontScanCallback(const sensor_msgs::LaserScan::ConstPtr front_scan) { - if (front_scan_received_) - points_.clear(); - - if (!rear_scan_received_ || rear_scan_error_) - stamp_ = front_scan->header.stamp; + if (!tf_ls_.waitForTransform(front_scan->header.frame_id, p_fixed_frame_id_, + front_scan->header.stamp + ros::Duration().fromSec(front_scan->ranges.size() * front_scan->time_increment), ros::Duration(1.0))) + return; - geometry_msgs::Point32 local_point, base_point; - float phi = front_scan->angle_min; - - for (const float r : front_scan->ranges) { - if (r > front_scan->range_min && r < front_scan->range_max) { - local_point.x = r * cos(phi); - local_point.y = r * sin(phi); - - base_point = transformPoint(local_point, front_tf_.x, front_tf_.y, front_tf_.theta); - - if (checkPointInLimits(base_point, p_min_x_range_, p_max_x_range_, p_min_y_range_, p_max_y_range_)) - points_.push_back(base_point); - } - - phi += front_scan->angle_increment; - } + projector_.transformLaserScanToPointCloud(p_fixed_frame_id_, *front_scan, front_pcl_, tf_ls_); front_scan_received_ = true; front_scan_error_ = false; if (rear_scan_received_ || rear_scan_error_) - publishAll(); + publishMessages(); else rear_scan_error_ = true; } void ScansMerger::rearScanCallback(const sensor_msgs::LaserScan::ConstPtr rear_scan) { - if (rear_scan_received_) - points_.clear(); - - if (!front_scan_received_ || front_scan_error_) - stamp_ = rear_scan->header.stamp; - - geometry_msgs::Point32 local_point, base_point; - float phi = rear_scan->angle_min; - - for (const float r : rear_scan->ranges) { - if (r > rear_scan->range_min && r < rear_scan->range_max) { - local_point.x = r * cos(phi); - local_point.y = r * sin(phi); + if (!tf_ls_.waitForTransform(rear_scan->header.frame_id, p_fixed_frame_id_, + rear_scan->header.stamp + ros::Duration().fromSec(rear_scan->ranges.size() * rear_scan->time_increment), ros::Duration(1.0))) + return; - base_point = transformPoint(local_point, rear_tf_.x, rear_tf_.y, rear_tf_.theta); - - if (checkPointInLimits(base_point, p_min_x_range_, p_max_x_range_, p_min_y_range_, p_max_y_range_)) - points_.push_back(base_point); - } - - phi += rear_scan->angle_increment; - } + projector_.transformLaserScanToPointCloud(p_fixed_frame_id_, *rear_scan, rear_pcl_, tf_ls_); rear_scan_received_ = true; rear_scan_error_ = false; if (front_scan_received_ || front_scan_error_) - publishAll(); + publishMessages(); else front_scan_error_ = true; } -void ScansMerger::publishScan() { - sensor_msgs::LaserScanPtr scan_msg(new sensor_msgs::LaserScan); +void ScansMerger::publishMessages() { + ros::Time now = ros::Time::now(); - scan_msg->header.frame_id = p_target_frame_id_; - scan_msg->header.stamp = stamp_; - scan_msg->angle_min = -M_PI; - scan_msg->angle_max = M_PI; - scan_msg->angle_increment = 2.0 * M_PI / (p_ranges_num_ - 1); - scan_msg->time_increment = 0.0; - scan_msg->scan_time = 0.1; - scan_msg->range_min = p_min_scanner_range_; - scan_msg->range_max = p_max_scanner_range_; + if (!tf_ls_.waitForTransform(p_target_frame_id_, now, front_pcl_.header.frame_id, front_pcl_.header.stamp, p_fixed_frame_id_, ros::Duration(1.0))) + return; - ranges_.assign(p_ranges_num_, 2.0 * p_max_scanner_range_); + if (!tf_ls_.waitForTransform(p_target_frame_id_, now, rear_pcl_.header.frame_id, rear_pcl_.header.stamp, p_fixed_frame_id_, ros::Duration(1.0))) + return; - for (auto& point : points_) { - float angle = atan2(point.y, point.x); - float range = sqrt(pow(point.x, 2.0) + pow(point.y, 2.0)); + sensor_msgs::PointCloud new_front_pcl, new_rear_pcl; + tf_ls_.transformPointCloud(p_target_frame_id_, now, front_pcl_, p_fixed_frame_id_, new_front_pcl); + tf_ls_.transformPointCloud(p_target_frame_id_, now, rear_pcl_, p_fixed_frame_id_, new_rear_pcl); - int idx = static_cast(p_ranges_num_ * (angle + M_PI) / (2.0 * M_PI)); + if (p_publish_scan_) { + sensor_msgs::LaserScanPtr scan_msg(new sensor_msgs::LaserScan); - if (ranges_[idx] > range) - ranges_[idx] = range; - } + scan_msg->header.frame_id = p_target_frame_id_; + scan_msg->header.stamp = now; + scan_msg->angle_min = -M_PI; + scan_msg->angle_max = M_PI; + scan_msg->angle_increment = 2.0 * M_PI / (p_ranges_num_ - 1); + scan_msg->time_increment = 0.0; + scan_msg->scan_time = 0.1; + scan_msg->range_min = p_min_scanner_range_; + scan_msg->range_max = p_max_scanner_range_; - for (int jdx = 0; jdx < ranges_.size(); ++jdx) - if (ranges_[jdx] < p_min_scanner_range_ || ranges_[jdx] > p_max_scanner_range_) - ranges_[jdx] = nan(""); + vector ranges; + ranges.assign(p_ranges_num_, 2.0 * p_max_scanner_range_); - scan_msg->ranges = ranges_; + for (auto& point : new_front_pcl.points) { + float angle = atan2(point.y, point.x); + float range = sqrt(pow(point.x, 2.0) + pow(point.y, 2.0)); - scan_pub_.publish(scan_msg); -} + int idx = static_cast(p_ranges_num_ * (angle + M_PI) / (2.0 * M_PI)); + + if (ranges[idx] > range) + ranges[idx] = range; + } -void ScansMerger::publishPCL() { - sensor_msgs::PointCloudPtr pcl_msg(new sensor_msgs::PointCloud); + for (auto& point : new_rear_pcl.points) { + float angle = atan2(point.y, point.x); + float range = sqrt(pow(point.x, 2.0) + pow(point.y, 2.0)); - pcl_msg->header.frame_id = p_target_frame_id_; - pcl_msg->header.stamp = ros::Time::now(); - pcl_msg->points = points_; + int idx = static_cast(p_ranges_num_ * (angle + M_PI) / (2.0 * M_PI)); - pcl_pub_.publish(pcl_msg); -} + if (ranges[idx] > range) + ranges[idx] = range; + } + + for (int jdx = 0; jdx < ranges.size(); ++jdx) + if (ranges[jdx] < p_min_scanner_range_ || ranges[jdx] > p_max_scanner_range_) + ranges[jdx] = nan(""); + + scan_msg->ranges = ranges; + + scan_pub_.publish(scan_msg); + } -void ScansMerger::publishAll() { - if (p_publish_scan_) - publishScan(); + if (p_publish_pcl_) { + sensor_msgs::PointCloudPtr pcl_msg(new sensor_msgs::PointCloud); - if (p_publish_pcl_) - publishPCL(); + pcl_msg->header.frame_id = p_target_frame_id_; + pcl_msg->header.stamp = now; + + pcl_msg->points.insert(pcl_msg->points.end(), new_front_pcl.points.begin(), new_front_pcl.points.end()); + pcl_msg->points.insert(pcl_msg->points.end(), new_rear_pcl.points.begin(), new_rear_pcl.points.end()); + + pcl_pub_.publish(pcl_msg); + } front_scan_received_ = false; rear_scan_received_ = false; - - points_.clear(); }