diff --git a/src/datalink/abstractobject.h b/src/datalink/abstractobject.h
index 327492b91853e846be48570e0463f081c831402b..06127326636f94abbb6c70d586a9ad46a9d9249b 100644
--- a/src/datalink/abstractobject.h
+++ b/src/datalink/abstractobject.h
@@ -1,9 +1,12 @@
#include <vector>
#include </home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/worldobject.h>
+#ifndef ROBOT_MODELS_AO_H
+#define ROBOT_MODELS_AO_H
+
class abstractobject:public worldobject
{
public:
- std::string name;
+
vector<int> size;
//no need of constructor here
@@ -11,4 +14,5 @@ class abstractobject:public worldobject
//{
//}
-};
\ No newline at end of file
+};
+#endif //ROBOT_MODELS_TEST_H
\ No newline at end of file
diff --git a/src/datalink/graspobject.h b/src/datalink/graspobject.h
index 4f0e0959027851d63b9c5b6a46be5c87eafadc1d..e15791a8555c081ae53bc3a7978e83af8d73b453 100644
--- a/src/datalink/graspobject.h
+++ b/src/datalink/graspobject.h
@@ -1,5 +1,6 @@
#include </home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/abstractobject.h>
-
+#ifndef ROBOT_MODELS_GO_H
+#define ROBOT_MODELS_GO_H
class graspobject : public abstractobject
{
@@ -7,3 +8,4 @@ class graspobject : public abstractobject
};
+#endif //ROBOT_MODELS_TEST_H
\ No newline at end of file
diff --git a/src/datalink/hand.h b/src/datalink/hand.h
index 1465f80b3e7f7ce525a9c9e2ef10ea97b7c024d9..d6b32caa005633efb84f8f5060808a9b42058d7b 100644
--- a/src/datalink/hand.h
+++ b/src/datalink/hand.h
@@ -1,7 +1,10 @@
#include <vector>
+#ifndef ROBOT_MODELS_H_H
+#define ROBOT_MODELS_H_H
using namespace std;
class hand
{
public:
vector<int> grippos;
};
+#endif //ROBOT_MODELS_TEST_H
\ No newline at end of file
diff --git a/src/datalink/humanspace.h b/src/datalink/humanspace.h
index 038a2e2e574efc1612126599596c99ff010182f0..82c2e5291114062afc66028b409888cbeea7d5f0 100644
--- a/src/datalink/humanspace.h
+++ b/src/datalink/humanspace.h
@@ -1,3 +1,10 @@
+
+#include </home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/world.h>
+#ifndef ROBOT_MODELS_HS_H
+#define ROBOT_MODELS_HS_H
+
+
+
class humanspace : public world
{
public:
@@ -6,3 +13,4 @@ int humannum;
//constructor
};
+#endif //ROBOT_MODELS_TEST_H
\ No newline at end of file
diff --git a/src/datalink/joint.h b/src/datalink/joint.h
index a95bc3db8d9a5b9e6077cef804a32be7d35bea53..479af8cd6b65d6e38a6aaf31f5fe795ef935c30c 100644
--- a/src/datalink/joint.h
+++ b/src/datalink/joint.h
@@ -1,11 +1,8 @@
#include </home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/worldobject.h>
-
+#ifndef ROBOT_MODELS_J_H
+#define ROBOT_MODELS_J_H
class joint : public worldobject
-{ public:
- int j1=0;
-
-
-
-
+{
};
+#endif //ROBOT_MODELS_TEST_H
\ No newline at end of file
diff --git a/src/datalink/obstacle.h b/src/datalink/obstacle.h
index a82c0dbf3e3db603011b962c3b4ce2f96d7f70e8..36c30b38673fd027766336bd492426af1641b377 100644
--- a/src/datalink/obstacle.h
+++ b/src/datalink/obstacle.h
@@ -1,7 +1,10 @@
#include </home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/abstractobject.h>
+#ifndef ROBOT_MODELS_O_H
+#define ROBOT_MODELS_O_H
class obstacle : public abstractobject
{
//constructor
};
+#endif //ROBOT_MODELS_TEST_H
\ No newline at end of file
diff --git a/src/datalink/quaternion.h b/src/datalink/quaternion.h
new file mode 100644
index 0000000000000000000000000000000000000000..1bf6f40056a3213f118b0c01deb2bbf81462d1a5
--- /dev/null
+++ b/src/datalink/quaternion.h
@@ -0,0 +1,14 @@
+#include <vector>
+#ifndef ROBOT_MODELS_Q_H
+#define ROBOT_MODELS_Q_H
+
+class quaternion
+{
+public:
+ float x,y,z,w;
+
+
+ //no need of constructor here
+
+};
+#endif //ROBOT_MODELS_TEST_H
\ No newline at end of file
diff --git a/src/datalink/robot.h b/src/datalink/robot.h
index fdb1627473e23040aee5e6ac48b24cdd7b9bd8c7..d6a045d3576993eadc11475d9df98203fbaecf7f 100644
--- a/src/datalink/robot.h
+++ b/src/datalink/robot.h
@@ -1,9 +1,12 @@
#include </home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/worldobject.h>
-
#include <vector>
+#ifndef ROBOT_MODELS_R_H
+#define ROBOT_MODELS_R_H
using namespace std;
class robot : public worldobject
{
public:
- vector<char> robname;
+ std::string robname;
+
};
+#endif //ROBOT_MODELS_TEST_H
\ No newline at end of file
diff --git a/src/datalink/robotfpe.h b/src/datalink/robotfpe.h
index f253c2371b50a1f12d2fa831f9bafda0909298c0..8958cc712a8474e0541a36eea345fa892d6f4de8 100644
--- a/src/datalink/robotfpe.h
+++ b/src/datalink/robotfpe.h
@@ -1,10 +1,14 @@
#include </home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/robot.h>
#include <vector>
-#include <stdbool.h>
+#include<string>
+#ifndef ROBOT_MODELS_RF_H
+#define ROBOT_MODELS_RF_H
using namespace std;
class robotfpe : public robot
{
public:
bool of,moving,grasppos;
- vector<char> planninggrp;
+
+ std::string planninggrp;
};
+#endif //ROBOT_MODELS_TEST_H
\ No newline at end of file
diff --git a/src/datalink/world.h b/src/datalink/world.h
index f306874d92479fa9d76b66fb534ae16fc39bf700..113f8aee390b3ea0fa4d31b5f8d5885c0e1daf3e 100644
--- a/src/datalink/world.h
+++ b/src/datalink/world.h
@@ -1,7 +1,7 @@
+#ifndef ROBOT_MODELS_W_H
+#define ROBOT_MODELS_W_H
class world
{
-
-
};
-
+#endif //ROBOT_MODELS_TEST_H
diff --git a/src/datalink/worldobject.h b/src/datalink/worldobject.h
index eb8a1865af5a4939ed41788b9528ccc2071b4823..11484d8a3995065d180d99709d0485ff76c0e545 100644
--- a/src/datalink/worldobject.h
+++ b/src/datalink/worldobject.h
@@ -1,11 +1,16 @@
#include <vector>
+#ifndef ROBOT_MODELS_WO_H
+#define ROBOT_MODELS_WO_H
+#include </home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/quaternion.h>
using namespace std;
-class worldobject
+class worldobject : public quaternion
{
public:
- vector<int> p1,or2,h3;
-
+ vector<int> p1,h3;// saw no purpose of using h3 here in WO ,its used seperately in hand class
+ std::string name;
+ quaternion or2;
//no need of constructor here
};
+#endif //ROBOT_MODELS_TEST_H
\ No newline at end of file
diff --git a/src/robot_models_node.cpp b/src/robot_models_node.cpp
index 1a52f6c90ed6d295596f82feee05570bff0328aa..7c759c5d32bb7537079f7794f90d40b84009b88f 100644
--- a/src/robot_models_node.cpp
+++ b/src/robot_models_node.cpp
@@ -9,54 +9,121 @@
#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 <vector>
#include <iostream>
-int main(int argc, char** argv)
-{ int x=0,y=0,z=0;
- ros::init(argc, argv, "ROS NODE 1");
- ros::NodeHandle node_handle("namespace_name");
- ros::AsyncSpinner spinner(1);
- spinner.start();
+#include <tf2_ros/transform_listener.h>
+#include <geometry_msgs/TransformStamped.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <tf2_ros/static_transform_broadcaster.h>
+#include <cstdio>
+#include <tf2/LinearMath/Quaternion.h>
+#include "tf2_ros/message_filter.h"
+#include "message_filters/subscriber.h"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
+#include "geometry_msgs/PointStamped.h"
+#include <tf2_ros/transform_broadcaster.h>
+#include <tf2/LinearMath/Quaternion.h>
+ int main(int argc, char** argv) {
+ ros::init(argc, argv, "ROSNODE1");
+ ros::NodeHandle node_handle("namespace_name");
+ ros::AsyncSpinner spinner(1);
+ spinner.start();
- ROS_INFO("HELLO, WORLD");
- abstractobject o1;
- graspobject o2;
- hand o3;
- humanspace o4;
- joint o5;
- obstacle o6;
- robot o7;
- robotfpe o8;
- world o9;
- worldobject o10;
+ ROS_INFO("HELLO, WORLD");
- o5.j1.p1.push_back(12,11,9,18);
+ abstractobject o1;
+ graspobject o2;
+ hand o3;
+ humanspace o4;
+ joint o5a;
+ joint o5b;
+ joint o5c;
+ joint o5d;
+ joint o5e;
+ joint o5f;
+ joint o5g;
- o8.robname.push_back("Robot FPE");
- o8.of.push_back(1);
- o8.moving.push_back(0);
- o8.grasppos.push_back(1);
- o8.planninggrp.push_back("Panda Teaching");
+ obstacle o6;
+ robot o7;
+ robotfpe o8;
+ world o9;
+ worldobject o10;
- o3.grippos.push_back(12,14,18);
- o4.humannum=1;
+ o5a.name = "panda_joint1";
+ o5b.name = "panda_joint2";
+ o5c.name = "panda_joint3";
+ o5d.name = "panda_joint4";
+ o5e.name = "panda_joint5";
+ o5f.name = "panda_joint6";
+ o5g.name = "panda_joint7";
+ o5a.x = 1.7;
+ o5b.y = 1.8;
+ o5c.z = 1.9;
+ o5d.w = 2.0;
+ o5a.p1 = {12, 11, 9, 18};
- o2.name="Cube";
- o2.size.push_back(5,7,9);
- o2.p1;
- o2.or2;
+ o8.robname = "Robot FPE";
+ o8.of = true;
+ o8.moving = false;
+ o8.grasppos = true;
+ o8.planninggrp = "Panda Teaching";
- o6.name="Ball";
- o6.size.push_back(4,9,6);
- o6.p1;
- o6.or2;
+ o3.grippos = {12, 14, 18};
+
+ o4.humannum = 1;
+
+ o2.name = "Cube";
+ o2.size = {5, 7, 9};
+ o2.p1 = {13, 14, 17};
+ o2.x = 2.3;
+ o2.y = 1.2;
+ o2.z = 0.2;
+ o2.w = 0.1;
+
+
+ o6.name = "Ball";
+ o6.size = {1, 2, 3};
+ o6.p1 = {12, 16, 17};
+ o6.x = 1.1;
+ o6.y = 1.4;
+ o6.z = 1.6;
+ o6.w = 1.9;
-}
+ tf2_ros::Buffer tfBuffer;
+ tf2_ros::TransformListener tfListener(tfBuffer);
+ ros::Rate rate(10);
+
+ while (node_handle.ok()) {
+ //for (const auto &topic : ROSNODE1::topics)
+ {
+ geometry_msgs::TransformStamped transformStamped;
+ try {
+ transformStamped = tfBuffer.lookupTransform("panda_joint1", tfBuffer.o5a, ros::Time(0));
+ transformStamped = tfBuffer.lookupTransform("panda_joint2", tfBuffer.o5b, ros::Time(0));
+ transformStamped = tfBuffer.lookupTransform("panda_joint3", o5c, ros::Time(0));
+ transformStamped = tfBuffer.lookupTransform("panda_joint4", o5d, ros::Time(0));
+ transformStamped = tfBuffer.lookupTransform("panda_joint5", o5e, ros::Time(0));
+ transformStamped = tfBuffer.lookupTransform("panda_joint6", o5f, ros::Time(0));
+ transformStamped = tfBuffer.lookupTransform("panda_joint7", To5g, ros::Time(0));
+ } catch (tf2::TransformException &ex) {
+ ROS_WARN("%s", ex.what());
+ ros::Duration(0.1).sleep();
+ continue;
+
+ }
+ }
+ rate.sleep();
+ }
+
+
+return 0;
+}