From 2c93f880e208e657447e51bbdaf65ea4bd405eec Mon Sep 17 00:00:00 2001 From: Nikhil Ambardar <202-nikaviator@users.noreply.git-st.inf.tu-dresden.de> Date: Fri, 2 Oct 2020 10:55:50 +0000 Subject: [PATCH] Update robot_models_node.cpp --- src/robot_models_node.cpp | 177 ++++++++++++++++++++------------------ 1 file changed, 92 insertions(+), 85 deletions(-) diff --git a/src/robot_models_node.cpp b/src/robot_models_node.cpp index b37fe89..ee85be1 100644 --- a/src/robot_models_node.cpp +++ b/src/robot_models_node.cpp @@ -1,15 +1,15 @@ #include <ros/ros.h> -#include </home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/abstractobject.h> -#include </home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/graspobject.h> -#include </home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/hand.h> -#include </home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/humanspace.h> -#include </home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/joint.h> -#include </home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/obstacle.h> -#include </home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/robot.h> -#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 "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> @@ -30,9 +30,9 @@ ros::NodeHandle node_handle("namespace_name"); ros::AsyncSpinner spinner(1); spinner.start(); + ROS_INFO("HELLO, WORLD"); - ROS_INFO("HELLO, WORLD"); abstractobject o1; @@ -140,15 +140,22 @@ while (node_handle.ok()) { //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 { - transformStamped = tfBuffer.lookupTransform("world", o5a.name, ros::Time(0)); - transformStamped = tfBuffer.lookupTransform("world", o5b.name, ros::Time(0)); - transformStamped = tfBuffer.lookupTransform("world", o5c.name, ros::Time(0)); - transformStamped = tfBuffer.lookupTransform("world", o5d.name, ros::Time(0)); - transformStamped = tfBuffer.lookupTransform("world", o5e.name, ros::Time(0)); - transformStamped = tfBuffer.lookupTransform("world", o5f.name, ros::Time(0)); - transformStamped = tfBuffer.lookupTransform("world", o5g.name, ros::Time(0)); + 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(); @@ -156,70 +163,70 @@ } - - o5a.p1.push_back(transformStamped.transform.translation.x); - o5a.p1.push_back(transformStamped.transform.translation.y); - o5a.p1.push_back(transformStamped.transform.translation.z); - o5a.p1[0] = 0.0; - o5b.p1.push_back(transformStamped.transform.translation.x); - o5b.p1.push_back(transformStamped.transform.translation.y); - o5b.p1.push_back(transformStamped.transform.translation.z); - o5b.p1[0] = 0.0; - o5c.p1.push_back(transformStamped.transform.translation.x); - o5c.p1.push_back(transformStamped.transform.translation.y); - o5c.p1.push_back(transformStamped.transform.translation.z); - o5c.p1[0] = 0.0; - o5d.p1.push_back(transformStamped.transform.translation.x); - o5d.p1.push_back(transformStamped.transform.translation.y); - o5d.p1.push_back(transformStamped.transform.translation.z); - o5d.p1[0] = 0.0; - o5e.p1.push_back(transformStamped.transform.translation.x); - o5e.p1.push_back(transformStamped.transform.translation.y); - o5e.p1.push_back(transformStamped.transform.translation.z); - o5e.p1[0] = 0.0; - o5f.p1.push_back(transformStamped.transform.translation.x); - o5f.p1.push_back(transformStamped.transform.translation.y); - o5f.p1.push_back(transformStamped.transform.translation.z); - o5f.p1[0] = 0.0; - o5g.p1.push_back(transformStamped.transform.translation.x); - o5g.p1.push_back(transformStamped.transform.translation.y); - o5g.p1.push_back(transformStamped.transform.translation.z); - o5g.p1[0] = 0.0; - - o5a.or2.w=transformStamped.transform.rotation.w; - o5a.or2.x=transformStamped.transform.rotation.x; - o5a.or2.y=transformStamped.transform.rotation.y; - o5a.or2.z=transformStamped.transform.rotation.z; - - o5b.or2.w=transformStamped.transform.rotation.w; - o5b.or2.x=transformStamped.transform.rotation.x; - o5b.or2.y=transformStamped.transform.rotation.y; - o5b.or2.z=transformStamped.transform.rotation.z; - - o5c.or2.w=transformStamped.transform.rotation.w; - o5c.or2.x=transformStamped.transform.rotation.x; - o5c.or2.y=transformStamped.transform.rotation.y; - o5c.or2.z=transformStamped.transform.rotation.z; - - o5d.or2.w=transformStamped.transform.rotation.w; - o5d.or2.x=transformStamped.transform.rotation.x; - o5d.or2.y=transformStamped.transform.rotation.y; - o5d.or2.z=transformStamped.transform.rotation.z; - - o5e.or2.w=transformStamped.transform.rotation.w; - o5e.or2.x=transformStamped.transform.rotation.x; - o5e.or2.y=transformStamped.transform.rotation.y; - o5e.or2.z=transformStamped.transform.rotation.z; - - o5f.or2.w=transformStamped.transform.rotation.w; - o5f.or2.x=transformStamped.transform.rotation.x; - o5f.or2.y=transformStamped.transform.rotation.y; - o5f.or2.z=transformStamped.transform.rotation.z; - - o5g.or2.w=transformStamped.transform.rotation.w; - o5g.or2.x=transformStamped.transform.rotation.x; - o5g.or2.y=transformStamped.transform.rotation.y; - o5g.or2.z=transformStamped.transform.rotation.z; + //o5a.p1[0] = 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("%f",&o5a.p1); + o5b.p1[0]=transformStamped2.transform.translation.x; + o5b.p1[1]=transformStamped2.transform.translation.y; + o5b.p1[2]=transformStamped2.transform.translation.z; + + o5c.p1[0]=transformStamped3.transform.translation.x; + o5c.p1[1]=transformStamped3.transform.translation.y; + o5c.p1[2]=transformStamped3.transform.translation.z; + + o5d.p1[0]=transformStamped4.transform.translation.x; + o5d.p1[1]=transformStamped4.transform.translation.y; + o5d.p1[2]=transformStamped4.transform.translation.z; + + o5e.p1[0]=transformStamped5.transform.translation.x; + o5e.p1[1]=transformStamped5.transform.translation.y; + o5e.p1[2]=transformStamped5.transform.translation.z; + + o5f.p1[0]=transformStamped6.transform.translation.x; + o5f.p1[1]=transformStamped6.transform.translation.y; + o5f.p1[2]=transformStamped6.transform.translation.z; + + o5g.p1[0]=transformStamped7.transform.translation.x; + o5g.p1[1]=transformStamped7.transform.translation.y; + o5g.p1[2]=transformStamped7.transform.translation.z; + + + 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; + + 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; + + 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; + + 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; + + 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; + + 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; + + 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; } @@ -228,4 +235,4 @@ return 0; -} \ No newline at end of file +} -- GitLab