From 2c93f880e208e657447e51bbdaf65ea4bd405eec Mon Sep 17 00:00:00 2001
From: Nikhil Ambardar <202-nikaviator@users.noreply.git-st.inf.tu-dresden.de>
Date: Fri, 2 Oct 2020 10:55:50 +0000
Subject: [PATCH] Update robot_models_node.cpp

---
 src/robot_models_node.cpp | 177 ++++++++++++++++++++------------------
 1 file changed, 92 insertions(+), 85 deletions(-)

diff --git a/src/robot_models_node.cpp b/src/robot_models_node.cpp
index b37fe89..ee85be1 100644
--- a/src/robot_models_node.cpp
+++ b/src/robot_models_node.cpp
@@ -1,15 +1,15 @@
 #include <ros/ros.h>
-#include </home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/abstractobject.h>
-#include </home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/graspobject.h>
-#include </home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/hand.h>
-#include </home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/humanspace.h>
-#include </home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/joint.h>
-#include </home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/obstacle.h>
-#include </home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/robot.h>
-#include </home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/robotfpe.h>
-#include </home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/world.h>
-#include </home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/worldobject.h>
-#include </home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/quaternion.h>
+#include "datalink/abstractobject.h"
+#include "datalink/graspobject.h"
+#include "datalink/hand.h"
+#include "datalink/humanspace.h"
+#include "datalink/joint.h"
+#include "datalink/obstacle.h"
+#include "datalink/robot.h"
+#include "datalink/robotfpe.h"
+#include "datalink/world.h"
+#include "datalink/worldobject.h"
+#include "datalink/quaternion.h"
 #include <vector>
 #include <iostream>
 #include <tf2_ros/transform_listener.h>
@@ -30,9 +30,9 @@
         ros::NodeHandle node_handle("namespace_name");
         ros::AsyncSpinner spinner(1);
         spinner.start();
+        ROS_INFO("HELLO, WORLD");
 
 
-        ROS_INFO("HELLO, WORLD");
 
 
         abstractobject o1;
