From 7bf7e6faac2813365f5847ddf4635093b20f5aaf Mon Sep 17 00:00:00 2001
From: Nikhil Ambardar <202-nikaviator@users.noreply.git-st.inf.tu-dresden.de>
Date: Wed, 23 Sep 2020 02:12:56 +0200
Subject: [PATCH] Avoiding use of Terminal to push
---
src/datalink/abstractobject.h | 8 +-
src/datalink/graspobject.h | 4 +-
src/datalink/hand.h | 3 +
src/datalink/humanspace.h | 8 ++
src/datalink/joint.h | 11 +--
src/datalink/obstacle.h | 3 +
src/datalink/quaternion.h | 14 ++++
src/datalink/robot.h | 7 +-
src/datalink/robotfpe.h | 8 +-
src/datalink/world.h | 6 +-
src/datalink/worldobject.h | 11 ++-
src/robot_models_node.cpp | 135 +++++++++++++++++++++++++---------
12 files changed, 164 insertions(+), 54 deletions(-)
create mode 100644 src/datalink/quaternion.h
diff --git a/src/datalink/abstractobject.h b/src/datalink/abstractobject.h
index 327492b..0612732 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 4f0e095..e15791a 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 1465f80..d6b32ca 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 038a2e2..82c2e52 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 a95bc3d..479af8c 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 a82c0db..36c30b3 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 0000000..1bf6f40
--- /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 fdb1627..d6a045d 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 f253c23..8958cc7 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 f306874..113f8ae 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 eb8a186..11484d8 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 1a52f6c..7c759c5 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;
+}
--
GitLab