[wip/infuse-crisp-genom] Fixed transFormWithCovariance export

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

infuse-crisp-genom3/distinfo

 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

infuse-crisp-genom3/patches/patch-aa

+--- 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;
+ }