From 9e25d3ed07f0815607dd94aba6523607256bad07 Mon Sep 17 00:00:00 2001
From: Nikhil Ambardar <202-nikaviator@users.noreply.git-st.inf.tu-dresden.de>
Date: Mon, 19 Oct 2020 21:00:10 +0000
Subject: [PATCH] Upload New File
---
src/testappm.cpp | 146 +++++++++++++++++++++++++++++++++++++++++++++++
1 file changed, 146 insertions(+)
create mode 100644 src/testappm.cpp
diff --git a/src/testappm.cpp b/src/testappm.cpp
new file mode 100644
index 0000000..8e790e1
--- /dev/null
+++ b/src/testappm.cpp
@@ -0,0 +1,146 @@
+#include <ros/ros.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>
+#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, "TestNode");
+ ros::NodeHandle node_handle("namespace_name");
+ ros::AsyncSpinner spinner(1);
+ spinner.start();
+ ROS_INFO("Welcome to Test Code");
+
+ abstractobject o1;
+ graspobject o2;
+ hand o3;
+ humanspace o4;
+
+ joint o5a;
+ joint o5b;
+ joint o5c;
+ joint o5d;
+ joint o5e;
+ joint o5f;
+ joint o5g;
+
+ obstacle o6;
+ robot o7;
+ robotfpe o8;
+ world o9;
+ worldobject o10;
+
+ o7.jo1->name= "panda_link1";
+ o7.jo2->name= "panda_link2";
+ o7.jo3->name= "panda_link3";
+ o7.jo4->name= "panda_link4";
+ o7.jo5->name= "panda_link5";
+ o7.jo6->name= "panda_link6";
+ o7.jo7->name= "panda_link7";
+
+ /* o7.jo1->or2= {1.2,5.3,6.5,7.4};
+ o7.jo2->or2= {2.5,3.6,5.8,4.9};
+ o7.jo3->or2= {5.1,1.2,9.9,6.8};
+ o7.jo4->or2= {6.3,3.4,8.5,9.6};
+ o7.jo5->or2= {8.2,2.6,7.9,5.5};
+ o7.jo6->or2= {7.3,4.4,6.7,4.0};
+ o7.jo7->or2= {7.3,4.4,6.7,4.0};
+
+ o7.jo1->p1= {1.3, 10.1, 9.4};
+ o7.jo2->p1= {14.7, 11.3, 6.5};
+ o7.jo3->p1= {4.9, 13.8, 7.7};
+ o7.jo4->p1= {2.4, 12.5, 1.6};
+ o7.jo5->p1= {3.1, 16.2, 5.3};
+ o7.jo6->p1= {11.5, 15.4, 3.4};
+ o7.jo7->p1= {5.1, 18.3, 4.2};
+*/
+ o9.c->robname="Robot FPE";
+ o9.c->of=true;
+ o9.c->moving=false;
+ o9.c->grasppos=true;
+ o9.c->planninggrp= "Panda Teaching";
+
+ o8.h->grippos= {12.1, 14.3, 18.2};
+ o3.grippos={1.1,2.3,2.1};
+
+ o9.a->humannum = 1;
+ o9.b1->name="Cube";
+ o9.b1->size={5.2, 7.5, 9.1};
+ o9.b1->p1= {13.2, 14.1, 17.5};
+ o9.b1->x= 2.3;
+ o9.b1->y = 1.2;
+ o9.b1->z = 0.2;
+ o9.b1->w = 0.1;
+ o9.b2->name="Ball";
+ o9.b2->size={1.3, 2.1, 3.2};
+ o9.b2->p1= {12.5, 16.4, 17.3};
+ o9.b2->x= 1.1;
+ o9.b2->y = 1.4;
+ o9.b2->z = 1.6;
+ o9.b2->w = 1.9;
+
+ tf2_ros::Buffer tfBuffer;
+ tf2_ros::TransformListener tfListener(tfBuffer);
+
+ ros::Rate rate(10);
+ int i=0;
+ while (node_handle.ok()) {
+ //for (const auto &topic : ROSNODE1::topics)
+ {
+
+ geometry_msgs::TransformStamped transformStamped[7];
+
+
+ try {
+
+ transformStamped[i++] = tfBuffer.lookupTransform("world", o7.jo1->name, ros::Time(0));
+ transformStamped[i++] = tfBuffer.lookupTransform("world", o7.jo2->name, ros::Time(0));
+ transformStamped[i++] = tfBuffer.lookupTransform("world", o7.jo3->name, ros::Time(0));
+ transformStamped[i++] = tfBuffer.lookupTransform("world", o7.jo4->name, ros::Time(0));
+ transformStamped[i++] = tfBuffer.lookupTransform("world", o7.jo5->name, ros::Time(0));
+ transformStamped[i++] = tfBuffer.lookupTransform("world", o7.jo6->name, ros::Time(0));
+ transformStamped[i++] = tfBuffer.lookupTransform("world", o7.jo7->name, ros::Time(0));
+
+
+ } catch (tf2::TransformException &ex) {
+ ROS_WARN("%s", ex.what());
+ ros::Duration(0.1).sleep();
+ continue;
+
+ }
+ if(i==7)
+ ROS_INFO("TEST CASE SUCCESSFUL.ROBOT FPE HAS 7 JOINTS");
+ else
+ ROS_INFO("TEST CASE NOT SUCCESSFUL.ROBOT FPE HAS WRONG NUMBER OF JOINTS");
+
+
+
+
+ }
+ rate.sleep();
+ }
+
+
+return 0;
+}
--
GitLab