diff --git a/hello_wolf/processor_range_bearing.cpp b/hello_wolf/processor_range_bearing.cpp
index ca3f56c3ccef6052be4dc89301334c4eed7104ef..0ce5d610105a6984ed4a0beb4474f06a35d2ba67 100644
--- a/hello_wolf/processor_range_bearing.cpp
+++ b/hello_wolf/processor_range_bearing.cpp
@@ -15,7 +15,7 @@ namespace wolf
{
ProcessorRangeBearing::ProcessorRangeBearing(ProcessorParamsBasePtr _params) :
- ProcessorBase("RANGE BEARING", _params)
+ ProcessorBase("ProcessorRangeBearing", _params)
{
//
}
@@ -163,7 +163,7 @@ bool ProcessorRangeBearing::storeCapture(CaptureBasePtr _cap_ptr)
#include "core/processor/processor_factory.h"
namespace wolf
{
-WOLF_REGISTER_PROCESSOR("RANGE BEARING", ProcessorRangeBearing)
-WOLF_REGISTER_PROCESSOR_AUTO("RANGE BEARING", ProcessorRangeBearing)
+WOLF_REGISTER_PROCESSOR("ProcessorRangeBearing", ProcessorRangeBearing)
+WOLF_REGISTER_PROCESSOR_AUTO("ProcessorRangeBearing", ProcessorRangeBearing)
} // namespace wolf
diff --git a/hello_wolf/sensor_range_bearing.cpp b/hello_wolf/sensor_range_bearing.cpp
index 3294c7f7fcba9afc9ac499815ee5a28820b98335..a6b2242118fa009dac985b07e68c595aafe9f2a9 100644
--- a/hello_wolf/sensor_range_bearing.cpp
+++ b/hello_wolf/sensor_range_bearing.cpp
@@ -13,8 +13,9 @@ namespace wolf
WOLF_PTR_TYPEDEFS(SensorRangeBearing);
+//<<<<<<< HEAD
SensorRangeBearing::SensorRangeBearing(const Eigen::VectorXd& _extrinsics, const Eigen::Vector2d& _noise_std) :
- SensorBase("RANGE BEARING",
+ SensorBase("SensorRangeBearing",
std::make_shared<StateBlock>(_extrinsics.head<2>(), true),
std::make_shared<StateAngle>(_extrinsics(2), true),
nullptr,
@@ -41,6 +42,6 @@ SensorRangeBearing::~SensorRangeBearing()
#include "core/sensor/sensor_factory.h"
namespace wolf
{
-WOLF_REGISTER_SENSOR("RANGE BEARING", SensorRangeBearing)
-WOLF_REGISTER_SENSOR_AUTO("RANGE BEARING", SensorRangeBearing)
+WOLF_REGISTER_SENSOR("SensorRangeBearing", SensorRangeBearing)
+WOLF_REGISTER_SENSOR_AUTO("SensorRangeBearing", SensorRangeBearing)
} // namespace wolf
diff --git a/hello_wolf/yaml/hello_wolf_config.yaml b/hello_wolf/yaml/hello_wolf_config.yaml
index 5ac02b2f2bd369b3ecce0533c50c7f4efa520cb5..167dea95d3c6d49f7eca2f8e1a682c7473d415c2 100644
--- a/hello_wolf/yaml/hello_wolf_config.yaml
+++ b/hello_wolf/yaml/hello_wolf_config.yaml
@@ -12,14 +12,14 @@ config:
sensors:
- - type: "ODOM 2D"
+ - type: "SensorOdom2D"
plugin: "core"
name: "sen odom"
extrinsic:
pose: [0,0, 0]
follow: "hello_wolf/yaml/sensor_odom_2D.yaml" # config parameters in this file
- - type: "RANGE BEARING"
+ - type: "SensorRangeBearing"
plugin: "core"
name: "sen rb"
extrinsic:
@@ -29,20 +29,19 @@ config:
processors:
- - type: "ODOM 2D"
+ - type: "ProcessorOdom2D"
plugin: "core"
name: "prc odom"
sensor_name: "sen odom" # attach processor to this sensor
follow: hello_wolf/yaml/processor_odom_2D.yaml # config parameters in this file
- - type: "RANGE BEARING"
+ - type: "ProcessorRangeBearing"
plugin: "core"
name: "prc rb"
sensor_name: "sen rb" # attach processor to this sensor
follow: hello_wolf/yaml/processor_range_bearing.yaml # config parameters in this file
-files:
-# - "/Users/jsola/dev/wolf lib/core/lib/libhellowolf.dylib"
+
\ No newline at end of file
diff --git a/include/core/utils/params_server.hpp b/include/core/utils/params_server.hpp
index 5dad0e8262b603c3de5eaeacecf8726f536e8eb0..a750eab79f4ee90b39e4e21d1926129897c29d92 100644
--- a/include/core/utils/params_server.hpp
+++ b/include/core/utils/params_server.hpp
@@ -40,14 +40,19 @@ public:
_params.insert(std::pair<std::string, std::string>(key, value));
}
-// template<typename T>
-// T getParam(std::string key, std::string def_value) const {
-// if(_params.find(key) != _params.end()){
-// return converter<T>::convert(_params.find(key)->second);
-// }else{
-// return converter<T>::convert(def_value);
-// }
-// }
+ void addParams(std::map<std::string, std::string> params)
+ {
+ _params.insert(params.begin(), params.end());
+ }
+
+ // template<typename T>
+ // T getParam(std::string key, std::string def_value) const {
+ // if(_params.find(key) != _params.end()){
+ // return converter<T>::convert(_params.find(key)->second);
+ // }else{
+ // return converter<T>::convert(def_value);
+ // }
+ // }
template<typename T>
T getParam(std::string key) const {
diff --git a/include/core/yaml/parser_yaml.hpp b/include/core/yaml/parser_yaml.hpp
index 99c1c8c10180318e3a84f0e307ef80be97d85c76..b4dd578252bbf357809a69dae625538add0b0e37 100644
--- a/include/core/yaml/parser_yaml.hpp
+++ b/include/core/yaml/parser_yaml.hpp
@@ -355,6 +355,8 @@ void ParserYAML::walkTreeR(YAML::Node n, std::vector<std::string>& tags, std::st
}
break;
}
+ case YAML::NodeType::Null :
+ break;
default:
assert(1 == 0 && "Unsupported node Type at walkTreeR.");
break;
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index f9bf2ac08410712cfef51e7016cf5303289b1982..b7c37175ad4d7b130240b77b29aab1295cdcab42 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -84,7 +84,8 @@ ProblemPtr Problem::autoSetup(ParamsServer &_server)
std::string frame_structure = _server.getParam<std::string> ("problem/frame_structure");
int dim = _server.getParam<int> ("problem/dimension");
auto problem = Problem::create(frame_structure, dim);
- // cout << "PRINTING SERVER MAP" << endl;
+ //
+ // cout << "PRINTING SERVER MAP" << endl;
// _server.print();
// cout << "-----------------------------------" << endl;
WOLF_TRACE("Setting up problem with frame structure {" + frame_structure + "} and dimension " + std::to_string(dim) + "D");
diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp
index e1b2744d1f83f4373a777a6692f0d8ab7d8ecac0..227f819941fb886e657cb03c0f0cf1422218738b 100644
--- a/src/processor/processor_tracker.cpp
+++ b/src/processor/processor_tracker.cpp
@@ -87,8 +87,9 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
// Issue KF callback with new KF
getProblem()->keyFrameCallback(kfrm, shared_from_this(), params_tracker_->time_tolerance);
- // Update pointers
resetDerived();
+
+ // Update pointers
origin_ptr_ = incoming_ptr_;
last_ptr_ = incoming_ptr_;
incoming_ptr_ = nullptr;
@@ -175,9 +176,9 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
last_ptr_->getFrame()->setState(getProblem()->getState(last_ptr_->getTimeStamp()));
last_ptr_->getFrame()->setKey();
- // make F; append incoming to new F
- FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED, incoming_ptr_->getTimeStamp());
- incoming_ptr_->link(frm);
+ // // make F; append incoming to new F
+ // FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED, incoming_ptr_->getTimeStamp());
+ // incoming_ptr_->link(frm);
// Establish factors
establishFactors();
@@ -187,6 +188,11 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
// Update pointers
resetDerived();
+
+ // make F; append incoming to new F
+ FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED, last_ptr_->getFrame()->getState(), incoming_ptr_->getTimeStamp());
+ incoming_ptr_->link(frm);
+
origin_ptr_ = last_ptr_;
last_ptr_ = incoming_ptr_;
incoming_ptr_ = nullptr;
diff --git a/wolf_scripts/rename.sh b/wolf_scripts/rename.sh
index f628c06bd9f773430db68330ff2f89b78d04e0cc..27c83e736e3b2b29a0033932f5c134f1bceb8ad0 100755
--- a/wolf_scripts/rename.sh
+++ b/wolf_scripts/rename.sh
@@ -50,8 +50,13 @@ types=$(getTypes | sort | uniq)
# done
for target in $(find include/ src/ test/ -type f); do
for ctype in $types; do
+ filename=$(echo $target | rev | cut -d '/' -f1 | cut -d '.' -f2 | rev)
+ snake2camel $filename
oldtype=$ctype
ctype=$(echo $ctype | sed -r "s/([^\ 0-9])([^\ 0-9]+)\ ?/\1\L\2/g")
- sed -ri "s/(Capture|Feature|Processor|Landmark|Factor|Sensor)(.*)\"${oldtype}\"/\1\2\"\1${ctype}\"/" $target
+ # echo $target $oldtype $ctype
+ # sed -ri "s/(Capture|Feature|Processor|Landmark|Factor|Sensor)(.*)\"${oldtype}\"/\1\2\"\1${ctype}\"/" $target
+ sed -ri "s/(Capture|Feature|Processor|Landmark|Factor|Sensor)(.*)\"${oldtype}\"/\1\2\"\1${ctype}\"/" $target
+ sed -ri "s%\"${oldtype}\"%\"${new_camel}\"%" $target
done
-done
\ No newline at end of file
+done