Skip to content

GitLab

Commit 6579ecfc authored by Marius's avatar Marius
Browse files
Options

ad

parent 5546665b
Hide whitespace changes
Inline Side-by-side
Showing with 537 additions and 75 deletions
.idea/workspace.xml
View file @ 6579ecfc
......@@ -35,6 +35,7 @@
<config projectName="crs" targetName="nodelet_generate_messages_eus" />
<config projectName="crs" targetName="bond_generate_messages_py" />
<config projectName="crs" targetName="_crs_generate_messages_check_deps_ER1ControlMsgStudent" />
<config projectName="crs" targetName="crs_registration_icp_student" />
<config projectName="crs" targetName="crs_generate_messages_cpp" />
<config projectName="crs" targetName="crs_registration_icp" />
<config projectName="crs" targetName="std_srvs_generate_messages_nodejs" />
......@@ -133,9 +134,8 @@
<list default="true" id="70f5d2b5-e1c9-415f-bbb3-eae2e165b096" name="Default Changelist" comment="Merge branch 'master' of https://fsstud.is.uni-due.de:8090/sfmabock/crs&#10;&#10;# Conflicts:&#10;#&#9;src/er1_driver_node/main.cpp&#10;#&#9;src/point_processing_node/main.cpp&#10;#&#9;src/point_processing_node_student/main.cpp">
<change beforePath="$PROJECT_DIR$/.idea/workspace.xml" beforeDir="false" afterPath="$PROJECT_DIR$/.idea/workspace.xml" afterDir="false" />
<change beforePath="$PROJECT_DIR$/CMakeLists.txt" beforeDir="false" afterPath="$PROJECT_DIR$/CMakeLists.txt" afterDir="false" />
<change beforePath="$PROJECT_DIR$/include/registration_feature/helper.hpp" beforeDir="false" />
<change beforePath="$PROJECT_DIR$/src/registration_feature/helper.cpp" beforeDir="false" />
<change beforePath="$PROJECT_DIR$/src/registration_feature/main.cpp" beforeDir="false" />
<change beforePath="$PROJECT_DIR$/include/registration_icp/helper.hpp" beforeDir="false" />
<change beforePath="$PROJECT_DIR$/src/registration_icp/helper.cpp" beforeDir="false" />
<change beforePath="$PROJECT_DIR$/src/registration_icp/main.cpp" beforeDir="false" afterPath="$PROJECT_DIR$/src/registration_icp/main.cpp" afterDir="false" />
</list>
<ignored path="$PROJECT_DIR$/cmake-build-debug/" />
......@@ -151,70 +151,76 @@
<component name="FileEditorManager">
<leaf SIDE_TABS_SIZE_LIMIT_KEY="300">
<file pinned="false" current-in-tab="false">
<entry file="file://$PROJECT_DIR$/src/er1_driver_node/main.cpp">
<entry file="file://$PROJECT_DIR$/src/registration_icp/main.cpp">
<provider selected="true" editor-type-id="text-editor">
<state relative-caret-position="1170">
<caret line="78" selection-start-line="78" selection-end-line="78" />
<state relative-caret-position="203">
<caret line="28" selection-start-line="28" selection-end-line="28" />
<folding>
<element signature="e#0#20#0" expanded="true" />
</folding>
</state>
</provider>
</entry>
</file>
<file pinned="false" current-in-tab="false">
<entry file="file://$PROJECT_DIR$/src/point_processing_node_student/main.cpp">
<file pinned="false" current-in-tab="true">
<entry file="file://$PROJECT_DIR$/CMakeLists.txt">
<provider selected="true" editor-type-id="text-editor">
<state relative-caret-position="510">
<caret line="34" column="55" selection-start-line="34" selection-start-column="55" selection-end-line="34" selection-end-column="55" />
<folding>
<element signature="e#0#20#0" expanded="true" />
</folding>
<state relative-caret-position="291">
<caret line="66" selection-start-line="66" selection-end-line="66" />
</state>
</provider>
</entry>
</file>
<file pinned="false" current-in-tab="false">
<entry file="file://$PROJECT_DIR$/../../../pcl/pcl-bin/include/pcl-1.9/pcl/registration/impl/registration.hpp">
<entry file="file://$PROJECT_DIR$/src/registration_icp_student/main.cpp">
<provider selected="true" editor-type-id="text-editor">
<state relative-caret-position="2295">
<caret line="153" column="53" selection-start-line="153" selection-start-column="53" selection-end-line="153" selection-end-column="53" />
<state relative-caret-position="278">
<caret line="23" column="30" selection-start-line="23" selection-start-column="30" selection-end-line="23" selection-end-column="30" />
<folding>
<element signature="e#0#20#0" expanded="true" />
</folding>
</state>
</provider>
</entry>
</file>
<file pinned="false" current-in-tab="false">
<entry file="file://$PROJECT_DIR$/src/point_processing_node/main.cpp">
<entry file="file://$PROJECT_DIR$/src/comon/pclhelper.cpp">
<provider selected="true" editor-type-id="text-editor">
<state relative-caret-position="1005">
<caret line="67" selection-start-line="67" selection-end-line="67" />
<state relative-caret-position="30">
<caret line="2" selection-start-line="2" selection-end-line="2" />
</state>
</provider>
</entry>
</file>
<file pinned="false" current-in-tab="true">
<entry file="file://$PROJECT_DIR$/src/registration_icp/main.cpp">
<file pinned="false" current-in-tab="false">
<entry file="file://$PROJECT_DIR$/include/comon/pclhelper.hpp">
<provider selected="true" editor-type-id="text-editor">
<state relative-caret-position="264">
<caret line="86" column="62" selection-start-line="86" selection-start-column="62" selection-end-line="86" selection-end-column="62" />
<state relative-caret-position="150">
<caret line="10" column="46" selection-start-line="10" selection-start-column="46" selection-end-line="10" selection-end-column="46" />
<folding>
<element signature="e#0#20#0" expanded="true" />
<element signature="e#14#46#0" expanded="true" />
</folding>
</state>
</provider>
</entry>
</file>
<file pinned="false" current-in-tab="false">
<entry file="file://$PROJECT_DIR$/src/registration_icp/helper.cpp">
<entry file="file://$PROJECT_DIR$/src/comon/rosbaghelper.cpp">
<provider selected="true" editor-type-id="text-editor">
<state relative-caret-position="-810">
<caret column="26" selection-start-column="26" selection-end-column="26" />
<state>
<caret column="32" selection-start-column="32" selection-end-column="32" />
</state>
</provider>
</entry>
</file>
<file pinned="false" current-in-tab="false">
<entry file="file://$PROJECT_DIR$/../../../pcl/pcl-bin/include/pcl-1.9/pcl/registration/impl/icp.hpp">
<entry file="file://$PROJECT_DIR$/include/comon/rosbaghelper.hpp">
<provider selected="true" editor-type-id="text-editor">
<state relative-caret-position="3240">
<caret line="216" column="25" selection-start-line="216" selection-start-column="25" selection-end-line="216" selection-end-column="25" />
<state relative-caret-position="450">
<caret line="30" selection-start-line="30" selection-end-line="30" />
<folding>
<element signature="e#14#31#0" expanded="true" />
</folding>
</state>
</provider>
</entry>
......@@ -236,10 +242,10 @@
<find>tf_publisher_node</find>
<find>tf_publisher_node_</find>
<find>point_processing_node_student</find>
<find>registration_icp</find>
<find>PointCloud&lt;PointXYZRGB&gt;</find>
<find>PointXYZRGB</find>
<find>transformation_epsilon_</find>
<find>registration_icp</find>
</findStrings>
<replaceStrings>
<replace>tf_publisher</replace>
......@@ -249,6 +255,7 @@
<replace>registration_feature</replace>
<replace>CloudRGBA</replace>
<replace>PointXYZRGBA</replace>
<replace>registration_icp_student</replace>
</replaceStrings>
</component>
<component name="Git.Settings">
......@@ -263,15 +270,21 @@
<option value="$PROJECT_DIR$/include/comon/clockhelper.hpp" />
<option value="$PROJECT_DIR$/src/tf_publisher_node_student/main.cpp" />
<option value="$PROJECT_DIR$/src/tf_publisher_node/main.cpp" />
<option value="$PROJECT_DIR$/CMakeLists.txt" />
<option value="$PROJECT_DIR$/src/er1_driver_node/main.cpp" />
<option value="$PROJECT_DIR$/src/point_processing_node/main.cpp" />
<option value="$PROJECT_DIR$/src/point_processing_node_student/main.cpp" />
<option value="$PROJECT_DIR$/src/registration_feature/helper.cpp" />
<option value="$PROJECT_DIR$/include/registration_feature/helper.hpp" />
<option value="$PROJECT_DIR$/src/registration_feature/main.cpp" />
<option value="$PROJECT_DIR$/src/comon/pclhelper.cpp" />
<option value="$PROJECT_DIR$/include/comon/pclhelper.hpp" />
<option value="$PROJECT_DIR$/src/registration_icp/helper.cpp" />
<option value="$PROJECT_DIR$/src/comon/rosbaghelper.cpp" />
<option value="$PROJECT_DIR$/include/registration_icp/helper.hpp" />
<option value="$PROJECT_DIR$/include/comon/rosbaghelper.hpp" />
<option value="$PROJECT_DIR$/src/registration_icp_student/main.cpp" />
<option value="$PROJECT_DIR$/src/registration_icp/main.cpp" />
<option value="$PROJECT_DIR$/CMakeLists.txt" />
</list>
</option>
</component>
......@@ -305,6 +318,13 @@
<item name="crs" type="462c0819:PsiDirectoryNode" />
<item name="include" type="462c0819:PsiDirectoryNode" />
</path>
<path>
<item name="crs" type="b2602c69:ProjectViewProjectNode" />
<item name="crs" type="47feb1d3:ProjectViewModuleNode" />
<item name="crs" type="462c0819:PsiDirectoryNode" />
<item name="include" type="462c0819:PsiDirectoryNode" />
<item name="comon" type="462c0819:PsiDirectoryNode" />
</path>
<path>
<item name="crs" type="b2602c69:ProjectViewProjectNode" />
<item name="crs" type="47feb1d3:ProjectViewModuleNode" />
......@@ -353,6 +373,13 @@
<item name="src" type="462c0819:PsiDirectoryNode" />
<item name="registration_icp" type="462c0819:PsiDirectoryNode" />
</path>
<path>
<item name="crs" type="b2602c69:ProjectViewProjectNode" />
<item name="crs" type="47feb1d3:ProjectViewModuleNode" />
<item name="crs" type="462c0819:PsiDirectoryNode" />
<item name="src" type="462c0819:PsiDirectoryNode" />
<item name="registration_icp_student" type="462c0819:PsiDirectoryNode" />
</path>
<path>
<item name="crs" type="b2602c69:ProjectViewProjectNode" />
<item name="crs" type="47feb1d3:ProjectViewModuleNode" />
......@@ -567,6 +594,11 @@
<option name="com.jetbrains.cidr.execution.CidrBuildBeforeRunTaskProvider$BuildBeforeRunTask" enabled="true" />
</method>
</configuration>
<configuration name="crs_registration_icp_student" type="CMakeRunConfiguration" factoryName="Application" PASS_PARENT_ENVS_2="true" PROJECT_NAME="crs" TARGET_NAME="crs_registration_icp_student" CONFIG_NAME="Debug" RUN_TARGET_PROJECT_NAME="crs" RUN_TARGET_NAME="crs_registration_icp_student">
<method v="2">
<option name="com.jetbrains.cidr.execution.CidrBuildBeforeRunTaskProvider$BuildBeforeRunTask" enabled="true" />
</method>
</configuration>
<configuration name="crs_tf_publisher_node" type="CMakeRunConfiguration" factoryName="Application" PASS_PARENT_ENVS_2="true" PROJECT_NAME="crs" TARGET_NAME="crs_tf_publisher_node" CONFIG_NAME="Debug" RUN_TARGET_PROJECT_NAME="crs" RUN_TARGET_NAME="crs_tf_publisher_node">
<method v="2">
<option name="com.jetbrains.cidr.execution.CidrBuildBeforeRunTaskProvider$BuildBeforeRunTask" enabled="true" />
......@@ -1024,6 +1056,7 @@
<item itemvalue="Application.crs_point_processing_node" />
<item itemvalue="Application.crs_point_processing_node_student" />
<item itemvalue="Application.crs_registration_icp" />
<item itemvalue="Application.crs_registration_icp_student" />
<item itemvalue="Application.crs_tf_publisher_node" />
<item itemvalue="Application.crs_tf_publisher_node_student" />
<item itemvalue="Application.download_extra_data" />
......@@ -1129,12 +1162,12 @@
<workItem from="1546847120071" duration="25722000" />
<workItem from="1546957001358" duration="7821000" />
<workItem from="1547026884628" duration="197000" />
<workItem from="1547055031686" duration="347000" />
<workItem from="1547055031686" duration="5276000" />
</task>
<servers />
</component>
<component name="TimeTrackingManager">
<option name="totallyTimeSpent" value="59177000" />
<option name="totallyTimeSpent" value="64106000" />
</component>
<component name="ToolWindowManager">
<frame x="1079" y="369" width="1922" height="1175" extended-state="6" />
......@@ -1238,13 +1271,6 @@
</state>
</provider>
</entry>
<entry file="file://$PROJECT_DIR$/CMakeLists.txt">
<provider selected="true" editor-type-id="text-editor">
<state relative-caret-position="454">
<caret line="267" column="88" lean-forward="true" selection-start-line="267" selection-start-column="88" selection-end-line="267" selection-end-column="88" />
</state>
</provider>
</entry>
<entry file="file://$PROJECT_DIR$/src/comon/result.cpp">
<provider selected="true" editor-type-id="text-editor">
<state relative-caret-position="240">
......@@ -1372,22 +1398,76 @@
</state>
</provider>
</entry>
<entry file="file://$PROJECT_DIR$/src/comon/pclhelper.cpp">
<provider selected="true" editor-type-id="text-editor">
<state relative-caret-position="30">
<caret line="2" selection-start-line="2" selection-end-line="2" />
</state>
</provider>
</entry>
<entry file="file://$PROJECT_DIR$/include/registration_icp/helper.hpp">
<provider selected="true" editor-type-id="text-editor" />
</entry>
<entry file="file://$PROJECT_DIR$/src/registration_icp/helper.cpp">
<provider selected="true" editor-type-id="text-editor">
<state relative-caret-position="-810">
<caret column="26" selection-start-column="26" selection-end-column="26" />
<state relative-caret-position="60">
<caret line="4" lean-forward="true" selection-start-line="4" selection-end-line="172" />
</state>
</provider>
</entry>
<entry file="file://$PROJECT_DIR$/include/comon/pclhelper.hpp">
<provider selected="true" editor-type-id="text-editor">
<state relative-caret-position="150">
<caret line="10" column="46" selection-start-line="10" selection-start-column="46" selection-end-line="10" selection-end-column="46" />
<folding>
<element signature="e#14#46#0" expanded="true" />
</folding>
</state>
</provider>
</entry>
<entry file="file://$PROJECT_DIR$/src/registration_icp_student/main.cpp">
<provider selected="true" editor-type-id="text-editor">
<state relative-caret-position="278">
<caret line="23" column="30" selection-start-line="23" selection-start-column="30" selection-end-line="23" selection-end-column="30" />
<folding>
<element signature="e#0#20#0" expanded="true" />
</folding>
</state>
</provider>
</entry>
<entry file="file://$PROJECT_DIR$/src/registration_icp/main.cpp">
<provider selected="true" editor-type-id="text-editor">
<state relative-caret-position="264">
<caret line="86" column="62" selection-start-line="86" selection-start-column="62" selection-end-line="86" selection-end-column="62" />
<state relative-caret-position="203">
<caret line="28" selection-start-line="28" selection-end-line="28" />
<folding>
<element signature="e#0#20#0" expanded="true" />
</folding>
</state>
</provider>
</entry>
<entry file="file://$PROJECT_DIR$/src/comon/rosbaghelper.cpp">
<provider selected="true" editor-type-id="text-editor">
<state>
<caret column="32" selection-start-column="32" selection-end-column="32" />
</state>
</provider>
</entry>
<entry file="file://$PROJECT_DIR$/include/comon/rosbaghelper.hpp">
<provider selected="true" editor-type-id="text-editor">
<state relative-caret-position="450">
<caret line="30" selection-start-line="30" selection-end-line="30" />
<folding>
<element signature="e#14#31#0" expanded="true" />
</folding>
</state>
</provider>
</entry>
<entry file="file://$PROJECT_DIR$/CMakeLists.txt">
<provider selected="true" editor-type-id="text-editor">
<state relative-caret-position="291">
<caret line="66" selection-start-line="66" selection-end-line="66" />
</state>
</provider>
</entry>
</component>
</project>
\ No newline at end of file
CMakeLists.txt
View file @ 6579ecfc
......@@ -89,7 +89,7 @@ set(comon_SOURCE_DIR ${CMAKE_CURRENT_LIST_DIR}/src/comon/)
file(GLOB comon_SOURCES ${comon_SOURCE_DIR}/*.c*)
add_library(crs_comon ${comon_SOURCES} ${comon_HEADERS} ${threadpool_HEADERS} ${scopeguard_HEADERS} ${better_enum_HEADERS})
target_link_libraries(crs_comon ${TBB_LIBRARIES})
target_link_libraries(crs_comon ${TBB_LIBRARIES} ${PCL_COMMON_LIBRARIES} ${PCL_IO_LIBRARIES} ${catkin_LIBRARIES})
add_dependencies(crs_comon crs_generate_messages_cpp)
......@@ -261,3 +261,14 @@ add_executable(crs_registration_icp ${registration_icp_SOURCES} ${registration_i
set_target_properties(crs_registration_icp PROPERTIES OUTPUT_NAME registration_icp PREFIX "")
target_link_libraries(crs_registration_icp crs_comon ${PCL_COMMON_LIBRARIES} ${PCL_IO_LIBRARIES} ${catkin_LIBRARIES})
####################################### registration_icp_student #######################################
set(registration_icp_student_INCLUDE_DIR ${CMAKE_CURRENT_LIST_DIR}/include/registration_icp_student/)
file(GLOB registration_icp_student_HEADERS ${registration_icp_student_INCLUDE_DIR}/*.h*)
set(registration_icp_student_SOURCE_DIR ${CMAKE_CURRENT_LIST_DIR}/src/registration_icp_student/)
file(GLOB registration_icp_student_SOURCES ${registration_icp_student_SOURCE_DIR}/*.c*)
add_executable(crs_registration_icp_student ${registration_icp_student_SOURCES} ${registration_icp_student_HEADERS})
set_target_properties(crs_registration_icp_student PROPERTIES OUTPUT_NAME registration_icp_student PREFIX "")
target_link_libraries(crs_registration_icp_student crs_comon ${PCL_COMMON_LIBRARIES} ${PCL_IO_LIBRARIES} ${catkin_LIBRARIES})
include/comon/pclhelper.hpp 0 → 100644
View file @ 6579ecfc
#pragma once
#include <pcl_ros/point_cloud.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
using IterativeClosestPoint = pcl::IterativeClosestPoint<pcl::PointXYZRGBA, pcl::PointXYZRGBA>;
using Criteria = pcl::registration::DefaultConvergenceCriteria<float>;
// Convert a convergence criteria to a string.
const char* getCriteria(IterativeClosestPoint& icp) {
auto criteria = icp.getConvergeCriteria()->getConvergenceState();
switch (criteria) {
case Criteria::CONVERGENCE_CRITERIA_TRANSFORM:
return "CONVERGENCE_CRITERIA_TRANSFORM";
case Criteria::CONVERGENCE_CRITERIA_ABS_MSE:
return "CONVERGENCE_CRITERIA_ABS_MSE";
case Criteria::CONVERGENCE_CRITERIA_ITERATIONS:
return "CONVERGENCE_CRITERIA_ITERATIONS";
case Criteria::CONVERGENCE_CRITERIA_NO_CORRESPONDENCES:
return "CONVERGENCE_CRITERIA_NO_CORRESPONDENCES";
case Criteria::CONVERGENCE_CRITERIA_NOT_CONVERGED:
return "CONVERGENCE_CRITERIA_NOT_CONVERGED";
case Criteria::CONVERGENCE_CRITERIA_REL_MSE:
return "CONVERGENCE_CRITERIA_REL_MSE";
default:
return "CONVERGENCE_CRITERIA unknown ...";
}
}
\ No newline at end of file
include/registration_icp/helper.hpp → include/comon/rosbaghelper.hpp
View file @ 6579ecfc
......@@ -8,9 +8,6 @@
using CloudRGBA = pcl::PointCloud<pcl::PointXYZRGBA>;
/**
* - Has to be a
*/
class BagFileHelper {
public:
BagFileHelper(const BagFileHelper&) = delete;
......@@ -18,74 +15,16 @@ public:
~BagFileHelper();
void openBagFile(const std::string& filePath);
// void reset();
// initialize all topics
bool initTopic(std::string topic);
// bool getTopicSize(const std::string topic) const;
bool hasTopicAvailableMessages(const std::string& topic) const;
bool isTopicInitialized(const std::string& topic) const;
CloudRGBA::ConstPtr getNextConstCloudRGBAForTopic(const std::string& topic);
CloudRGBA::Ptr getNextCloudRGBAForTopic(const std::string& topic);
// template<class T>
// std::shared_ptr<T> getNextForTopic(const std::string& topic);
private:
/// A PIMPL class is required due to some PCL and ROS incompatibilities.
class Data;
std::unique_ptr<Data> data_;
};
class ROSParamHelper {
public:
ROSParamHelper();
void setROSNodePtr(ros::NodeHandlePtr ptr);
std::string dumpString();
bool updateAll();
bool updateICPMaxCorrespondenceDistance();
void setICPMaxCorrespondenceDistance(double value);
double getICPMaxCorrespondenceDistance() const;
bool updateICPMaxIterations();
void setICPMaxIterations(int value);
int getICPMaxIterations() const;
bool updateICPTransformationEpsilon();
void setICPTransformationEpsilon(double value);
double getICPTransformationEpsilon() const;
bool updateICPEuclidianFitnessEpsilon();
void setICPEuclidianFitnessEpsilon(double value);
double getICPEuclidianFitnessEpsilon() const;
bool updateDownsampleLeafSize();
void setDownsampleLeafSize(double value);
double getDownsampleLeafSize() const;
private:
std::tuple<std::string, std::string> mainCloudTopic_ = {"/kinect2/sd/points", "MainCloudTopic"};
std::tuple<std::string, std::string> mainCloudBag_ = {"/home/marius/bockdata/bags/crs.bag", "MainBagFile"};
std::tuple<double, std::string> icpMaxCorrespondenceDistance_ = {0.03, "ICPMaxCorrespondenceDistance"};
std::tuple<int, std::string> icpMaxIterations_ = {100, "ICPMaxIterations"};
std::tuple<double, std::string> icpTransformationEpsilon_ = {0.03, "ICPTransformationEpsilon"};
std::tuple<double, std::string> icpEuclidianFitnessEpsilon_ = {0.03, "ICPEuclidianFitnessEpsilon"};
std::tuple<double, std::string> downsampleLeafSize = {0.03, "DownsampleLeafSize"};
ros::NodeHandlePtr node_;
};
src/comon/pclhelper.cpp 0 → 100644
View file @ 6579ecfc
#include <comon/pclhelper.hpp>
src/registration_icp/helper.cpp → src/comon/rosbaghelper.cpp
View file @ 6579ecfc
#include <registration_icp/helper.hpp>
#include <comon/rosbaghelper.hpp>
#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <unordered_map>
......@@ -95,181 +95,9 @@ bool BagFileHelper::hasTopicAvailableMessages(const std::string &topic) const {
}
BagFileHelper::BagFileHelper() {
data_ = std::make_unique<BagFileHelper::Data>();
data_ = std::make_unique<BagFileHelper::Data>();
}
BagFileHelper::~BagFileHelper() {
}
//+++++++++++++++++++++++++++++++
//
//if (privateNode_.hasParam("iteration_mode")) {
//std::string iterationMode;
//privateNode_.getParam("iteration_mode", iterationMode);
//auto modeOptional = IterationMode::_from_string_nocase_nothrow(iterationMode.c_str());
//if (!modeOptional) {
//std::stringstream validValues;
//auto values = IterationMode::_values();
//for (auto value: values) {
//validValues << value._to_string() << " ";
//}
//return stringify("Invalid value for <iteration_mode>. Valid values are: ", validValues.str());
//}
//appCfg.setIterationMode(*modeOptional);
//}
//####################### ICPMaxCorrespondenceDistance #######################
void ROSParamHelper::setICPMaxCorrespondenceDistance(double newValue) {
auto& [value, key] = icpMaxCorrespondenceDistance_;
node_->setParam(key, newValue);
value = newValue;
}
double ROSParamHelper::getICPMaxCorrespondenceDistance() const {
return std::get<0>(icpMaxCorrespondenceDistance_);
}
bool ROSParamHelper::updateICPMaxCorrespondenceDistance() {
const auto& [defaultValue, key] = icpMaxCorrespondenceDistance_;
if (node_->hasParam(key)) {
double value;
if (node_->getParam(key, value)) {
setICPMaxCorrespondenceDistance(value);
return true;
}
return false;
}
return true;
}
//####################### ICPMaxIterations #######################
void ROSParamHelper::setICPMaxIterations(int newValue) {
auto& [value, key] = icpMaxIterations_;
node_->setParam(key, newValue);
value = newValue;
}
int ROSParamHelper::getICPMaxIterations() const {
return std::get<0>(icpMaxIterations_);
}
bool ROSParamHelper::updateICPMaxIterations() {
const auto& [defaultValue, key] = icpMaxIterations_;
if (node_->hasParam(key)) {
int value;
if (node_->getParam(key, value)) {
setICPMaxIterations(value);
return true;
}
return false;
}
return true;
}
//####################### ICPTransformationEpsilon #######################
double ROSParamHelper::getICPTransformationEpsilon() const {
return std::get<0>(icpTransformationEpsilon_);
}
void ROSParamHelper::setICPTransformationEpsilon(double newValue) {
auto& [value, key] = icpTransformationEpsilon_;
node_->setParam(key, newValue);
value = newValue;
}
bool ROSParamHelper::updateICPTransformationEpsilon() {
const auto& [defaultValue, key] = icpTransformationEpsilon_;
if (node_->hasParam(key)) {
double value;
if (node_->getParam(key, value)) {
setICPTransformationEpsilon(value);
return true;
}
return false;
}
return true;
}
//####################### ICPEuclidianFitnessEpsilon #######################
void ROSParamHelper::setICPEuclidianFitnessEpsilon(double newValue) {
auto& [value, key] = icpEuclidianFitnessEpsilon_;
node_->setParam(key, newValue);
value = newValue;
}
double ROSParamHelper::getICPEuclidianFitnessEpsilon() const {
return std::get<0>(icpEuclidianFitnessEpsilon_);
}
bool ROSParamHelper::updateICPEuclidianFitnessEpsilon() {
const auto& [defaultValue, key] = icpEuclidianFitnessEpsilon_;
if (node_->hasParam(key)) {
double value;
if (node_->getParam(key, value)) {
setICPEuclidianFitnessEpsilon(value);
return true;
}
return false;
}
return true;
}
void ROSParamHelper::setROSNodePtr(ros::NodeHandlePtr ptr) {
node_ = ptr;
}
bool ROSParamHelper::updateAll() {
return updateICPEuclidianFitnessEpsilon() && updateICPMaxCorrespondenceDistance() && updateICPMaxIterations() && updateICPTransformationEpsilon() && updateDownsampleLeafSize();
}
//####################### DownsampleLeafSize #######################
bool ROSParamHelper::updateDownsampleLeafSize() {
const auto& [defaultValue, key] =