Update robot_models_node.cpp

src/robot_models_node.cpp

@@ -157,20 +157,75 @@
                 }
 
 
-               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.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;
+
+
               }
             rate.sleep();
         }
 
 
 return 0;
-}
+}
\ No newline at end of file