diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml index 45acf90fed839847508a7a4dbd6edc4e35c6d59e..4b00943a3864276a8d91f5898c107089f26b36ef 100644 --- a/.gitlab-ci.yml +++ b/.gitlab-ci.yml @@ -1,3 +1,13 @@ +workflow: + rules: + - if: '$CI_PIPELINE_SOURCE == "web"' + - if: $CI_COMMIT_BRANCH && $CI_OPEN_MERGE_REQUESTS && $CI_PIPELINE_SOURCE == "push" + when: never + - if: '$CI_PIPELINE_SOURCE == "merge_request_event"' + - if: '$CI_COMMIT_BRANCH && $CI_OPEN_MERGE_REQUESTS' + when: never + - if: '$CI_COMMIT_BRANCH' + stages: - license - build_and_test @@ -6,12 +16,10 @@ stages: ############ YAML ANCHORS ############ .print_variables_template: &print_variables_definition # Print variables + - echo $CI_COMMIT_BRANCH - echo $WOLF_CORE_BRANCH - echo $WOLF_LASER_BRANCH - - echo $WOLF_ROS_NODE_NODE - - echo $CI_COMMIT_BRANCH - - echo $WOLF_ROS_LASER_NODE - - echo $WOLF_ROS_LASER_BRANCH + - echo $WOLF_ROS_NODE_BRANCH - echo $LASERSCANUTILS_BRANCH .preliminaries_template: &preliminaries_definition @@ -81,8 +89,7 @@ stages: - if [ -d wolf ]; then - echo "directory wolf exists" - cd wolf - - git checkout devel - - git pull + - git fetch --all - git checkout $WOLF_CORE_BRANCH - git pull - else @@ -101,8 +108,7 @@ stages: - if [ -d laser_scan_utils ]; then - echo "directory laser_scan_utils exists" - cd laser_scan_utils - - git checkout main - - git pull + - git fetch --all - git checkout $LASERSCANUTILS_BRANCH - git pull - else @@ -155,8 +161,7 @@ stages: - if [ -d laser ]; then - echo "directory laser exists" - cd laser - - git checkout devel - - git pull + - git fetch --all - git checkout $WOLF_LASER_BRANCH - git pull - else @@ -173,12 +178,12 @@ stages: .clone_wolfrosnode_template: &clone_wolfrosnode_definition - roscd - cd ../src - - git -b $WOLF_ROS_NODE_BRANCH clone ssh://git@gitlab.iri.upc.edu:2202/mobile_robotics/wolf_projects/wolf_ros/wolf_ros_node.git + - git clone -b $WOLF_ROS_NODE_BRANCH ssh://git@gitlab.iri.upc.edu:2202/mobile_robotics/wolf_projects/wolf_ros/wolf_ros_node.git .build_and_test_template: &build_and_test_definition - roscd - cd ../src - - git clone -b $CI_COMMIT_BRANCHssh://git@gitlab.iri.upc.edu:2202/${CI_PROJECT_PATH}.git + - git clone -b $CI_COMMIT_BRANCH ssh://git@gitlab.iri.upc.edu:2202/${CI_PROJECT_PATH}.git - cd .. - catkin_make diff --git a/include/publisher_laser_map.h b/include/publisher_laser_map.h index c018cfb8fa86893f082e182d1ecf9a98603966de..9471795421100679fca08bde46583876776bc6a1 100644 --- a/include/publisher_laser_map.h +++ b/include/publisher_laser_map.h @@ -59,6 +59,7 @@ class PublisherLaserMap: public Publisher protected: double update_dist_th_, update_angle_th_; + SensorBaseConstPtr sensor_; // OCCUPANCY MAP parameters and auxiliar variables double Lfree_, Lobst_, Lobst_th_, Lfree_th_; diff --git a/src/publisher_laser_map.cpp b/src/publisher_laser_map.cpp index b699f1f42f1eee61a87d464a4a4de2140faaa424..1046004391f1747c340849070bcee26ca9435321 100644 --- a/src/publisher_laser_map.cpp +++ b/src/publisher_laser_map.cpp @@ -38,6 +38,7 @@ PublisherLaserMap::PublisherLaserMap(const std::string& _unique_name, const ParamsServer& _server, ProblemConstPtr _problem) : Publisher(_unique_name, _server, _problem), + sensor_(nullptr), n_cells_(Eigen::Vector2i::Constant(100)), resized_(false), logodds_array_(Eigen::ArrayXXd::Zero(n_cells_(0),n_cells_(1))), @@ -48,6 +49,12 @@ PublisherLaserMap::PublisherLaserMap(const std::string& _unique_name, // common params update_dist_th_ = _server.getParam<double> (prefix_ + "/update_dist_th"); update_angle_th_ = _server.getParam<double> (prefix_ + "/update_angle_th"); + auto sensor_name = getParamWithDefault<std::string> (_server, prefix_ + "/sensor", ""); + if (not sensor_name.empty()) + { + sensor_ = _problem->findSensor(sensor_name); + WOLF_WARN_COND(not sensor_, "PublisherLaserMap: Sensor name not found: ", sensor_name); + } // occmap params occ_msg_.header.frame_id = _server.getParam<std::string> (prefix_ + "/map_frame_id"); @@ -86,7 +93,7 @@ PublisherLaserMap::PublisherLaserMap(const std::string& _unique_name, void PublisherLaserMap::initialize(ros::NodeHandle& nh, const std::string& topic) { - publisher_ = nh.advertise<nav_msgs::OccupancyGrid> (topic + "_occgrid", 1); + publisher_ = nh.advertise<nav_msgs::OccupancyGrid> (topic, 1); publisher_pc_ = nh.advertise<sensor_msgs::PointCloud2>(topic + "_pointcloud", 1); } @@ -150,7 +157,9 @@ void PublisherLaserMap::updateTrajectory() for (auto cap : cap_list) { auto cap_laser = std::dynamic_pointer_cast<const CaptureLaser2d>(cap); - if (cap_laser and not cap_laser->isRemoving()) + if (cap_laser and + not cap_laser->isRemoving() and + (not sensor_ or cap_laser->getSensor() == sensor_)) storeScan(frame_pair.second, cap_laser); } diff --git a/src/subscriber_laser2d.cpp b/src/subscriber_laser2d.cpp index 6bf2130194cc3c25681cf87bf9a38a317cb2ea97..976b54f447214293b98a6129754c5eb07402cdfb 100644 --- a/src/subscriber_laser2d.cpp +++ b/src/subscriber_laser2d.cpp @@ -34,7 +34,7 @@ SubscriberLaser2d::SubscriberLaser2d(const std::string& _unique_name, sensor_laser_ = std::dynamic_pointer_cast<SensorLaser2d>(sensor_ptr_); assert(sensor_laser_ != nullptr && "SubscriberLaser2d: sensor provided is not of type SensorLaser2d"); - laser_params_from_msg_ = not _server.getParam<bool>(prefix_ + "/load_params_from_msg"); + laser_params_from_msg_ = _server.getParam<bool>(prefix_ + "/load_params_from_msg"); upside_down_ = getParamWithDefault<bool>(_server, prefix_ + "/upside_down", false); }