From 5ba3880d5ab622ba44b81335a00ee6050db97575 Mon Sep 17 00:00:00 2001
From: Pierre Narvor <pnarvor@laas.fr>
Date: Thu, 23 Aug 2018 11:05:48 +0200
Subject: [PATCH] [wip/infuse-crisp-genom] Fixed transFormWithCovariance export
---
infuse-crisp-genom3/Makefile | 36 ++++++++++
infuse-crisp-genom3/distinfo | 1 +
infuse-crisp-genom3/patches/patch-aa | 104 +++++++++++++++++++++++++++
3 files changed, 141 insertions(+)
create mode 100644 infuse-crisp-genom3/patches/patch-aa
diff --git a/infuse-crisp-genom3/Makefile b/infuse-crisp-genom3/Makefile
index 3025c283..beab3efb 100644
--- a/infuse-crisp-genom3/Makefile
+++ b/infuse-crisp-genom3/Makefile
@@ -2,6 +2,7 @@
# Created: Quentin Labourey on Tue, 21 Aug 2018
#
+PKGREVISION= 1
VERSION= 0.1
DISTNAME= infuse_crisp-genom3-${VERSION}
CATEGORIES= wip
@@ -20,6 +21,41 @@ include ../../architecture/genom3/module.mk
# In InFuse we use ROS, so keep these options enabled by default
PKG_SUGGESTED_OPTIONS+= ros-client-c ros-client-ros ros-server
+# # This WAS SUPPOSED TO fix the double dependency on std_msgs in the
+# # generated package.xml files. This does not work here probably because
+# # PKG_OPTIONS here is empty. This should probably go in a 'define' to be
+# # expanded later, but I don't know how to do it. :(
+# ifneq (,$(filter ros-client-c,${PKG_OPTIONS}))
+# SUBST_CLASSES+= fix-ros-client-c-depends
+# SUBST_STAGE.fix-ros-client-c-depends= pre-install
+# SUBST_MESSAGE.fix-ros-client-c-depends= Fixing repeated dependency on std_msgs for option ros-client-c
+# SUBST_FILES.fix-ros-client-c-depends= ../templates/ros/client/c/*/package.xml
+# SUBST_SED.fix-ros-client-c-depends= -e '0,/<depend>std_msgs<\/depend>/! s/<depend>std_msgs<\/depend>//'
+# endif
+# ifneq (,$(filter ros-client-ros,${PKG_OPTIONS}))
+# SUBST_CLASSES+= fix-ros-client-ros-depends
+# SUBST_STAGE.fix-ros-client-ros-depends= pre-install
+# SUBST_MESSAGE.fix-ros-client-ros-depends= Fixing repeated dependency on std_msgs for option ros-client-ros
+# SUBST_FILES.fix-ros-client-ros-depends= ../templates/ros/client/ros/*/package.xml
+# SUBST_SED.fix-ros-client-ros-depends= -e '0,/<depend>std_msgs<\/depend>/! s/<depend>std_msgs<\/depend>//'
+# endif
+# ifneq (,$(filter ros-server,${PKG_OPTIONS}))
+# SUBST_CLASSES+= fix-ros-server-depends
+# SUBST_STAGE.fix-ros-server-depends= pre-install
+# SUBST_MESSAGE.fix-ros-server-depends= Fixing repeated dependency on std_msgs for option ros-server
+# SUBST_FILES.fix-ros-server-depends= ../templates/ros/server/*/package.xml
+# SUBST_SED.fix-ros-server-depends= -e '0,/<depend>std_msgs<\/depend>/! s/<depend>std_msgs<\/depend>//'
+# endif
+
+# Dirty hack to fix the repeated dependency because the solution commented
+# above didn't work. The installation of this package probably going to fail
+# if not compiled with ROS options
+SUBST_CLASSES+= fix-std-msgs-depends
+SUBST_STAGE.fix-std-msgs-depends= pre-install
+SUBST_MESSAGE.fix-std-msgs-depends= Fixing repeated dependency on std_msgs
+SUBST_FILES.fix-std-msgs-depends= ../templates/ros/server/*/package.xml
+SUBST_FILES.fix-std-msgs-depends+= ../templates/ros/client/*/*/package.xml
+SUBST_SED.fix-std-msgs-depends= -e '0,/<depend>std_msgs<\/depend>/! s/<depend>std_msgs<\/depend>//'
include ../../interfaces/openrobots-idl/depend.mk
include ../../wip/infuse-idl/depend.mk
diff --git a/infuse-crisp-genom3/distinfo b/infuse-crisp-genom3/distinfo
index 318b1b49..df48bee5 100644
--- a/infuse-crisp-genom3/distinfo
+++ b/infuse-crisp-genom3/distinfo
@@ -1,3 +1,4 @@
SHA1 (infuse_crisp-genom3-0.1.tar.gz) = 0ffcce3a26fb5d8bcac68ccde3b3e23477d500b2
RMD160 (infuse_crisp-genom3-0.1.tar.gz) = 77a45ea380f7d882c41c3538f445a2cf726c29ed
Size (infuse_crisp-genom3-0.1.tar.gz) = 375980 bytes
+SHA1 (patch-aa) = 7341ca61b129f7a60132f616f4f86525c1c46dcc
diff --git a/infuse-crisp-genom3/patches/patch-aa b/infuse-crisp-genom3/patches/patch-aa
new file mode 100644
index 00000000..f6f75ebe
--- /dev/null
+++ b/infuse-crisp-genom3/patches/patch-aa
@@ -0,0 +1,104 @@
+--- codels/infuse_crisp_update_internal_tree_codels.cc 2018-08-22 09:39:25.000000000 +0200
++++ codels/infuse_crisp_update_internal_tree_codels.cc 2018-08-23 11:02:48.878530834 +0200
+@@ -56,6 +56,7 @@
+ const genom_context self)
+ {
+ asn1_bitstream* gbstream;
++ vector<PositionManager::FrameId> leavesNames = ids->crisp->getLeavesNames();
+ for(int i = 0; i < ids->outputPortInfos._length; i++)
+ {
+ if(sensorPoseOutput->open(ids->outputPortInfos._buffer[i].name, self) != genom_ok)
+@@ -71,10 +72,23 @@
+ << string(ids->outputPortInfos._buffer[i].name) << endl;
+ continue;
+ }
+- gbstream->data._maximum = asn1SccTransformWithCovariance_REQUIRED_BYTES_FOR_ENCODING;
+- gbstream->data._length = asn1SccTransformWithCovariance_REQUIRED_BYTES_FOR_ENCODING;
+- gbstream->data._release = NULL;
+- gbstream->data._buffer = new uint8_t[asn1SccTransformWithCovariance_REQUIRED_BYTES_FOR_ENCODING];
++
++ struct timeval tv;
++ gettimeofday(&tv,NULL);
++ long long timeNow = tv.tv_sec*1000000 + tv.tv_usec;
++ gbstream->header.seq = 0;
++ gbstream->header.stamp.sec = timeNow / 1000000;
++ gbstream->header.stamp.nsec = (timeNow % 1000000) * 1000;
++ gbstream->header.frame_id = (char*)malloc(sizeof(char)*(1 + strlen(leavesNames[i].c_str())));
++ sprintf(gbstream->header.frame_id, leavesNames[i].c_str());
++
++ //// Init bistream type
++ gbstream->type = (char*)malloc(sizeof(char)*(1 + strlen("TransformWithCovariance")));
++ sprintf(gbstream->type, "TransformWithCovariance");
++ gbstream->serialization_method = 0; //uPER
++ //reserve memory for serialized data
++ genom_sequence_reserve(&(gbstream->data), asn1SccTransformWithCovariance_REQUIRED_BYTES_FOR_ENCODING);
++ gbstream->data._length = 0;
+
+ ids->crisp->toggleLeafState(string(ids->outputPortInfos._buffer[i].name));
+
+@@ -113,9 +127,6 @@
+ }
+
+ //This section is to not read several time the same pose
+- //if(gbstream->currentByte == -1)
+- // continue;
+- //gbstream->currentByte = -1;
+ if(gbstream->header.stamp.nsec == UINT32_MAX)
+ continue;
+ gbstream->header.stamp.nsec == UINT32_MAX;
+@@ -135,6 +146,9 @@
+ // cout << " \"" << leavesNames[i] << "\"";
+ //cout << endl;
+
++ struct timeval tv;
++ gettimeofday(&tv,NULL);
++ long long timeNow = tv.tv_sec*1000000 + tv.tv_usec;
+ for(int i = 0; i < leavesNames.size(); i++)
+ {
+ gbstream = sensorPoseOutput->data(leavesNames[i].c_str(), self);
+@@ -144,23 +158,34 @@
+ << leavesNames[i].c_str() << "\""<< endl;
+ continue;
+ }
++ gbstream->header.seq = gbstream->header.seq + 1;
++ gbstream->header.stamp.sec = timeNow / 1000000;
++ gbstream->header.stamp.nsec = (timeNow % 1000000) * 1000;
++
+ bstream.buf = gbstream->data._buffer;
+- ids->crisp->getLeafPose(leavesNames[i], bstream);
++ if(!ids->crisp->getLeafPose(leavesNames[i], bstream))
++ {
++ cout << "Encoding error, could not export sensor pose : " << leavesNames[i] << endl;
++ }
++ gbstream->data._length = asn1SccTransformWithCovariance_REQUIRED_BYTES_FOR_ENCODING;
++ gbstream->data._length = bstream.count;
++
+ if(sensorPoseOutput->write(leavesNames[i].c_str(), self) != genom_ok)
+ {
+ cout << "Could not export sensor pose : " << leavesNames[i] << endl;
+ }
+ }
+- /*map<PositionManager::FrameId,PositionManager::Pose> poses;
+- ids->crisp->getActiveLeavesPoses(poses);
+- for(map<PositionManager::FrameId,PositionManager::Pose>::iterator it = poses.begin();
+- it != poses.end(); ++it)
+- {
+- cout << it->second.toString() << "\n";
+- }
+- if(poses.size() != 0)
+- cout << endl;
+- */
++
++ //map<PositionManager::FrameId,PositionManager::Pose> poses;
++ //ids->crisp->getActiveLeavesPoses(poses);
++ //for(map<PositionManager::FrameId,PositionManager::Pose>::iterator it = poses.begin();
++ // it != poses.end(); ++it)
++ //{
++ // cout << it->second.toString() << "\n";
++ //}
++ //if(poses.size() != 0)
++ // cout << endl;
++
+
+ return infuse_crisp_pause_readjoints;
+ }
--
GitLab