@@ -140,15 +140,22 @@
         while (node_handle.ok()) {
             //for (const auto &topic : ROSNODE1::topics)
             {
-                geometry_msgs::TransformStamped transformStamped;
+                geometry_msgs::TransformStamped transformStamped1;
+                geometry_msgs::TransformStamped transformStamped2;
+                geometry_msgs::TransformStamped transformStamped3;
+                geometry_msgs::TransformStamped transformStamped4;
+                geometry_msgs::TransformStamped transformStamped5;
+                geometry_msgs::TransformStamped transformStamped6;
+                geometry_msgs::TransformStamped transformStamped7;
+
                 try {
-                    transformStamped = tfBuffer.lookupTransform("world", o5a.name, ros::Time(0));
-                    transformStamped = tfBuffer.lookupTransform("world", o5b.name, ros::Time(0));
-                    transformStamped = tfBuffer.lookupTransform("world", o5c.name, ros::Time(0));
-                    transformStamped = tfBuffer.lookupTransform("world", o5d.name, ros::Time(0));
-                    transformStamped = tfBuffer.lookupTransform("world", o5e.name, ros::Time(0));
-                    transformStamped = tfBuffer.lookupTransform("world", o5f.name, ros::Time(0));
-                    transformStamped = tfBuffer.lookupTransform("world", o5g.name, ros::Time(0));
+                    transformStamped1 = tfBuffer.lookupTransform("world", o5a.name, ros::Time(0));
+                    transformStamped2 = tfBuffer.lookupTransform("world", o5b.name, ros::Time(0));
+                    transformStamped3 = tfBuffer.lookupTransform("world", o5c.name, ros::Time(0));
+                    transformStamped4 = tfBuffer.lookupTransform("world", o5d.name, ros::Time(0));
+                    transformStamped5 = tfBuffer.lookupTransform("world", o5e.name, ros::Time(0));
+                    transformStamped6 = tfBuffer.lookupTransform("world", o5f.name, ros::Time(0));
+                    transformStamped7 = tfBuffer.lookupTransform("world", o5g.name, ros::Time(0));
                 } catch (tf2::TransformException &ex) {
                     ROS_WARN("%s", ex.what());
                     ros::Duration(0.1).sleep();
@@ -156,70 +163,70 @@
 
                 }
 
-
-                o5a.p1.push_back(transformStamped.transform.translation.x);
-                o5a.p1.push_back(transformStamped.transform.translation.y);
-                o5a.p1.push_back(transformStamped.transform.translation.z);
-                o5a.p1[0] = 0.0;
-                o5b.p1.push_back(transformStamped.transform.translation.x);
-                o5b.p1.push_back(transformStamped.transform.translation.y);
-                o5b.p1.push_back(transformStamped.transform.translation.z);
-                o5b.p1[0] = 0.0;
-                o5c.p1.push_back(transformStamped.transform.translation.x);
-                o5c.p1.push_back(transformStamped.transform.translation.y);
-                o5c.p1.push_back(transformStamped.transform.translation.z);
-                o5c.p1[0] = 0.0;
-                o5d.p1.push_back(transformStamped.transform.translation.x);
-                o5d.p1.push_back(transformStamped.transform.translation.y);
-                o5d.p1.push_back(transformStamped.transform.translation.z);
-                o5d.p1[0] = 0.0;
-                o5e.p1.push_back(transformStamped.transform.translation.x);
-                o5e.p1.push_back(transformStamped.transform.translation.y);
-                o5e.p1.push_back(transformStamped.transform.translation.z);
-                o5e.p1[0] = 0.0;
-                o5f.p1.push_back(transformStamped.transform.translation.x);
-                o5f.p1.push_back(transformStamped.transform.translation.y);
-                o5f.p1.push_back(transformStamped.transform.translation.z);
-                o5f.p1[0] = 0.0;
-                o5g.p1.push_back(transformStamped.transform.translation.x);
-                o5g.p1.push_back(transformStamped.transform.translation.y);
-                o5g.p1.push_back(transformStamped.transform.translation.z);
-                o5g.p1[0] = 0.0;
-
-               o5a.or2.w=transformStamped.transform.rotation.w;
-               o5a.or2.x=transformStamped.transform.rotation.x;
-               o5a.or2.y=transformStamped.transform.rotation.y;
-               o5a.or2.z=transformStamped.transform.rotation.z;
-
-                o5b.or2.w=transformStamped.transform.rotation.w;
-                o5b.or2.x=transformStamped.transform.rotation.x;
-                o5b.or2.y=transformStamped.transform.rotation.y;
-                o5b.or2.z=transformStamped.transform.rotation.z;
-
-                o5c.or2.w=transformStamped.transform.rotation.w;
-                o5c.or2.x=transformStamped.transform.rotation.x;
-                o5c.or2.y=transformStamped.transform.rotation.y;
-                o5c.or2.z=transformStamped.transform.rotation.z;
-
-                o5d.or2.w=transformStamped.transform.rotation.w;
-                o5d.or2.x=transformStamped.transform.rotation.x;
-                o5d.or2.y=transformStamped.transform.rotation.y;
-                o5d.or2.z=transformStamped.transform.rotation.z;
-
-                o5e.or2.w=transformStamped.transform.rotation.w;
-                o5e.or2.x=transformStamped.transform.rotation.x;
-                o5e.or2.y=transformStamped.transform.rotation.y;
-                o5e.or2.z=transformStamped.transform.rotation.z;
-
-                o5f.or2.w=transformStamped.transform.rotation.w;
-                o5f.or2.x=transformStamped.transform.rotation.x;
-                o5f.or2.y=transformStamped.transform.rotation.y;
-                o5f.or2.z=transformStamped.transform.rotation.z;
-
-                o5g.or2.w=transformStamped.transform.rotation.w;
-                o5g.or2.x=transformStamped.transform.rotation.x;
-                o5g.or2.y=transformStamped.transform.rotation.y;
-                o5g.or2.z=transformStamped.transform.rotation.z;
+                //o5a.p1[0] = 0.0;
+                o5a.p1[0]=transformStamped1.transform.translation.x;
+                o5a.p1[1]=transformStamped1.transform.translation.y;
+                o5a.p1[2]=transformStamped1.transform.translation.z;
+                //ROS_INFO("%f",&o5a.p1);
+                o5b.p1[0]=transformStamped2.transform.translation.x;
+                o5b.p1[1]=transformStamped2.transform.translation.y;
+                o5b.p1[2]=transformStamped2.transform.translation.z;
+             
+                o5c.p1[0]=transformStamped3.transform.translation.x;
+                o5c.p1[1]=transformStamped3.transform.translation.y;
+                o5c.p1[2]=transformStamped3.transform.translation.z;
+
+                o5d.p1[0]=transformStamped4.transform.translation.x;
+                o5d.p1[1]=transformStamped4.transform.translation.y;
+                o5d.p1[2]=transformStamped4.transform.translation.z;
+
+                o5e.p1[0]=transformStamped5.transform.translation.x;
+                o5e.p1[1]=transformStamped5.transform.translation.y;
+                o5e.p1[2]=transformStamped5.transform.translation.z;
+
+                o5f.p1[0]=transformStamped6.transform.translation.x;
+                o5f.p1[1]=transformStamped6.transform.translation.y;
+                o5f.p1[2]=transformStamped6.transform.translation.z;
+
+                o5g.p1[0]=transformStamped7.transform.translation.x;
+                o5g.p1[1]=transformStamped7.transform.translation.y;
+                o5g.p1[2]=transformStamped7.transform.translation.z;
+
+
+                o5a.or2.w=transformStamped1.transform.rotation.w;
+                o5a.or2.x=transformStamped1.transform.rotation.x;
+                o5a.or2.y=transformStamped1.transform.rotation.y;
+                o5a.or2.z=transformStamped1.transform.rotation.z;
+
+                o5b.or2.w=transformStamped2.transform.rotation.w;
+                o5b.or2.x=transformStamped2.transform.rotation.x;
+                o5b.or2.y=transformStamped2.transform.rotation.y;
+                o5b.or2.z=transformStamped2.transform.rotation.z;
+
+                o5c.or2.w=transformStamped3.transform.rotation.w;
+                o5c.or2.x=transformStamped3.transform.rotation.x;
+                o5c.or2.y=transformStamped3.transform.rotation.y;
+                o5c.or2.z=transformStamped3.transform.rotation.z;
+
+                o5d.or2.w=transformStamped4.transform.rotation.w;
+                o5d.or2.x=transformStamped4.transform.rotation.x;
+                o5d.or2.y=transformStamped4.transform.rotation.y;
+                o5d.or2.z=transformStamped4.transform.rotation.z;
+
+                o5e.or2.w=transformStamped5.transform.rotation.w;
+                o5e.or2.x=transformStamped5.transform.rotation.x;
+                o5e.or2.y=transformStamped5.transform.rotation.y;
+                o5e.or2.z=transformStamped5.transform.rotation.z;
+
+                o5f.or2.w=transformStamped6.transform.rotation.w;
+                o5f.or2.x=transformStamped6.transform.rotation.x;
+                o5f.or2.y=transformStamped6.transform.rotation.y;
+                o5f.or2.z=transformStamped6.transform.rotation.z;
+
+                o5g.or2.w=transformStamped7.transform.rotation.w;
+                o5g.or2.x=transformStamped7.transform.rotation.x;
+                o5g.or2.y=transformStamped7.transform.rotation.y;
+                o5g.or2.z=transformStamped7.transform.rotation.z;
 
 
               }
@@ -228,4 +235,4 @@
 
 
 return 0;
-}
\ No newline at end of file
+}
-- 
GitLab