diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000000000000000000000000000000000000..102209f46cc9047bfedc7848ab95d06374bf1193 --- /dev/null +++ b/.gitignore @@ -0,0 +1,3 @@ +/.cproject +/.project +/.settings/language.settings.xml diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml index 3ae6df78e82d0129570c877f4be2e0d5ac796c7e..11d85fa384bd868c0e8f9d47eb8dadf38d87826a 100644 --- a/.gitlab-ci.yml +++ b/.gitlab-ci.yml @@ -4,6 +4,15 @@ stages: - demos ############ YAML ANCHORS ############ +.print_variables_template: &print_variables_definition + # Print variables + - echo $WOLF_CORE_BRANCH + - echo $WOLF_GNSS_BRANCH + - echo $WOLF_ROS_NODE_BRANCH + - echo $CI_COMMIT_BRANCH + - echo $WOLF_ROS_GNSS_BRANCH + - echo $GNSSUTILS_BRANCH + .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) @@ -90,9 +99,12 @@ stages: - if [ -d gnss_utils ]; then - echo "directory gnss_utils exists" - cd gnss_utils + - git checkout devel + - git pull + - git checkout $GNSSUTILS_BRANCH - git pull - else - - git clone ssh://git@gitlab.iri.upc.edu:2202/mobile_robotics/gauss_project/gnss_utils.git + - git clone -b $GNSSUTILS_BRANCH ssh://git@gitlab.iri.upc.edu:2202/mobile_robotics/gauss_project/gnss_utils.git - cd gnss_utils - git submodule update --init - fi @@ -124,17 +136,13 @@ stages: .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 + - 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 ssh://git@gitlab.iri.upc.edu:2202/${CI_PROJECT_PATH}.git - - cd wolf_ros_gnss - - git checkout $CI_COMMIT_BRANCH - - cd ../.. + - git clone -b $CI_COMMIT_BRANCH ssh://git@gitlab.iri.upc.edu:2202/${CI_PROJECT_PATH}.git + - cd .. - catkin_make ############ LICENSE HEADERS ############ @@ -143,6 +151,7 @@ license_headers: image: labrobotica/wolf_deps_ros:20.04 cache: [] before_script: + - *print_variables_definition - *preliminaries_definition - *install_wolf_definition script: @@ -163,6 +172,7 @@ build_and_test:bionic: paths: - ci_deps/gnss/ before_script: + - *print_variables_definition - *preliminaries_definition - *install_wolf_definition - *install_gnssutils_definition @@ -187,6 +197,7 @@ build_and_test:focal: paths: - ci_deps/gnss/ before_script: + - *print_variables_definition - *preliminaries_definition - *install_wolf_definition - *install_gnssutils_definition @@ -202,7 +213,8 @@ demo_gnss: variables: WOLF_CORE_BRANCH: $WOLF_CORE_BRANCH WOLF_GNSS_BRANCH: $WOLF_GNSS_BRANCH - WOLF_ROS_CORE_BRANCH: $WOLF_ROS_CORE_BRANCH + GNSSUTILS_BRANCH: $GNSSUTILS_BRANCH + WOLF_ROS_NODE_BRANCH: $WOLF_ROS_NODE_BRANCH WOLF_ROS_GNSS_BRANCH: $CI_COMMIT_BRANCH trigger: project: mobile_robotics/wolf_projects/wolf_ros/demos/wolf_demo_gnss_imu diff --git a/CMakeLists.txt b/CMakeLists.txt index 3de46d0c3ee9c5b1a95b720b9394e590c3581b16..74945dbdfde8cad0d498daa269180c977abfbfda 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,13 +1,11 @@ cmake_minimum_required(VERSION 2.8.3) project(wolf_ros_gnss) -## Compile as C++11, supported in ROS Kinetic and newer -add_compile_options(-std=c++14) +## Compile as C++14 +add_compile_options(-std=c++14) +# -fPIC and -rdynamic ensure unique singleton instance across shared libraries (for factories) see: https://stackoverflow.com/a/8626922 +SET(CMAKE_CXX_FLAGS "-fPIC -rdynamic") -# SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/wolf_ros_wrapper/cmake_modules") -## 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 sensor_msgs @@ -25,96 +23,14 @@ find_package(novatel_oem7_msgs QUIET) ## System dependencies are found with CMake's conventions find_package(wolfcore REQUIRED) find_package(wolfgnss REQUIRED) +find_package(gnss_utils REQUIRED) -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend tag for "message_generation" -## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependency has been pulled in -## but can be declared for certainty nonetheless: -## * add a exec_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) - -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -#generate_messages( -# DEPENDENCIES -# iri_gnss_msgs -#) - -################################################ -## Declare ROS dynamic reconfigure parameters ## -################################################ - -## To declare and build dynamic reconfigure parameters within this -## package, follow these steps: -## * In the file package.xml: -## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" -## * In this file (CMakeLists.txt): -## * add "dynamic_reconfigure" to -## find_package(catkin REQUIRED COMPONENTS ...) -## * uncomment the "generate_dynamic_reconfigure_options" section below -## and list every .cfg file to be processed - -## Generate dynamic reconfigure parameters in the 'cfg' folder -#generate_dynamic_reconfigure_options( -# cfg/WolfROS.cfg -#) ################################### ## catkin specific configuration ## ################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if your package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need +# produces the XXXConfig.cmake file among other things catkin_package( -# INCLUDE_DIRS include -# LIBRARIES wolf_ros1 -# CATKIN_DEPENDS roscpp sensor_msgs std_msgs -# DEPENDS system_lib ) ########### @@ -123,10 +39,6 @@ catkin_package( set(INCLUDE_DIRS include - ${EIGEN_INCLUDE_DIRS} - ${wolfcore_INCLUDE_DIRS} - ${wolfgnss_INCLUDE_DIRS} - ${gnss_utils_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS} ) @@ -144,13 +56,6 @@ set(SUB_SRCS src/subscriber_gnss_ublox.cpp ) -set(LIBRARIES - ${wolfcore_LIBRARIES} - ${wolfgnss_LIBRARIES} - ${iri_gnss_msgs_LIBRARIES} - ${catkin_LIBRARIES} - ) - # OPTIONAL iri_gnss_msgs dependant code if (iri_gnss_msgs_FOUND) @@ -179,92 +84,33 @@ endif() ## Specify additional locations of header files ## Your package locations should be listed before other locations -message("Wolf include path: ${wolfcore_INCLUDE_DIRS}") -message("iri_gnss_msgs include path: ${iri_gnss_msgs_INCLUDE_DIRS}") include_directories(${INCLUDE_DIRS}) -# link_directories(/usr/local/lib/iri-algorithms) -## Declare a C++ library -# add_library(${PROJECT_NAME} -# src/${PROJECT_NAME}/wolf_ros.cpp -# ) + add_library(publisher_${PROJECT_NAME} ${PUB_SRCS}) add_library(subscriber_${PROJECT_NAME} ${SUB_SRCS}) -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -# add_dependencies(${PROJECT_NAME}_node ${PROJECT_NAME}_gencfg) -# add_dependencies(${PROJECT_NAME}_visualizer ${PROJECT_NAME}_gencfg) - -## Declare a C++ executable -## With catkin_make all packages are built within a single CMake context -## The recommended prefix ensures that target names across packages don't collide - -## Rename C++ executable without prefix -## The above recommended prefix causes long target names, the following renames the -## target back to the shorter version for ease of user use -## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg" -# set_target_properties(${PROJECT_NAME} PROPERTIES OUTPUT_NAME node PREFIX "") - -## Add cmake target dependencies of the executable -## same as for the library above -#add_dependencies(${PROJECT_NAME}_node ${PROJECT_NAME}_gencfg) -#add_dependencies(${PROJECT_NAME}_visualizer ${PROJECT_NAME}_gencfg) -if (iri_gnss_msgs_FOUND) - add_dependencies(subscriber_${PROJECT_NAME} iri_gnss_msgs_generate_messages_cpp) -endif() -if (novatel_oem7_msgs_FOUND) - add_dependencies(subscriber_${PROJECT_NAME} novatel_oem7_msgs_generate_messages_cpp) -endif() +# if (iri_gnss_msgs_FOUND) +# add_dependencies(subscriber_${PROJECT_NAME} iri_gnss_msgs_generate_messages_cpp) +# endif() +# if (novatel_oem7_msgs_FOUND) +# add_dependencies(subscriber_${PROJECT_NAME} novatel_oem7_msgs_generate_messages_cpp) +# endif() ## Specify libraries to link a library or executable target against -target_link_libraries(subscriber_${PROJECT_NAME} ${LIBRARIES}) -target_link_libraries(publisher_${PROJECT_NAME} ${LIBRARIES}) - -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# install(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables and/or libraries for installation -# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME} -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) +target_link_libraries(subscriber_${PROJECT_NAME} + wolfcore + wolfgnss + ${catkin_LIBRARIES} + ) -## Mark other files for installation (e.g. launch and bag files, etc.) -# install(FILES -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) +if (iri_gnss_msgs_FOUND OR novatel_oem7_msgs_FOUND) + target_link_libraries(subscriber_${PROJECT_NAME} ${LIBRARIES}) +endif() -############# -## Testing ## -############# -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_wolf_ros.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() +target_link_libraries(publisher_${PROJECT_NAME} + wolfcore + wolfgnss -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) + ${catkin_LIBRARIES} + ) diff --git a/include/publisher_gnss_accuracy.h b/include/publisher_gnss_accuracy.h index 57ca6cfb27e97a50d925f07cb779f29fac88352e..c494fc4da4d796de4d0741153d14b339e05d1cac 100644 --- a/include/publisher_gnss_accuracy.h +++ b/include/publisher_gnss_accuracy.h @@ -42,13 +42,13 @@ class PublisherGnssAccuracy: public Publisher { visualization_msgs::Marker PL_marker_msg_; std_msgs::ColorRGBA marker_color_; - SensorBasePtr sensor_; + SensorBaseConstPtr sensor_; double k_H_, k_V_; public: PublisherGnssAccuracy(const std::string& _unique_name, const ParamsServer& _server, - const ProblemPtr _problem); + ProblemConstPtr _problem); WOLF_PUBLISHER_CREATE(PublisherGnssAccuracy); virtual ~PublisherGnssAccuracy(){}; diff --git a/include/publisher_gnss_fix.h b/include/publisher_gnss_fix.h index a4f0caf70dcbfe8f693342c7d5651cc067962e10..4f2daad7ad59c5a6ef3619333edb5c9c370a7e8d 100644 --- a/include/publisher_gnss_fix.h +++ b/include/publisher_gnss_fix.h @@ -45,15 +45,15 @@ class PublishGnssFix: public Publisher geometry_msgs::PoseArray pose_array_msg_; geometry_msgs::PoseStamped pose_msg_; visualization_msgs::Marker marker_msg_; - SensorBasePtr sensor_; + SensorBaseConstPtr sensor_; std::string frame_id_, map_frame_id_; ros::Publisher pub_pose_array_, pub_marker_, pub_pose_; public: PublishGnssFix(const std::string& _unique_name, - const ParamsServer& _server, - const ProblemPtr _problem); + const ParamsServer& _server, + ProblemConstPtr _problem); WOLF_PUBLISHER_CREATE(PublishGnssFix); virtual ~PublishGnssFix(){}; diff --git a/include/publisher_gnss_tf.h b/include/publisher_gnss_tf.h index 6779049080bb5e17a560cb611db673b48fd42158..6d49cbe48b16e8df87390ab2004cfd5c659c8570 100644 --- a/include/publisher_gnss_tf.h +++ b/include/publisher_gnss_tf.h @@ -40,12 +40,12 @@ class PublisherGnssTf: public Publisher std::string map_frame_id_, enu_frame_id_, ecef_frame_id_; tf::StampedTransform T_enu_map_, T_ecef_enu_; tf::TransformBroadcaster tfb_; - SensorGnssPtr sensor_gnss_; + SensorGnssConstPtr sensor_gnss_; public: PublisherGnssTf(const std::string& _unique_name, - const ParamsServer& _server, - const ProblemPtr _problem); + const ParamsServer& _server, + ProblemConstPtr _problem); WOLF_PUBLISHER_CREATE(PublisherGnssTf); virtual ~PublisherGnssTf(){}; diff --git a/include/publisher_tracker_gnss_info.h b/include/publisher_tracker_gnss_info.h index ce057285292583562c24510723ede5976093d589..19ccd90f25f51565bf2121860e73188f1e597dff 100644 --- a/include/publisher_tracker_gnss_info.h +++ b/include/publisher_tracker_gnss_info.h @@ -42,13 +42,13 @@ class PublisherTrackerGnssInfo: public Publisher { protected: - ProcessorTrackerGnssPtr processor_tracker_gnss_; + ProcessorTrackerGnssConstPtr processor_tracker_gnss_; ros::Publisher publisher_untracked_; public: PublisherTrackerGnssInfo(const std::string& _unique_name, const ParamsServer& _server, - const ProblemPtr _problem); + ProblemConstPtr _problem); WOLF_PUBLISHER_CREATE(PublisherTrackerGnssInfo); virtual ~PublisherTrackerGnssInfo(){}; diff --git a/src/publisher_gnss_accuracy.cpp b/src/publisher_gnss_accuracy.cpp index a2ef3ca4ffab3a22328e4f0c59e8dcf9c2d20199..f57141022e54f96e5cd05d9e6134cebd44444f6f 100644 --- a/src/publisher_gnss_accuracy.cpp +++ b/src/publisher_gnss_accuracy.cpp @@ -31,16 +31,16 @@ namespace wolf { PublisherGnssAccuracy::PublisherGnssAccuracy(const std::string& _unique_name, - const ParamsServer& _server, - const ProblemPtr _problem) : - Publisher(_unique_name, _server, _problem) + const ParamsServer& _server, + ProblemConstPtr _problem) : + Publisher(_unique_name, _server, _problem) { Eigen::Vector4d col = _server.getParam<Eigen::Vector4d>(prefix_ + "/marker_color"); marker_color_.r = col(0); marker_color_.g = col(1); marker_color_.b = col(2); marker_color_.a = col(3); - sensor_ = _problem->getSensor(_server.getParam<std::string>(prefix_ + "/sensor")); + sensor_ = _problem->findSensor(_server.getParam<std::string>(prefix_ + "/sensor")); k_H_ = _server.getParam<double>(prefix_ + "/k_H"); k_V_ = _server.getParam<double>(prefix_ + "/k_V"); } diff --git a/src/publisher_gnss_fix.cpp b/src/publisher_gnss_fix.cpp index 70f6e3f5dd88f87e7d50a01fa58c6a8ec5d4555e..5b4750ca16e3139ff16258d001cccb3724eff3cd 100644 --- a/src/publisher_gnss_fix.cpp +++ b/src/publisher_gnss_fix.cpp @@ -28,11 +28,11 @@ namespace wolf { PublishGnssFix::PublishGnssFix(const std::string& _unique_name, - const ParamsServer& _server, - const ProblemPtr _problem) : + const ParamsServer& _server, + ProblemConstPtr _problem) : Publisher(_unique_name, _server, _problem) { - sensor_ = _problem->getSensor(_server.getParam<std::string>(prefix_ + "/sensor")); + sensor_ = _problem->findSensor(_server.getParam<std::string>(prefix_ + "/sensor")); frame_id_ = _server.getParam<std::string>(prefix_ + "/frame_id"); } diff --git a/src/publisher_gnss_tf.cpp b/src/publisher_gnss_tf.cpp index ed7ab474f9586167f15b207bd0dbb2dc89110fd6..da778fbb3e6ff5cae76a2b4c9a12ccf50e824611 100644 --- a/src/publisher_gnss_tf.cpp +++ b/src/publisher_gnss_tf.cpp @@ -33,10 +33,10 @@ namespace wolf PublisherGnssTf::PublisherGnssTf(const std::string& _unique_name, const ParamsServer& _server, - const ProblemPtr _problem) : + ProblemConstPtr _problem) : Publisher(_unique_name, _server, _problem) { - sensor_gnss_ = std::static_pointer_cast<SensorGnss>(_problem->getSensor(_server.getParam<std::string>(prefix_ + "/sensor_gnss_name"))); + sensor_gnss_ = std::static_pointer_cast<const SensorGnss>(_problem->findSensor(_server.getParam<std::string>(prefix_ + "/sensor_gnss_name"))); T_enu_map_.setIdentity(); T_enu_map_.frame_id_ = "ENU"; diff --git a/src/publisher_tracker_gnss_info.cpp b/src/publisher_tracker_gnss_info.cpp index 1fc154f2a2390adf28538b365294d3654d00632d..4634a96cd59bc5d1d88eaf25e148ac9c969a439c 100644 --- a/src/publisher_tracker_gnss_info.cpp +++ b/src/publisher_tracker_gnss_info.cpp @@ -29,24 +29,13 @@ namespace wolf PublisherTrackerGnssInfo::PublisherTrackerGnssInfo(const std::string& _unique_name, const ParamsServer& _server, - const ProblemPtr _problem) : + ProblemConstPtr _problem) : Publisher(_unique_name, _server, _problem), processor_tracker_gnss_(nullptr) { auto proc_name = _server.getParam<std::string>(prefix_ + "/processor_gnss"); - for (auto sen : _problem->getHardware()->getSensorList()) - { - for (auto proc : sen->getProcessorList()) - if (proc_name == proc->getName()) - { - processor_tracker_gnss_ = std::dynamic_pointer_cast<ProcessorTrackerGnss>(proc); - if (not processor_tracker_gnss_) - throw std::runtime_error("PublisherTrackerGnssInfo: the processor with name " + proc_name + " is not of type ProcessorTrackerGnss."); - break; - } - if (processor_tracker_gnss_) - break; - } + processor_tracker_gnss_ = std::dynamic_pointer_cast<const ProcessorTrackerGnss>(problem_->findProcessor(proc_name)); + if (not processor_tracker_gnss_) throw std::runtime_error("PublisherTrackerGnssInfo: the processor with name " + proc_name + " was not found."); }