Commit c7a6e351 authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

New release

parent afc5b351
/.cproject
/.project
/.settings/language.settings.xml
stages:
- license
- build_and_test
- demos
############ YAML ANCHORS ############
.preliminaries_template: &preliminaries_definition
## Install ssh-agent if not already installed, it is required by Docker.
## (change apt-get to yum if you use an RPM-based image)
- 'which ssh-agent || ( apt-get update -y && apt-get install openssh-client -y )'
## Run ssh-agent (inside the build environment)
- eval $(ssh-agent -s)
## Add the SSH key stored in SSH_PRIVATE_KEY variable to the agent store
## We're using tr to fix line endings which makes ed25519 keys work
## without extra base64 encoding.
## https://gitlab.com/gitlab-examples/ssh-private-key/issues/1#note_48526556
- mkdir -p ~/.ssh
- chmod 700 ~/.ssh
- echo "$SSH_PRIVATE_KEY" | tr -d '\r' | ssh-add - > /dev/null
# - echo "$SSH_KNOWN_HOSTS" > $HOME/.ssh/known_hosts
- ssh-keyscan -H -p 2202 gitlab.iri.upc.edu >> $HOME/.ssh/known_hosts
# update apt
- apt-get update
# create ci_deps folder (if not exists)
- mkdir -pv ci_deps
# manually source ros setup.bash
- source /root/catkin_ws/devel/setup.bash
- roscd # check that it works
.license_header_template: &license_header_definition
- cd $CI_PROJECT_DIR
# configure git
- export CI_NEW_BRANCH=ci_processing$RANDOM
- echo creating new temporary branch... $CI_NEW_BRANCH
- git config --global user.email "${CI_EMAIL}"
- git config --global user.name "${CI_USERNAME}"
- git checkout -b $CI_NEW_BRANCH # temporary branch
# license headers
- export CURRENT_YEAR=$( date +'%Y' )
- echo "current year:" ${CURRENT_YEAR}
- if [ -f license_header_${CURRENT_YEAR}.txt ]; then
# add license headers to new files
- echo "File license_header_${CURRENT_YEAR}.txt already exists. License headers are assumed to be updated. Adding headers to new files..."
- ./ci_deps/wolf/wolf_scripts/license_manager.sh --add --path=. --license-header=license_header_${CURRENT_YEAR}.txt --exclude=ci_deps
- else
# update license headers of all files
- export PREV_YEAR=$(( CURRENT_YEAR-1 ))
- echo "Creating new file license_header_${CURRENT_YEAR}.txt..."
- git mv license_header_${PREV_YEAR}.txt license_header_${CURRENT_YEAR}.txt
- sed -i "s/${PREV_YEAR}/${PREV_YEAR},${CURRENT_YEAR}/g" license_header_${CURRENT_YEAR}.txt
- ./ci_deps/wolf/wolf_scripts/license_manager.sh --update --path=. --license-header=license_header_${CURRENT_YEAR}.txt --exclude=ci_deps
- fi
# push changes (if any)
- if git commit -a -m "[skip ci] license headers added or modified" ; then
- git remote set-url --push origin "ssh://git@gitlab.iri.upc.edu:2202/${CI_PROJECT_PATH}.git"
- git push origin $CI_NEW_BRANCH:${CI_COMMIT_REF_NAME}
- else
- echo "No changes, nothing to commit!"
- fi
.install_wolf_template: &install_wolf_definition
- cd ${CI_PROJECT_DIR}/ci_deps
- if [ -d wolf ]; then
- echo "directory wolf exists"
- cd wolf
- git checkout devel
- git pull
- git checkout $WOLF_CORE_BRANCH
- git pull
- else
- git clone -b $WOLF_CORE_BRANCH ssh://git@gitlab.iri.upc.edu:2202/mobile_robotics/wolf_projects/wolf_lib/wolf.git
- cd wolf
- fi
- mkdir -pv build
- cd build
- cmake -DCMAKE_BUILD_TYPE=release -DBUILD_DEMOS=OFF -DBUILD_TESTS=OFF ..
- make -j$(nproc)
- make install
- ldconfig
.install_laserscanutils_template: &install_laserscanutils_definition
- cd ${CI_PROJECT_DIR}/ci_deps
- if [ -d laser_scan_utils ]; then
- echo "directory laser_scan_utils exists"
- cd laser_scan_utils
- git pull
- else
- git clone https://gitlab.iri.upc.edu/labrobotica/algorithms/laser_scan_utils.git
- cd laser_scan_utils
- fi
- mkdir -pv build
- cd build
- rm -rf *
- cmake -DCMAKE_BUILD_TYPE=release ..
- make -j$(nproc)
- make install
- ldconfig
.install_csm_template: &install_csm_definition
- cd ${CI_PROJECT_DIR}/ci_deps
- apt-get install -y libgsl-dev
- if [ -d csm ]; then
- echo "directory csm exists"
- cd csm
- git pull
- else
- git clone https://gitlab.iri.upc.edu/labrobotica/algorithms/csm.git
- cd csm
- fi
- cmake .
- make -j$(nproc)
- make install
- ldconfig
.install_falko_template: &install_falko_definition
- cd ${CI_PROJECT_DIR}/ci_deps
- if [ -d falkolib ]; then
- echo "directory falkolib exists"
- cd falkolib
- git pull
- else
- git clone https://gitlab.iri.upc.edu/labrobotica/algorithms/falkolib.git
- cd falkolib
- fi
- mkdir -pv build
- cd build
- cmake ..
- make -j$(nproc)
- make install
- ldconfig
.install_wolflaser_template: &install_wolflaser_definition
- cd ${CI_PROJECT_DIR}/ci_deps
- if [ -d laser ]; then
- echo "directory laser exists"
- cd laser
- git checkout devel
- git pull
- git checkout $WOLF_LASER_BRANCH
- git pull
- else
- git clone -b $WOLF_LASER_BRANCH ssh://git@gitlab.iri.upc.edu:2202/mobile_robotics/wolf_projects/wolf_lib/plugins/laser.git
- cd laser
- fi
- mkdir -pv build
- cd build
- cmake -DCMAKE_BUILD_TYPE=release -DBUILD_TESTS=OFF ..
- make -j$(nproc)
- make install
- ldconfig
.clone_wolfrosnode_template: &clone_wolfrosnode_definition
- roscd
- cd ../src
- git clone ssh://git@gitlab.iri.upc.edu:2202/mobile_robotics/wolf_projects/wolf_ros/wolf_ros_node.git
- cd wolf_ros_node
- git checkout $WOLF_ROS_CORE_BRANCH
.build_and_test_template: &build_and_test_definition
- roscd
- cd ../src
- git clone ssh://git@gitlab.iri.upc.edu:2202/${CI_PROJECT_PATH}.git
- cd wolf_ros_laser
- git checkout $CI_COMMIT_BRANCH
- cd ../..
- catkin_make
############ LICENSE HEADERS ############
license_headers:
stage: license
image: labrobotica/wolf_deps_ros:20.04
cache: []
except:
- master
before_script:
- *preliminaries_definition
- *install_wolf_definition
script:
- *license_header_definition
############ UBUNTU 18.04 TEST ############
build_and_test:bionic:
stage: build_and_test
image: labrobotica/wolf_deps_ros:18.04
cache:
- key: wolf_and_deps-bionic
paths:
- ci_deps
except:
- master
before_script:
- *preliminaries_definition
- *install_wolf_definition
- *install_csm_definition
- *install_falko_definition
- *install_laserscanutils_definition
- *install_wolflaser_definition
- *clone_wolfrosnode_definition
script:
- *build_and_test_definition
############ UBUNTU 20.04 TEST ############
build_and_test:focal:
stage: build_and_test
image: labrobotica/wolf_deps_ros:20.04
cache:
- key: wolf_and_deps-focal
paths:
- ci_deps
except:
- master
before_script:
- *preliminaries_definition
- *install_wolf_definition
- *install_csm_definition
- *install_falko_definition
- *install_laserscanutils_definition
- *install_wolflaser_definition
- *clone_wolfrosnode_definition
script:
- *build_and_test_definition
############ RUN DEMOS ############
demo_laser:
stage: demos
variables:
WOLF_CORE_BRANCH: $WOLF_CORE_BRANCH
WOLF_LASER_BRANCH: $WOLF_LASER_BRANCH
WOLF_ROS_CORE_BRANCH: $WOLF_ROS_CORE_BRANCH
WOLF_ROS_LASER_BRANCH: $CI_COMMIT_BRANCH
trigger:
project: mobile_robotics/wolf_projects/wolf_ros/demos/wolf_demo_laser2d
cmake_minimum_required(VERSION 2.8.3)
project(wolf_ros_laser)
## Compile as C++11, supported in ROS Kinetic and newer
add_compile_options(-std=c++11)
## Compile as C++14
add_compile_options(-std=c++14)
# SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/wolf_ros_wrapper/cmake_modules")
## Find catkin macros and libraries
......@@ -15,14 +15,17 @@ find_package(catkin REQUIRED COMPONENTS
tf
dynamic_reconfigure
wolf_ros_node
pcl_ros
laser_geometry
)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
# find_package(Ceres REQUIRED)
# find_package(Eigen3 REQUIRED)
find_package(wolf REQUIRED)
find_package(wolfcore REQUIRED)
find_package(wolflaser REQUIRED)
find_package(falkolib QUIET)
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
......@@ -121,11 +124,10 @@ catkin_package(
## Specify additional locations of header files
## Your package locations should be listed before other locations
message("Wolf include path: ${wolf_INCLUDE_DIR}")
include_directories(
include
${EIGEN_INCLUDE_DIRS}
${wolf_INCLUDE_DIRS}
${wolfcore_INCLUDE_DIRS}
${wolflaser_INCLUDE_DIRS}
${laser_scan_utils_INCLUDE_DIRS}
${catkin_INCLUDE_DIRS}
......@@ -136,7 +138,22 @@ include_directories(
# add_library(${PROJECT_NAME}
# src/${PROJECT_NAME}/wolf_ros.cpp
# )
add_library(wolf_subscriber_laser2D src/wolf_ros_subscriber_laser2D.cpp)
add_library(publisher_${PROJECT_NAME})
add_library(subscriber_${PROJECT_NAME})
target_sources(publisher_${PROJECT_NAME} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/src/publisher_laser_map.cpp)
target_sources(subscriber_${PROJECT_NAME} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/src/subscriber_laser2d.cpp)
if (falkolib_FOUND)
message("Found Falkolib. Compiling publisher_falko.")
target_sources(publisher_${PROJECT_NAME} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/src/publisher_falko.cpp)
endif()
find_file(ICP wolflaser_INCLUDE_DIRS laser/processor/processor_odom_icp.h)
if (NOT ICP_NOTFOUND)
message("Found 'processor_odom_icp.h'. Compiling publisher_odom_icp.")
target_sources(publisher_${PROJECT_NAME} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/src/publisher_odom_icp.cpp)
endif ()
## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
......@@ -160,11 +177,15 @@ add_library(wolf_subscriber_laser2D src/wolf_ros_subscriber_laser2D.cpp)
#add_dependencies(${PROJECT_NAME}_visualizer ${PROJECT_NAME}_gencfg)
## Specify libraries to link a library or executable target against
target_link_libraries(wolf_subscriber_laser2D
${wolf_LIBRARIES}
${wolflaser_LIBRARIES}
)
target_link_libraries(subscriber_${PROJECT_NAME}
${wolfcore_LIBRARIES}
${wolflaser_LIBRARIES}
)
target_link_libraries(publisher_${PROJECT_NAME}
${wolfcore_LIBRARIES}
${wolflaser_LIBRARIES}
${catkin_LIBRARIES}
)
#############
## Install ##
#############
......
This diff is collapsed.
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
#ifndef PUBLISHER_FALKO_BSC_H
#define PUBLISHER_FALKO_BSC_H
/**************************
* ROS includes *
**************************/
#include <geometry_msgs/PoseArray.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <visualization_msgs/MarkerArray.h>
/**************************
* WOLF includes *
**************************/
#include <core/common/wolf.h>
#include <core/problem/problem.h>
#include "laser/capture/capture_laser_2d.h"
#include "laser/feature/feature_scene_falko.h"
#include "publisher.h"
namespace wolf {
class PublisherFalko : public Publisher
{
bool pose_array_, marker_, pose_with_cov_;
int max_points_;
visualization_msgs::Marker marker_msg_falko_scene;
std_msgs::ColorRGBA marker_color_target_;
std_msgs::ColorRGBA marker_color_reference_;
bool extrinsics_;
SensorBasePtr sensor_;
std::string frame_id_, map_frame_id_;
FrameBasePtr last_frame;
FrameBasePtr KF_old;
ros::Publisher pub_keypoints_;
ros::Publisher pub_marker_scene_target_;
ros::Publisher pub_marker_scene_reference_;
ros::Publisher pub_marker_associations_;
// Reference scene info
std::map<CaptureBasePtr, visualization_msgs::Marker> map_markers_reference_scenes_;
public:
PublisherFalko(const std::string &_unique_name, const ParamsServer &_server, const ProblemPtr _problem);
WOLF_PUBLISHER_CREATE(PublisherFalko);
virtual ~PublisherFalko(){};
void initialize(ros::NodeHandle &nh, const std::string &topic) override;
void publishDerived() override;
void publishScene(std::vector<falkolib::FALKO> &_keypoints, laserscanutils::LaserScan &_scan,
Eigen::Vector3d &_p_rob, Eigen::Quaterniond &_q_rob, ros::Publisher &_pub_lines,
std_msgs::ColorRGBA _marker_color, int & _id, CaptureBasePtr cap);
void publishEmpty();
void publishAssociations(std::vector<std::pair<int, int>> &associations_, CaptureBasePtr &cap_target,
CaptureBasePtr &cap_reference);
void getKeypointsAndScan(FrameBasePtr &_frame, laserscanutils::LaserScan &_scan,
std::vector<falkolib::FALKO> &_keypoints);
void getPosition(Eigen::Vector3d &p, Eigen::Quaterniond &q, VectorComposite current_state);
protected:
//Eigen::Quaterniond q_frame_;
//Eigen::Vector3d t_frame_;
int id_old_ = 1;
std::map<FrameBasePtr, VectorComposite> map_frame_states;
};
WOLF_REGISTER_PUBLISHER(PublisherFalko)
} // namespace wolf
#endif
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
#ifndef PUBLISHER_OCCUPANCY_MAP_H
#define PUBLISHER_OCCUPANCY_MAP_H
/**************************
* ROS includes *
**************************/
#include <nav_msgs/OccupancyGrid.h>
#include <sensor_msgs/PointCloud2.h>
/**************************
* PCL includes *
**************************/
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
/**************************
* WOLF includes *
**************************/
#include <core/common/wolf.h>
#include <core/problem/problem.h>
#include <laser/capture/capture_laser_2d.h>
#include "publisher.h"
using namespace wolf;
struct ScanData
{
CaptureLaser2dPtr capture_;
laserscanutils::LaserScanParams params_;
Eigen::MatrixXf local_points_;
pcl::PointCloud<pcl::PointXYZRGB> local_pc_;
};
class PublisherLaserMap: public Publisher
{
protected:
double update_dist_th_, update_angle_th_;
// OCCUPANCY MAP parameters and auxiliar variables
double Lfree_, Lobst_, Lobst_th_, Lfree_th_;
int max_n_cells_;
Eigen::Vector2i n_cells_;
double grid_size_;
bool resized_;
bool discard_max_range_;
Eigen::Vector2d map_origin_;
Eigen::ArrayXXd logodds_array_;
nav_msgs::OccupancyGrid occ_msg_;
// AGGREGATED POINTCLOUD
sensor_msgs::PointCloud2 pc_msg_;
pcl::PointCloud<pcl::PointXYZRGB> agg_pc_;
ros::Publisher publisher_pc_;
int pc_r_, pc_g_, pc_b_;
// std::maps
std::map<FrameBasePtr,std::list<ScanData>> scans_;
std::map<FrameBasePtr,Eigen::Vector3d> current_trajectory_, last_trajectory_occ_, last_trajectory_pc_;
public:
PublisherLaserMap(const std::string& _unique_name,
const ParamsServer& _server,
const ProblemPtr _problem);
WOLF_PUBLISHER_CREATE(PublisherLaserMap);
virtual ~PublisherLaserMap(){};
void initialize(ros::NodeHandle &nh, const std::string& topic) override;
void publishDerived() override;
protected:
void updateTrajectory();
bool trajectoryChanged(const std::map<FrameBasePtr,Eigen::Vector3d>& _last_trajectory,
const std::map<FrameBasePtr,Eigen::Vector3d>& _current_trajectory) const;
void storeScan(FrameBasePtr frame, CaptureLaser2dPtr capture);
// occmap
void resetOccMap();
bool updateOccMap();
void addScanToOccMap(const ScanData& scan_data, const Eigen::Vector3d& pose);
template<typename T>
Eigen::Vector2i vectorToCell(const Eigen::DenseBase<T>& p);
void drawFreeLine(const Eigen::Vector2i& cell_0, const Eigen::Vector2i& cell_1);
void resizeOccMap(const Eigen::Vector4i& oversize);
void updateOccMsg();
void resizeOccMsg();
// pointcloud
void resetPcMap();
bool updatePcMap();
void addScanToPcMap(const ScanData& scan_data, const Eigen::Vector3d& pose);
void updatePcMsg();
};
template<typename T>
inline Eigen::Vector2i PublisherLaserMap::vectorToCell(const Eigen::DenseBase<T>& p)
{
MatrixSizeCheck<2, 1>::check(p);
Eigen::Vector2i cell;
cell(0) = int( std::floor((p(0) - map_origin_(0)) / grid_size_ ));
cell(1) = int( std::floor((p(1) - map_origin_(1)) / grid_size_ ));
return cell;
}
#endif
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
#ifndef PUBLISHER_ODOM_ICP_H
#define PUBLISHER_ODOM_ICP_H
/**************************
* WOLF includes *
**************************/
#include "core/problem/problem.h"
#include "publisher.h"
#include "laser/processor/processor_odom_icp.h"
/**************************
* ROS includes *
**************************/
#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
namespace wolf
{
class PublisherOdomIcp: public Publisher
{
protected:
ProcessorOdomIcpPtr processor_odom_icp_;
geometry_msgs::PoseStamped msg_;
public:
PublisherOdomIcp(const std::string& _unique_name,
const ParamsServer& _server,
const ProblemPtr _problem);
WOLF_PUBLISHER_CREATE(PublisherOdomIcp);
virtual ~PublisherOdomIcp(){};
void initialize(ros::NodeHandle &nh, const std::string& topic) override;
void publishDerived() override;
};