From 539e23ed4ae7e06d228115b4d67c13cd911d5341 Mon Sep 17 00:00:00 2001
From: Rohan Budhiraja <budhiraja@laas.fr>
Date: Fri, 2 Jun 2017 15:59:52 +0200
Subject: [PATCH] [Bug] nbOfJoints_ is not initialized if
rosparam(sot/state_vector_map) is undefined. Leads to resize with garbage
value
TODO: where to set the size of angleEncoder_?
---
src/sot_loader.cpp | 9 +++++++--
1 file changed, 7 insertions(+), 2 deletions(-)
diff --git a/src/sot_loader.cpp b/src/sot_loader.cpp
index 60f151c..411ed6c 100644
--- a/src/sot_loader.cpp
+++ b/src/sot_loader.cpp
@@ -76,15 +76,20 @@ void SotLoader::startControlLoop()
void SotLoader::initializeRosNode(int argc, char *argv[])
{
SotLoaderBasic::initializeRosNode(argc, argv);
- angleEncoder_.resize(nbOfJoints_);
+ //Temporary fix. TODO: where should nbOfJoints_ be initialized from?
+ if (ros::param::has("/sot/state_vector_map")) {
+ angleEncoder_.resize(nbOfJoints_);
+ }
+
startControlLoop();
}
void
SotLoader::fillSensors(map<string,dgs::SensorValues> & sensorsIn)
{
-
// Update joint values.w
+ assert(angleControl_.size() == angleEncoder_.size());
+
sensorsIn["joints"].setName("angle");
for(unsigned int i=0;i<angleControl_.size();i++)
angleEncoder_[i] = angleControl_[i];
--
GitLab