diff --git a/.gitignore b/.gitignore
index 79b8fcfdbb9fe6dbfa89f2133c10fc67b8e18f1e..3cf468ee6d2a274afeef59699469fb8ad24354df 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,4 +1,8 @@
/.settings/
/.cproject
/.project
-/.clangd
\ No newline at end of file
+/.clangd
+.vscode/
+build-debug/
+build-release/
+build/
diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml
index b007cb18b8d79841bdb0b70ebe07d0eef58e8eba..05026060218f95900ef4517df78e099099b80c89 100644
--- a/.gitlab-ci.yml
+++ b/.gitlab-ci.yml
@@ -4,6 +4,25 @@ stages:
- deploy_packages
############ YAML ANCHORS ############
+.print_variables_template: &print_variables_definition
+ # Print variables
+ - echo $WOLF_IMU_BRANCH
+ - echo $WOLF_GNSS_BRANCH
+ - echo $WOLF_LASER_BRANCH
+ - echo $WOLF_VISION_BRANCH
+ - echo $WOLF_APRILTAG_BRANCH
+ - echo $WOLF_BODYDYNAMICS_BRANCH
+ - echo $GNSSUTILS_BRANCH
+ - echo $LASERSCANUTILS_BRANCH
+ - echo $CI_COMMIT_BRANCH
+ - echo $WOLF_ROS_NODE_BRANCH
+ - echo $WOLF_ROS_IMU_BRANCH
+ - echo $WOLF_ROS_GNSS_BRANCH
+ - echo $WOLF_ROS_LASER_BRANCH
+ - echo $WOLF_ROS_VISION_BRANCH
+ - echo $WOLF_ROS_APRILTAG_BRANCH
+ - echo $WOLF_ROS_BODYDYNAMICS_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)
@@ -99,8 +118,6 @@ license_headers:
stage: license
image: labrobotica/wolf_deps_ros:20.04
cache: []
- except:
- - master
before_script:
- *preliminaries_definition
- *install_wolf_definition
@@ -147,27 +164,42 @@ deploy_imu:
variables:
WOLF_CORE_BRANCH: $WOLF_CORE_BRANCH
WOLF_IMU_BRANCH: $WOLF_IMU_BRANCH
- WOLF_ROS_CORE_BRANCH: $CI_COMMIT_BRANCH
+ WOLF_ROS_NODE_BRANCH: $CI_COMMIT_BRANCH
trigger:
project: mobile_robotics/wolf_projects/wolf_ros/wolf_ros_imu
+ branch: $WOLF_ROS_IMU_BRANCH
deploy_gnss:
stage: deploy_packages
variables:
WOLF_CORE_BRANCH: $WOLF_CORE_BRANCH
WOLF_APRILTAG_BRANCH: $WOLF_GNSS_BRANCH
- WOLF_ROS_CORE_BRANCH: $CI_COMMIT_BRANCH
+ WOLF_ROS_NODE_BRANCH: $CI_COMMIT_BRANCH
+ GNSSUTILS_BRANCH: $GNSSUTILS_BRANCH
trigger:
project: mobile_robotics/wolf_projects/wolf_ros/wolf_ros_gnss
+ branch: $WOLF_ROS_GNSS_BRANCH
deploy_laser:
stage: deploy_packages
variables:
WOLF_CORE_BRANCH: $WOLF_CORE_BRANCH
WOLF_LASER_BRANCH: $WOLF_LASER_BRANCH
- WOLF_ROS_CORE_BRANCH: $CI_COMMIT_BRANCH
+ WOLF_ROS_NODE_BRANCH: $CI_COMMIT_BRANCH
+ LASERSCANUTILS_BRANCH: $LASERSCANUTILS_BRANCH
trigger:
project: mobile_robotics/wolf_projects/wolf_ros/wolf_ros_laser
+ branch: $WOLF_ROS_LASER_BRANCH
+
+deploy_vision:
+ stage: deploy_packages
+ variables:
+ WOLF_CORE_BRANCH: $WOLF_CORE_BRANCH
+ WOLF_VISION_BRANCH: $WOLF_VISION_BRANCH
+ WOLF_ROS_NODE_BRANCH: $CI_COMMIT_BRANCH
+ trigger:
+ project: mobile_robotics/wolf_projects/wolf_ros/wolf_ros_apriltag
+ branch: $WOLF_ROS_VISION_BRANCH
deploy_apriltag:
stage: deploy_packages
@@ -175,6 +207,7 @@ deploy_apriltag:
WOLF_CORE_BRANCH: $WOLF_CORE_BRANCH
WOLF_VISION_BRANCH: $WOLF_VISION_BRANCH
WOLF_APRILTAG_BRANCH: $WOLF_APRILTAG_BRANCH
- WOLF_ROS_CORE_BRANCH: $CI_COMMIT_BRANCH
+ WOLF_ROS_NORE_BRANCH: $CI_COMMIT_BRANCH
trigger:
project: mobile_robotics/wolf_projects/wolf_ros/wolf_ros_apriltag
+ branch: $WOLF_ROS_APRILTAG_BRANCH
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 1851015a20181da59c2d94b5c351bcbe88888b42..dfb477604a3919687abde4cd76fd6618c185bacd 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,13 +1,11 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.10)
project(wolf_ros_node)
-## 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
roslib
roscpp
@@ -17,104 +15,18 @@ find_package(catkin REQUIRED COMPONENTS
tf
tf_conversions
tf2_ros
-# dynamic_reconfigure
)
## System dependencies are found with CMake's conventions
-# find_package(Boost REQUIRED COMPONENTS system)
-# find_package(Ceres REQUIRED)
-# find_package(Eigen3 REQUIRED)
find_package(wolfcore 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
-# sensor_msgs# std_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
+ INCLUDE_DIRS include
)
###########
@@ -123,121 +35,36 @@ INCLUDE_DIRS include
## Specify additional locations of header files
## Your package locations should be listed before other locations
-message("Wolf include path: ${wolfcore_INCLUDE_DIRS}")
include_directories(
- include
- ${EIGEN_INCLUDE_DIRS}
- ${wolfcore_INCLUDE_DIRS}
+ include
${catkin_INCLUDE_DIRS}
- ${CERES_INCLUDE_DIRS}
)
-# link_directories(/usr/local/lib/iri-algorithms)
-## Declare a C++ library
-# add_library(${PROJECT_NAME}
-# src/${PROJECT_NAME}/wolf_ros.cpp
-# )
-
-## 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
add_executable(${PROJECT_NAME} src/node.cpp)
add_library(subscriber_${PROJECT_NAME}
- src/subscriber_diffdrive.cpp
- src/subscriber_odom2d.cpp)
-add_library(publisher_${PROJECT_NAME}
- src/publisher_graph.cpp
- src/publisher_pose.cpp
- src/publisher_trajectory.cpp
- src/publisher_state_block.cpp
- src/publisher_tf.cpp)
+ src/subscriber_diffdrive.cpp
+ src/subscriber_odom2d.cpp)
-## 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} ${PROJECT_NAME}_gencfg)
+add_library(publisher_${PROJECT_NAME}
+ src/publisher_graph.cpp
+ src/publisher_pose.cpp
+ src/publisher_trajectory.cpp
+ src/publisher_state_block.cpp
+ src/publisher_tf.cpp)
## Specify libraries to link a library or executable target against
target_link_libraries(subscriber_${PROJECT_NAME}
${catkin_LIBRARIES}
- ${CERES_LIBRARIES}
- ${wolfcore_LIBRARIES}
- yaml-cpp
- dl
+ wolfcore
)
target_link_libraries(publisher_${PROJECT_NAME}
${catkin_LIBRARIES}
- ${CERES_LIBRARIES}
- ${wolfcore_LIBRARIES}
- yaml-cpp
- dl
+ wolfcore
)
target_link_libraries(${PROJECT_NAME}
${catkin_LIBRARIES}
- ${CERES_LIBRARIES}
- ${wolfcore_LIBRARIES}
- yaml-cpp
- dl
- )
-
-#############
-## 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
-# )
-
-## Mark other files for installation (e.g. launch and bag files, etc.)
-# install(FILES
-# # myfile1
-# # myfile2
-# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-# )
-
-#############
-## 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()
-
-## Add folders to be run by python nosetests
-# catkin_add_nosetests(test)
+ wolfcore
+ )
\ No newline at end of file
diff --git a/include/factory_publisher.h b/include/factory_publisher.h
index 79f2d1b86bfde30b0d3282f06a28a3b209c0bde6..1911c43b42d8a01516c615223b8f42a237178a0e 100644
--- a/include/factory_publisher.h
+++ b/include/factory_publisher.h
@@ -176,7 +176,7 @@ class Publisher;
typedef Factory<Publisher,
const std::string&,
const ParamsServer&,
- const ProblemPtr,
+ ProblemConstPtr,
ros::NodeHandle&> FactoryPublisher;
template<>
diff --git a/include/node.h b/include/node.h
index 0af71ac04df2154ae3a9ee4e4428b4b201d14388..8324843dbbc865276bb527563164660e32e5b19a 100644
--- a/include/node.h
+++ b/include/node.h
@@ -61,7 +61,6 @@
#include "publisher.h"
using namespace wolf;
-using namespace std;
class WolfRosNode
{
@@ -79,7 +78,7 @@ class WolfRosNode
protected:
- std::vector<std::shared_ptr<Loader>> loaders_;
+ //std::vector<std::shared_ptr<Loader>> loaders_;
// solver
SolverManagerPtr solver_;
diff --git a/include/publisher.h b/include/publisher.h
index 822b2e4158266ed1f0280cd295b4ea639933c362..e86ad782577d46932d9ff9f61d21ab6c78f7be4a 100644
--- a/include/publisher.h
+++ b/include/publisher.h
@@ -48,14 +48,14 @@ WOLF_PTR_TYPEDEFS(Publisher);
*
* PublisherClass(const std::string& _unique_name,
* const ParamsServer& _server,
- * const ProblemPtr _problem);
+ * ProblemConstPtr _problem);
*/
#define WOLF_PUBLISHER_CREATE(PublisherClass) \
static PublisherPtr create(const std::string& _unique_name, \
const ParamsServer& _server, \
- const ProblemPtr _problem, \
+ ProblemConstPtr _problem, \
ros::NodeHandle& _nh) \
- { \
+{ \
PublisherPtr pub = std::make_shared<PublisherClass>(_unique_name, _server, _problem); \
pub->initialize(_nh, pub->getTopic()); \
return pub; \
@@ -67,7 +67,7 @@ class Publisher
Publisher(const std::string& _unique_name,
const ParamsServer& _server,
- const ProblemPtr _problem) :
+ ProblemConstPtr _problem) :
problem_(_problem),
first_publish_time_(ros::Time(0)),
last_n_period_(0),
@@ -99,14 +99,16 @@ class Publisher
std::string getName() const;
+ void printProfiling(std::ostream& stream = std::cout) const;
+
protected:
template<typename T>
T getParamWithDefault(const ParamsServer &_server,
- const std::string &_param_name,
- const T _default_value) const;
+ const std::string &_param_name,
+ const T _default_value) const;
- ProblemPtr problem_;
+ ProblemConstPtr problem_;
ros::Publisher publisher_;
double period_;
ros::Time first_publish_time_;
@@ -121,9 +123,6 @@ class Publisher
unsigned int n_publish_;
std::chrono::microseconds acc_duration_;
std::chrono::microseconds max_duration_;
-
- public:
- void printProfiling(std::ostream& stream = std::cout) const;
};
inline std::string Publisher::getTopic() const
diff --git a/include/publisher_graph.h b/include/publisher_graph.h
index de5d2e4c966526dd6c435dd5f2c7308149c7b11d..22bdd40aa09890400c3dcf71640b1f4ae573ce42 100644
--- a/include/publisher_graph.h
+++ b/include/publisher_graph.h
@@ -42,8 +42,8 @@ class PublisherGraph: public Publisher
{
public:
PublisherGraph(const std::string& _unique_name,
- const ParamsServer& _server,
- const ProblemPtr _problem);
+ const ParamsServer& _server,
+ ProblemConstPtr _problem);
WOLF_PUBLISHER_CREATE(PublisherGraph);
virtual ~PublisherGraph(){};
diff --git a/include/publisher_pose.h b/include/publisher_pose.h
index 6005454986066cc185241ccd159fba280903fc50..1e7428f959bc7fe78654e9266345e631dc658299 100644
--- a/include/publisher_pose.h
+++ b/include/publisher_pose.h
@@ -51,7 +51,7 @@ class PublisherPose: public Publisher
visualization_msgs::Marker marker_msg_;
geometry_msgs::PoseWithCovarianceStamped pose_with_cov_msg_;
std_msgs::ColorRGBA marker_color_;
- SensorBasePtr sensor_;
+ SensorBaseConstPtr sensor_;
std::string frame_id_, map_frame_id_;
ros::Publisher pub_pose_array_, pub_marker_, pub_pose_with_cov_;
@@ -59,7 +59,7 @@ class PublisherPose: public Publisher
public:
PublisherPose(const std::string& _unique_name,
const ParamsServer& _server,
- const ProblemPtr _problem);
+ ProblemConstPtr _problem);
WOLF_PUBLISHER_CREATE(PublisherPose);
virtual ~PublisherPose(){};
diff --git a/include/publisher_state_block.h b/include/publisher_state_block.h
index 053dc40b7f22fbfbaf8e0834e105cc3baf4a0e18..78bd104683cecc9a68bbbdc08bb8871d97d83a15 100644
--- a/include/publisher_state_block.h
+++ b/include/publisher_state_block.h
@@ -42,14 +42,14 @@ class PublisherStateBlock: public Publisher
{
protected:
std_msgs::Float64MultiArray state_msg_;
- SensorBasePtr sensor_;
+ SensorBaseConstPtr sensor_;
char key_;
bool msg_init_;
public:
PublisherStateBlock(const std::string& _unique_name,
- const ParamsServer& _server,
- const ProblemPtr _problem);
+ const ParamsServer& _server,
+ ProblemConstPtr _problem);
WOLF_PUBLISHER_CREATE(PublisherStateBlock);
virtual ~PublisherStateBlock(){};
diff --git a/include/publisher_tf.h b/include/publisher_tf.h
index 4b4eff9d35c3051a1dec6417106337519e4ba2a0..333676b43bf7c1a0da7317722431685d4e49fed8 100644
--- a/include/publisher_tf.h
+++ b/include/publisher_tf.h
@@ -75,8 +75,8 @@ class PublisherTf: public Publisher
public:
PublisherTf(const std::string& _unique_name,
- const ParamsServer& _server,
- const ProblemPtr _problem);
+ const ParamsServer& _server,
+ ProblemConstPtr _problem);
WOLF_PUBLISHER_CREATE(PublisherTf);
virtual ~PublisherTf(){};
diff --git a/include/publisher_trajectory.h b/include/publisher_trajectory.h
index 53f3340a1519fa7fc31df1df78cd5df0aed2ad0a..bd26b6cc366d3dfd139049f6ddf4cd9ac612f246 100644
--- a/include/publisher_trajectory.h
+++ b/include/publisher_trajectory.h
@@ -52,8 +52,8 @@ class PublisherTrajectory: public Publisher
public:
PublisherTrajectory(const std::string& _unique_name,
- const ParamsServer& _server,
- const ProblemPtr _problem);
+ const ParamsServer& _server,
+ ProblemConstPtr _problem);
WOLF_PUBLISHER_CREATE(PublisherTrajectory);
virtual ~PublisherTrajectory(){};
diff --git a/include/subscriber.h b/include/subscriber.h
index 806a834dcf139a288383d44d0612ab3512b04e6d..0db5b5cad5158ef5aaa4e8371e03e2be0cb21d5a 100644
--- a/include/subscriber.h
+++ b/include/subscriber.h
@@ -131,7 +131,7 @@ inline double Subscriber::secondsSinceLastCallback()
{
if (last_stamp_ == ros::Time(0))
{
- WOLF_WARN("Subscriber: 'last_stamp_` not initialized. No messages have been received or ", name_, " is not updating headers (be sure to add 'updateLastHeader(msg->header)' in your subscriber callback).");
+ WOLF_WARN("[", name_, "]: 'last_stamp_` not initialized. No messages have been received or ", name_, " is not updating headers (be sure to add 'updateLastHeader(msg->header)' in your subscriber callback).");
return 0;
}
return (ros::Time::now() - last_stamp_).toSec();
@@ -162,7 +162,7 @@ inline T Subscriber::getParamWithDefault(const ParamsServer &_server,
}
catch (...)
{
- WOLF_INFO("Subscriber: Parameter ", _param_name, " is missing. Taking default value: ", _default_value);
+ WOLF_INFO("[", name_, "]: Parameter ", _param_name, " is missing. Taking default value: ", _default_value);
return _default_value;
}
}
diff --git a/src/node.cpp b/src/node.cpp
index 1b78215f1d2065fc4032fe87f04837f7f1016201..910cfdacd7ce47c71426bc8553d4dbf32cb9feea 100644
--- a/src/node.cpp
+++ b/src/node.cpp
@@ -71,30 +71,25 @@ WolfRosNode::WolfRosNode()
#else
std::string lib_extension = ".so";
#endif
- for (auto subscriber_name : server.getParam<std::vector<std::string>>("packages_subscriber")) {
- std::string subscriber = packages_path + "/libsubscriber_" + subscriber_name + lib_extension;
- WOLF_TRACE("Loading subscriber " + subscriber_name + " via " + subscriber);
- auto l = std::make_shared<LoaderRaw>(subscriber);
- l->load();
- loaders_.push_back(l);
- }
- for (auto publisher_name : server.getParam<std::vector<std::string>>("packages_publisher")) {
- std::string publisher = packages_path + "/libpublisher_" + publisher_name + lib_extension;
- WOLF_TRACE("Loading publisher " + publisher_name + " via " + publisher);
- auto l = std::make_shared<LoaderRaw>(publisher);
- l->load();
- loaders_.push_back(l);
- }
// PUBLISHERS
ROS_INFO("Creating publishers...");
for (auto it : server.getParam<std::vector<std::map<std::string, std::string>>>("ROS publisher"))
{
- std::string publisher = it["type"];
- std::string topic = it["topic"];
- WOLF_INFO("Pub: ", publisher, " name: ", publisher+" - "+topic);
- publishers_.push_back(FactoryPublisher::create(publisher,
- publisher+" - "+topic,
+ std::string type = it["type"];
+ std::string topic = it["topic"];
+ std::string package = it["package"];
+ std::string name = type + " - " + topic;
+ std::string lib_publisher = packages_path + "/libpublisher_" + package + lib_extension;
+
+ WOLF_TRACE("Loading publisher " + type + " via " + lib_publisher);
+ auto l = std::make_shared<LoaderRaw>(lib_publisher);
+ l->load();
+ //loaders_.push_back(l);
+
+ WOLF_INFO("Pub: ", type, " name: ", name);
+ publishers_.push_back(FactoryPublisher::create(type,
+ name,
server,
problem_ptr_,
nh_));
@@ -104,14 +99,23 @@ WolfRosNode::WolfRosNode()
ROS_INFO("Creating subscribers...");
for (auto it : server.getParam<std::vector<std::map<std::string, std::string>>>("ROS subscriber"))
{
- std::string subscriber = it["type"];
- std::string topic = it["topic"];
- std::string sensor = it["sensor_name"];
- WOLF_TRACE("From sensor {" + sensor + "} subscribing {" + subscriber + "} to {" + topic + "} topic");
- subscribers_.push_back(FactorySubscriber::create(subscriber,
- subscriber+" - " + topic,
+ std::string type = it["type"];
+ std::string topic = it["topic"];
+ std::string sensor = it["sensor_name"];
+ std::string package = it["package"];
+ std::string name = type + " - " + topic;
+ std::string lib_subscriber = packages_path + "/libsubscriber_" + package + lib_extension;
+
+ WOLF_TRACE("Loading subscriber " + type + " via " + lib_subscriber);
+ auto l = std::make_shared<LoaderRaw>(lib_subscriber);
+ l->load();
+ //loaders_.push_back(l);
+
+ WOLF_TRACE("From sensor {" + sensor + "} subscribing {" + type + "} to {" + topic + "} topic");
+ subscribers_.push_back(FactorySubscriber::create(type,
+ name,
server,
- problem_ptr_->getSensor(sensor),
+ problem_ptr_->findSensor(sensor),
nh_));
}
diff --git a/src/publisher_graph.cpp b/src/publisher_graph.cpp
index 19ba3608f858726e99dc12ebe9bf705ef37d5ba9..562634885f86c85246bfd60f978a5154ff43c326 100644
--- a/src/publisher_graph.cpp
+++ b/src/publisher_graph.cpp
@@ -28,7 +28,7 @@ namespace wolf
PublisherGraph::PublisherGraph(const std::string& _unique_name,
const ParamsServer& _server,
- const ProblemPtr _problem) :
+ ProblemConstPtr _problem) :
Publisher(_unique_name, _server, _problem)
{
Eigen::Vector4d color;
@@ -209,7 +209,7 @@ PublisherGraph::PublisherGraph(const std::string& _unique_name,
landmark_marker_.scale.x = viz_scale_*landmark_length_;
landmark_marker_.scale.y = viz_scale_*landmark_width_;
landmark_marker_.scale.z = viz_scale_*landmark_width_;
- landmark_marker_.color.a = 0.5;
+ landmark_marker_.color.a = 1.0;
landmark_text_marker_ = landmark_marker_;
landmark_text_marker_.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
landmark_text_marker_.ns = "landmarks_text";
@@ -307,7 +307,7 @@ void PublisherGraph::publishFactors()
factors_marker_array_.markers.front().action = visualization_msgs::Marker::DELETEALL;
// Get a list of factors of the trajectory (discarded all prior factors for extrinsics/intrinsics..)
- FactorBasePtrList fac_list;
+ FactorBaseConstPtrList fac_list;
problem_->getTrajectory()->getFactorList(fac_list);
// reset previously drawn factors
@@ -356,9 +356,11 @@ void PublisherGraph::publishTrajectory()
// Iterate over the key frames
int marker_i = 0;
- auto trajectory = *problem_->getTrajectory();
- for (auto frm : trajectory)
+ auto frame_map = problem_->getTrajectory()->getFrameMap();
+ for (auto frm_pair : frame_map)
{
+ auto frm = frm_pair.second;
+
// Try to fill marker
if (not fillFrameMarker(frm, frame_marker, frame_text_marker))
continue;
@@ -443,11 +445,21 @@ bool PublisherGraph::fillLandmarkMarkers(LandmarkBaseConstPtr lmk,
// POSITION & ORIENTATION ------------------------------------------------------
// position
- lmk_marker.pose.position.x = lmk->getP()->getState()(0);
- lmk_marker.pose.position.y = lmk->getP()->getState()(1);
- if (lmk->getP()->getSize() > 2)
- lmk_marker.pose.position.z = lmk->getP()->getState()(2);
-
+ Eigen::VectorXd lmk_pos;
+ lmk_pos = lmk->getP()->getState();
+ lmk_marker.pose.position.x = lmk_pos(0);
+ lmk_marker.pose.position.y = lmk_pos(1);
+ if (lmk_pos.size() > 2) // this is a 3d point
+ {
+ lmk_marker.pose.position.z = lmk_pos(2);
+ if (lmk_pos.size() == 4) // this is a homogeneous 3d point with 4 numbers. Divide xyz by last element:
+ {
+ double w = lmk_pos(3);
+ lmk_marker.pose.position.x /= w;
+ lmk_marker.pose.position.y /= w;
+ lmk_marker.pose.position.z /= w;
+ }
+ }
// orientation
if (lmk->getO() != nullptr)
{
@@ -463,6 +475,13 @@ bool PublisherGraph::fillLandmarkMarkers(LandmarkBaseConstPtr lmk,
else
lmk_marker.pose.orientation = tf::createQuaternionMsgFromYaw(lmk->getO()->getState()(0));
}
+ else
+ {
+ lmk_marker.pose.orientation.x = 0;
+ lmk_marker.pose.orientation.y = 0;
+ lmk_marker.pose.orientation.z = 0;
+ lmk_marker.pose.orientation.w = 1;
+ }
// TEXT MARKER ------------------------------------------------------
lmk_text_marker.text = std::to_string(lmk->id());
@@ -504,36 +523,36 @@ bool PublisherGraph::fillFactorMarker(FactorBaseConstPtr fac,
if (fac->getFrameOther() != nullptr)
{
// special case: Motion from ProcessorMotion
- auto proc_motion = std::dynamic_pointer_cast<ProcessorMotion>(fac->getProcessor());
+ auto proc_motion = std::dynamic_pointer_cast<const ProcessorMotion>(fac->getProcessor());
if (proc_motion and fac->getCaptureOther())
{
// Get state of other
- const auto& x_other = fac->getFrameOther()->getState(proc_motion->getStateStructure());
+ auto x_other = fac->getFrameOther()->getState(proc_motion->getStateStructure());
// Get most recent motion
- const auto& cap_own = std::static_pointer_cast<CaptureMotion>(fac->getFeature()->getCapture());
+ auto cap_own = std::static_pointer_cast<const CaptureMotion>(fac->getFeature()->getCapture());
const auto& motion = cap_own->getBuffer().back();
// Get delta preintegrated up to now
const auto& delta_preint = motion.delta_integr_;
// Get calibration preint -- stored in last capture
- const auto& calib_preint = cap_own->getCalibrationPreint();
+ auto calib_preint = cap_own->getCalibrationPreint();
VectorComposite state_integrated;
if ( proc_motion->hasCalibration())
{
// Get current calibration -- from other capture
- const auto& calib = proc_motion->getCalibration(fac->getCaptureOther());
+ auto calib = proc_motion->getCalibration(fac->getCaptureOther());
// get Jacobian of delta wrt calibration
const auto& J_delta_calib = motion.jacobian_calib_;
// compute delta change
- const auto& delta_step = J_delta_calib * (calib - calib_preint);
+ auto delta_step = J_delta_calib * (calib - calib_preint);
// correct delta // this is (+)
- const auto& delta_corrected = proc_motion->correctDelta(delta_preint, delta_step);
+ auto delta_corrected = proc_motion->correctDelta(delta_preint, delta_step);
// compute current state // this is [+]
proc_motion->statePlusDelta(x_other, delta_corrected, cap_own->getTimeStamp() - fac->getCaptureOther()->getTimeStamp(), state_integrated);
@@ -614,10 +633,22 @@ bool PublisherGraph::fillFactorMarker(FactorBaseConstPtr fac,
not fac->getLandmarkOther()->getP())
return false;
- point2.x = fac->getLandmarkOther()->getP()->getState()(0);
- point2.y = fac->getLandmarkOther()->getP()->getState()(1);
+ Eigen::VectorXd lmk_pos;
+ lmk_pos = fac->getLandmarkOther()->getP()->getState();
+ point2.x = lmk_pos(0);
+ point2.y = lmk_pos(1);
if (fac->getProblem()->getDim() == 3)
- point2.z = fac->getLandmarkOther()->getP()->getState()(2);
+ {
+ point2.z = lmk_pos(2);
+ if (lmk_pos.size() == 4)
+ {
+ // homogeneous 3d point. Divide XYZ by 4th component
+ double w = lmk_pos(3);
+ point2.x /= w;
+ point2.y /= w;
+ point2.z /= w;
+ }
+ }
else
point2.z = 0;
}
diff --git a/src/publisher_pose.cpp b/src/publisher_pose.cpp
index 7c85dc12794aa3b7111892328b55b5820258dd64..abcecb335300a170dfc443f26511f4b30f6dfbd1 100644
--- a/src/publisher_pose.cpp
+++ b/src/publisher_pose.cpp
@@ -33,7 +33,7 @@ namespace wolf
PublisherPose::PublisherPose(const std::string& _unique_name,
const ParamsServer& _server,
- const ProblemPtr _problem) :
+ ProblemConstPtr _problem) :
Publisher(_unique_name, _server, _problem)
{
Eigen::Vector4d marker_color_v;
@@ -54,7 +54,7 @@ PublisherPose::PublisherPose(const std::string& _unique_name,
extrinsics_ = _server.getParam<bool>(prefix_ + "/extrinsics");
if (extrinsics_)
- 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_state_block.cpp b/src/publisher_state_block.cpp
index bc39947dab7bd5d0691e99505b4916da308ee32b..8300340954bef448333c990eb8ab2462ad4016bc 100644
--- a/src/publisher_state_block.cpp
+++ b/src/publisher_state_block.cpp
@@ -25,12 +25,12 @@ namespace wolf
{
PublisherStateBlock::PublisherStateBlock(const std::string& _unique_name,
- const ParamsServer& _server,
- const ProblemPtr _problem) :
+ const ParamsServer& _server,
+ ProblemConstPtr _problem) :
Publisher(_unique_name, _server, _problem),
msg_init_(false)
{
- sensor_ = _problem->getSensor(_server.getParam<std::string>(prefix_ + "/sensor"));
+ sensor_ = _problem->findSensor(_server.getParam<std::string>(prefix_ + "/sensor"));
assert(sensor_);
key_ = _server.getParam<char>(prefix_ + "/key");
}
diff --git a/src/publisher_tf.cpp b/src/publisher_tf.cpp
index 044f820a1646195c3a9406607c35eb1e90de00a3..2591644b7278b9a0c1dbf2e174c2834afd03a061 100644
--- a/src/publisher_tf.cpp
+++ b/src/publisher_tf.cpp
@@ -33,7 +33,7 @@ namespace wolf
PublisherTf::PublisherTf(const std::string& _unique_name,
const ParamsServer& _server,
- const ProblemPtr _problem) :
+ ProblemConstPtr _problem) :
Publisher(_unique_name, _server, _problem),
state_available_(true)
{
diff --git a/src/publisher_trajectory.cpp b/src/publisher_trajectory.cpp
index e220449a651bab3f8efd2876ffd1e45adbaeafee..85ba93583df987e18f34abfacb27c9532594551a 100644
--- a/src/publisher_trajectory.cpp
+++ b/src/publisher_trajectory.cpp
@@ -37,8 +37,8 @@ namespace wolf
{
PublisherTrajectory::PublisherTrajectory(const std::string& _unique_name,
- const ParamsServer& _server,
- const ProblemPtr _problem) :
+ const ParamsServer& _server,
+ ProblemConstPtr _problem) :
Publisher(_unique_name, _server, _problem)
{
frame_id_ = _server.getParam<std::string>(prefix_ + "/frame_id");
@@ -65,7 +65,6 @@ void PublisherTrajectory::publishTrajectory()
{
path_msg_.header.stamp = ros::Time::now();
- auto trajectory = problem_->getTrajectory();
int frame_num = 0;
//Fill path message with PoseStamped from trajectory
@@ -73,7 +72,8 @@ void PublisherTrajectory::publishTrajectory()
Eigen::Vector3d p = Eigen::Vector3d::Zero();
Eigen::Quaterniond q;
- for (auto frm: trajectory->getFrameMap())
+ auto frame_map = problem_->getTrajectory()->getFrameMap();
+ for (auto frm : frame_map)
{
auto loc_ts = frm.first;
framepose.header.frame_id = frame_id_;