#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, "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 o5a; joint o5b; joint o5c; joint o5d; joint o5e; joint o5f; joint o5g; obstacle o6; robot o7; robotfpe o8; world o9; worldobject o10; o5a.name = "panda_link1"; o5b.name = "panda_link2"; o5c.name = "panda_link3"; o5d.name = "panda_link4"; o5e.name = "panda_link5"; o5f.name = "panda_link6"; o5g.name = "panda_link7"; o5a.or2 = {1.2,5.3,6.5,7.4}; o5b.or2 = {2.5,3.6,5.8,4.9}; o5c.or2 = {5.1,1.2,9.9,6.8}; o5d.or2 = {6.3,3.4,8.5,9.6}; o5e.or2 = {8.2,2.6,7.9,5.5}; o5f.or2 = {7.3,4.4,6.7,4.0}; o5g.or2 = {9.0,5.2,5.5,8.7}; o5a.p1 = {1.3, 10.1, 9.4}; o5b.p1 = {14.7, 11.3, 6.5}; o5c.p1 = {4.9, 13.8, 7.7}; o5d.p1 = {2.4, 12.5, 1.6}; o5e.p1 = {3.1, 16.2, 5.3}; o5f.p1 = {11.5, 15.4, 3.4}; o5g.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}; 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); while (node_handle.ok()) { //for (const auto &topic : ROSNODE1::topics) { 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 { 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(); continue; } o5a.p1[0]=0.0; o5a.p1[1]=0.0; o5a.p1[2]=0.0; o5b.p1[0]=0.0; o5b.p1[1]=0.0; o5b.p1[2]=0.0; o5c.p1[0]=0.0; o5c.p1[1]=0.0; o5c.p1[2]=0.0; o5d.p1[0]=0.0; o5d.p1[1]=0.0; o5d.p1[2]=0.0; o5e.p1[0]=0.0; o5e.p1[1]=0.0; o5e.p1[2]=0.0; o5f.p1[0]=0.0; o5f.p1[1]=0.0; o5f.p1[2]=0.0; o5g.p1[0]=0.0; o5g.p1[1]=0.0; o5g.p1[2]=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_STREAM("panda_link1 Position is " <<" x = "<< o5a.p1[0] << ", y = "<< o5a.p1[1]<<", z = "<<o5a.p1[2]); o5b.p1[0]=transformStamped2.transform.translation.x; o5b.p1[1]=transformStamped2.transform.translation.y; o5b.p1[2]=transformStamped2.transform.translation.z; ROS_INFO_STREAM("panda_link2 Position is " <<" x = "<< o5b.p1[0] << ", y = "<< o5b.p1[1]<<", z = "<<o5b.p1[2]); o5c.p1[0]=transformStamped3.transform.translation.x; o5c.p1[1]=transformStamped3.transform.translation.y; o5c.p1[2]=transformStamped3.transform.translation.z; ROS_INFO_STREAM("panda_link3 Position is " <<" x = "<< o5c.p1[0] << ", y = "<< o5c.p1[1]<<", z = "<<o5c.p1[2]); o5d.p1[0]=transformStamped4.transform.translation.x; o5d.p1[1]=transformStamped4.transform.translation.y; o5d.p1[2]=transformStamped4.transform.translation.z; ROS_INFO_STREAM("panda_link4 Position is " <<" x = "<< o5d.p1[0] << ", y = "<< o5d.p1[1]<<", z = "<<o5d.p1[2]); o5e.p1[0]=transformStamped5.transform.translation.x; o5e.p1[1]=transformStamped5.transform.translation.y; o5e.p1[2]=transformStamped5.transform.translation.z; ROS_INFO_STREAM("panda_link5 Position is " <<" x = "<< o5e.p1[0] << ", y = "<< o5e.p1[1]<<", z = "<<o5e.p1[2]); o5f.p1[0]=transformStamped6.transform.translation.x; o5f.p1[1]=transformStamped6.transform.translation.y; o5f.p1[2]=transformStamped6.transform.translation.z; ROS_INFO_STREAM("panda_link6 Position is " <<" x = "<< o5f.p1[0] << ", y = "<< o5f.p1[1]<<", z = "<<o5f.p1[2]); o5g.p1[0]=transformStamped7.transform.translation.x; o5g.p1[1]=transformStamped7.transform.translation.y; o5g.p1[2]=transformStamped7.transform.translation.z; ROS_INFO_STREAM("panda_link7 Position is " <<" x = "<< o5g.p1[0] << ", y = "<< o5g.p1[1]<<", z = "<<o5g.p1[2]); o5a.or2.w=0.0; o5a.or2.x=0.0; o5a.or2.y=0.0; o5a.or2.z=0.0; o5b.or2.w=0.0; o5b.or2.x=0.0; o5b.or2.y=0.0; o5b.or2.z=0.0; o5c.or2.w=0.0; o5c.or2.x=0.0; o5c.or2.y=0.0; o5c.or2.z=0.0; o5d.or2.w=0.0; o5d.or2.x=0.0; o5d.or2.y=0.0; o5d.or2.z=0.0; o5e.or2.w=0.0; o5e.or2.x=0.0; o5e.or2.y=0.0; o5e.or2.z=0.0; o5f.or2.w=0.0; o5f.or2.x=0.0; o5f.or2.y=0.0; o5f.or2.z=0.0; o5g.or2.w=0.0; o5g.or2.x=0.0; o5g.or2.y=0.0; o5g.or2.z=0.0; 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; ROS_INFO_STREAM("panda_link1 Orientation is " <<" w = "<< o5a.or2.w<<", x = "<<o5a.or2.x<<", y = "<<o5a.or2.y<<", z = "<<o5a.or2.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; ROS_INFO_STREAM("panda_link2 Orientation is " <<" w = "<< o5b.or2.w<<", x = "<<o5b.or2.x<<", y = "<<o5b.or2.y<<", z = "<<o5b.or2.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; ROS_INFO_STREAM("panda_link3 Orientation is " <<" w = "<< o5c.or2.w<<", x = "<<o5c.or2.x<<", y = "<<o5c.or2.y<<", z = "<<o5c.or2.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; ROS_INFO_STREAM("panda_link4 Orientation is " <<" w = "<< o5d.or2.w<<", x = "<<o5d.or2.x<<", y = "<<o5d.or2.y<<", z = "<<o5d.or2.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; ROS_INFO_STREAM("panda_link5 Orientation is " <<" w = "<< o5e.or2.w<<", x = "<<o5e.or2.x<<", y = "<<o5e.or2.y<<", z = "<<o5e.or2.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; ROS_INFO_STREAM("panda_link6 Orientation is " <<" w = "<< o5f.or2.w<<", x = "<<o5f.or2.x<<", y = "<<o5f.or2.y<<", z = "<<o5f.or2.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; ROS_INFO_STREAM("panda_link7 Orientation is " <<" w = "<< o5g.or2.w<<", x = "<<o5g.or2.x<<", y = "<<o5g.or2.y<<", z = "<<o5g.or2.z); } rate.sleep(); } return 0; }