Skip to content
Snippets Groups Projects
Commit 2c93f880 authored by Nikhil Ambardar's avatar Nikhil Ambardar
Browse files

Update robot_models_node.cpp

parent 629a1aa2
No related branches found
No related tags found
No related merge requests found
Show whitespace changes
Inline Side-by-side
src/robot_models_node.cpp
+ 92
− 85
View file @ 2c93f880
#include <ros/ros.h> #include <ros/ros.h>
#include </home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/abstractobject.h> #include "datalink/abstractobject.h"
#include </home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/graspobject.h> #include "datalink/graspobject.h"
#include </home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/hand.h> #include "datalink/hand.h"
#include </home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/humanspace.h> #include "datalink/humanspace.h"
#include </home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/joint.h> #include "datalink/joint.h"
#include </home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/obstacle.h> #include "datalink/obstacle.h"
#include </home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/robot.h> #include "datalink/robot.h"
#include </home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/robotfpe.h> #include "datalink/robotfpe.h"
#include </home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/world.h> #include "datalink/world.h"
#include </home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/worldobject.h> #include "datalink/worldobject.h"
#include </home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/quaternion.h> #include "datalink/quaternion.h"
#include <vector> #include <vector>
#include <iostream> #include <iostream>
#include <tf2_ros/transform_listener.h> #include <tf2_ros/transform_listener.h>
...@@ -30,9 +30,9 @@ ...@@ -30,9 +30,9 @@
ros::NodeHandle node_handle("namespace_name"); ros::NodeHandle node_handle("namespace_name");
ros::AsyncSpinner spinner(1); ros::AsyncSpinner spinner(1);
spinner.start(); spinner.start();
ROS_INFO("HELLO, WORLD");
ROS_INFO("HELLO, WORLD");
abstractobject o1; abstractobject o1;
...@@ -140,15 +140,22 @@ ...@@ -140,15 +140,22 @@
while (node_handle.ok()) { while (node_handle.ok()) {
//for (const auto &topic : ROSNODE1::topics) //for (const auto &topic : ROSNODE1::topics)
{ {
geometry_msgs::TransformStamped transformStamped; 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 { try {
transformStamped = tfBuffer.lookupTransform("world", o5a.name, ros::Time(0)); transformStamped1 = tfBuffer.lookupTransform("world", o5a.name, ros::Time(0));
transformStamped = tfBuffer.lookupTransform("world", o5b.name, ros::Time(0)); transformStamped2 = tfBuffer.lookupTransform("world", o5b.name, ros::Time(0));
transformStamped = tfBuffer.lookupTransform("world", o5c.name, ros::Time(0)); transformStamped3 = tfBuffer.lookupTransform("world", o5c.name, ros::Time(0));
transformStamped = tfBuffer.lookupTransform("world", o5d.name, ros::Time(0)); transformStamped4 = tfBuffer.lookupTransform("world", o5d.name, ros::Time(0));
transformStamped = tfBuffer.lookupTransform("world", o5e.name, ros::Time(0)); transformStamped5 = tfBuffer.lookupTransform("world", o5e.name, ros::Time(0));
transformStamped = tfBuffer.lookupTransform("world", o5f.name, ros::Time(0)); transformStamped6 = tfBuffer.lookupTransform("world", o5f.name, ros::Time(0));
transformStamped = tfBuffer.lookupTransform("world", o5g.name, ros::Time(0)); transformStamped7 = tfBuffer.lookupTransform("world", o5g.name, ros::Time(0));
} catch (tf2::TransformException &ex) { } catch (tf2::TransformException &ex) {
ROS_WARN("%s", ex.what()); ROS_WARN("%s", ex.what());
ros::Duration(0.1).sleep(); ros::Duration(0.1).sleep();
...@@ -156,70 +163,70 @@ ...@@ -156,70 +163,70 @@
} }
//o5a.p1[0] = 0.0;
o5a.p1.push_back(transformStamped.transform.translation.x); o5a.p1[0]=transformStamped1.transform.translation.x;
o5a.p1.push_back(transformStamped.transform.translation.y); o5a.p1[1]=transformStamped1.transform.translation.y;
o5a.p1.push_back(transformStamped.transform.translation.z); o5a.p1[2]=transformStamped1.transform.translation.z;
o5a.p1[0] = 0.0; //ROS_INFO("%f",&o5a.p1);
o5b.p1.push_back(transformStamped.transform.translation.x); o5b.p1[0]=transformStamped2.transform.translation.x;
o5b.p1.push_back(transformStamped.transform.translation.y); o5b.p1[1]=transformStamped2.transform.translation.y;
o5b.p1.push_back(transformStamped.transform.translation.z); o5b.p1[2]=transformStamped2.transform.translation.z;
o5b.p1[0] = 0.0;
o5c.p1.push_back(transformStamped.transform.translation.x); o5c.p1[0]=transformStamped3.transform.translation.x;
o5c.p1.push_back(transformStamped.transform.translation.y); o5c.p1[1]=transformStamped3.transform.translation.y;
o5c.p1.push_back(transformStamped.transform.translation.z); o5c.p1[2]=transformStamped3.transform.translation.z;
o5c.p1[0] = 0.0;
o5d.p1.push_back(transformStamped.transform.translation.x); o5d.p1[0]=transformStamped4.transform.translation.x;
o5d.p1.push_back(transformStamped.transform.translation.y); o5d.p1[1]=transformStamped4.transform.translation.y;
o5d.p1.push_back(transformStamped.transform.translation.z); o5d.p1[2]=transformStamped4.transform.translation.z;
o5d.p1[0] = 0.0;
o5e.p1.push_back(transformStamped.transform.translation.x); o5e.p1[0]=transformStamped5.transform.translation.x;
o5e.p1.push_back(transformStamped.transform.translation.y); o5e.p1[1]=transformStamped5.transform.translation.y;
o5e.p1.push_back(transformStamped.transform.translation.z); o5e.p1[2]=transformStamped5.transform.translation.z;
o5e.p1[0] = 0.0;
o5f.p1.push_back(transformStamped.transform.translation.x); o5f.p1[0]=transformStamped6.transform.translation.x;
o5f.p1.push_back(transformStamped.transform.translation.y); o5f.p1[1]=transformStamped6.transform.translation.y;
o5f.p1.push_back(transformStamped.transform.translation.z); o5f.p1[2]=transformStamped6.transform.translation.z;
o5f.p1[0] = 0.0;
o5g.p1.push_back(transformStamped.transform.translation.x); o5g.p1[0]=transformStamped7.transform.translation.x;
o5g.p1.push_back(transformStamped.transform.translation.y); o5g.p1[1]=transformStamped7.transform.translation.y;
o5g.p1.push_back(transformStamped.transform.translation.z); o5g.p1[2]=transformStamped7.transform.translation.z;
o5g.p1[0] = 0.0;
o5a.or2.w=transformStamped.transform.rotation.w; o5a.or2.w=transformStamped1.transform.rotation.w;
o5a.or2.x=transformStamped.transform.rotation.x; o5a.or2.x=transformStamped1.transform.rotation.x;
o5a.or2.y=transformStamped.transform.rotation.y; o5a.or2.y=transformStamped1.transform.rotation.y;
o5a.or2.z=transformStamped.transform.rotation.z; o5a.or2.z=transformStamped1.transform.rotation.z;
o5b.or2.w=transformStamped.transform.rotation.w; o5b.or2.w=transformStamped2.transform.rotation.w;
o5b.or2.x=transformStamped.transform.rotation.x; o5b.or2.x=transformStamped2.transform.rotation.x;
o5b.or2.y=transformStamped.transform.rotation.y; o5b.or2.y=transformStamped2.transform.rotation.y;
o5b.or2.z=transformStamped.transform.rotation.z; o5b.or2.z=transformStamped2.transform.rotation.z;
o5c.or2.w=transformStamped.transform.rotation.w; o5c.or2.w=transformStamped3.transform.rotation.w;
o5c.or2.x=transformStamped.transform.rotation.x; o5c.or2.x=transformStamped3.transform.rotation.x;
o5c.or2.y=transformStamped.transform.rotation.y; o5c.or2.y=transformStamped3.transform.rotation.y;
o5c.or2.z=transformStamped.transform.rotation.z; o5c.or2.z=transformStamped3.transform.rotation.z;
o5d.or2.w=transformStamped.transform.rotation.w; o5d.or2.w=transformStamped4.transform.rotation.w;
o5d.or2.x=transformStamped.transform.rotation.x; o5d.or2.x=transformStamped4.transform.rotation.x;
o5d.or2.y=transformStamped.transform.rotation.y; o5d.or2.y=transformStamped4.transform.rotation.y;
o5d.or2.z=transformStamped.transform.rotation.z; o5d.or2.z=transformStamped4.transform.rotation.z;
o5e.or2.w=transformStamped.transform.rotation.w; o5e.or2.w=transformStamped5.transform.rotation.w;
o5e.or2.x=transformStamped.transform.rotation.x; o5e.or2.x=transformStamped5.transform.rotation.x;
o5e.or2.y=transformStamped.transform.rotation.y; o5e.or2.y=transformStamped5.transform.rotation.y;
o5e.or2.z=transformStamped.transform.rotation.z; o5e.or2.z=transformStamped5.transform.rotation.z;
o5f.or2.w=transformStamped.transform.rotation.w; o5f.or2.w=transformStamped6.transform.rotation.w;
o5f.or2.x=transformStamped.transform.rotation.x; o5f.or2.x=transformStamped6.transform.rotation.x;
o5f.or2.y=transformStamped.transform.rotation.y; o5f.or2.y=transformStamped6.transform.rotation.y;
o5f.or2.z=transformStamped.transform.rotation.z; o5f.or2.z=transformStamped6.transform.rotation.z;
o5g.or2.w=transformStamped.transform.rotation.w; o5g.or2.w=transformStamped7.transform.rotation.w;
o5g.or2.x=transformStamped.transform.rotation.x; o5g.or2.x=transformStamped7.transform.rotation.x;
o5g.or2.y=transformStamped.transform.rotation.y; o5g.or2.y=transformStamped7.transform.rotation.y;
o5g.or2.z=transformStamped.transform.rotation.z; o5g.or2.z=transformStamped7.transform.rotation.z;
} }
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment