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

segfaults

parent 702c76f6
Branches
No related tags found
No related merge requests found
empty
\ No newline at end of file
2
......@@ -39,6 +39,8 @@ vector
/home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/robot.h
worldobject.h
/home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/worldobject.h
joint.h
/home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/joint.h
vector
-
......@@ -87,6 +89,8 @@ datalink/worldobject.h
/home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/worldobject.h
datalink/quaternion.h
/home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/quaternion.h
datalink/joint.h
/home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/joint.h
vector
-
iostream
......
File added
No preview for this file type
#include "worldobject.h"
#include "joint.h"
#include <vector>
#ifndef ROBOT_MODELS_R_H
#define ROBOT_MODELS_R_H
......
......@@ -9,6 +9,7 @@
#include "datalink/world.h"
#include "datalink/worldobject.h"
#include "datalink/quaternion.h"
#include "datalink/joint.h"
#include <vector>
#include <iostream>
#include <tf2_ros/transform_listener.h>
......@@ -44,6 +45,8 @@ void pub(const std_msgs::UInt32::ConstPtr& msg)
spinner.start();
ROS_INFO("HELLO, WORLD");
world world1;
auto robot1 = new robotfpe;
abstractobject o1;
graspobject o2;
hand o3;
......@@ -56,35 +59,35 @@ void pub(const std_msgs::UInt32::ConstPtr& msg)
joint o5f;
joint o5g;
obstacle o6;
robot robot1;
//robot robot1;
robotfpe o8;
world o9;
worldobject o10;
robot1.jo.push_back(o5a);
robot1.jo.push_back(o5b);
robot1.jo.push_back(o5c);
robot1.jo.push_back(o5d);
robot1.jo.push_back(o5e);
robot1.jo.push_back(o5f);
robot1.jo.push_back(o5g);
robot1->jo.push_back(o5a);
robot1->jo.push_back(o5b);
robot1->jo.push_back(o5c);
robot1->jo.push_back(o5d);
robot1->jo.push_back(o5e);
robot1->jo.push_back(o5f);
robot1->jo.push_back(o5g);
int yx = 0;
robot1.jo[++yx].name = "panda_link1";
robot1.jo[++yx].name = "panda_link2";
robot1.jo[++yx].name = "panda_link3";
robot1.jo[++yx].name = "panda_link4";
robot1.jo[++yx].name = "panda_link5";
robot1.jo[++yx].name = "panda_link6";
robot1.jo[++yx].name = "panda_link7";
robot1->jo[++yx].name = "panda_link1";
robot1->jo[++yx].name = "panda_link2";
robot1->jo[++yx].name = "panda_link3";
robot1->jo[++yx].name = "panda_link4";
robot1->jo[++yx].name = "panda_link5";
robot1->jo[++yx].name = "panda_link6";
robot1->jo[++yx].name = "panda_link7";
o9.c->robname = "Robot FPE";
o9.c->of = true;
o9.c->moving = false;
o9.c->grasppos = true;
o9.c->planninggrp = "Panda Teaching";
robot1->robname = "Robot FPE";
robot1->of = true;
robot1->moving = false;
robot1->grasppos = true;
robot1->planninggrp = "Panda Teaching";
o8.h->grippos = {12.1, 14.3, 18.2};
robot1->h->grippos = {12.1, 14.3, 18.2};
auto *obstacle1 = new obstacle{};
obstacle1->name = "Cube";
......@@ -120,7 +123,7 @@ void pub(const std_msgs::UInt32::ConstPtr& msg)
o9.b2->w = 1.9;
*/
ros::Subscriber sub = node_handle.subscribe("updateDectectedHumans", 1000, pub);
ros::spin();
//ros::spin();
tf2_ros::Buffer tfBuffer;
tf2_ros::TransformListener tfListener(tfBuffer);
......@@ -138,13 +141,13 @@ void pub(const std_msgs::UInt32::ConstPtr& msg)
geometry_msgs::TransformStamped transformStamped7;
try {
transformStamped1 = tfBuffer.lookupTransform("world", robot1.jo[1].name, ros::Time(0));
transformStamped2 = tfBuffer.lookupTransform("world", robot1.jo[2].name, ros::Time(0));
transformStamped3 = tfBuffer.lookupTransform("world", robot1.jo[3].name, ros::Time(0));
transformStamped4 = tfBuffer.lookupTransform("world", robot1.jo[4].name, ros::Time(0));
transformStamped5 = tfBuffer.lookupTransform("world", robot1.jo[5].name, ros::Time(0));
transformStamped6 = tfBuffer.lookupTransform("world", robot1.jo[6].name, ros::Time(0));
transformStamped7 = tfBuffer.lookupTransform("world", robot1.jo[7].name, ros::Time(0));
transformStamped1 = tfBuffer.lookupTransform("world", robot1->jo[1].name, ros::Time(0));
transformStamped2 = tfBuffer.lookupTransform("world", robot1->jo[2].name, ros::Time(0));
transformStamped3 = tfBuffer.lookupTransform("world", robot1->jo[3].name, ros::Time(0));
transformStamped4 = tfBuffer.lookupTransform("world", robot1->jo[4].name, ros::Time(0));
transformStamped5 = tfBuffer.lookupTransform("world", robot1->jo[5].name, ros::Time(0));
transformStamped6 = tfBuffer.lookupTransform("world", robot1->jo[6].name, ros::Time(0));
transformStamped7 = tfBuffer.lookupTransform("world", robot1->jo[7].name, ros::Time(0));
o9.humans->humannum =gvar;
} catch (tf2::TransformException &ex) {
ROS_WARN("%s", ex.what());
......@@ -152,155 +155,161 @@ void pub(const std_msgs::UInt32::ConstPtr& msg)
continue;
}
robot1.jo[3].p1[0] = 0.0;
robot1.jo[3].p1[1] = 0.0;
robot1.jo[3].p1[2] = 0.0;
robot1.jo[4].p1[0] = 0.0;
robot1.jo[4].p1[1] = 0.0;
robot1.jo[4].p1[2] = 0.0;
robot1.jo[5].p1[0] = 0.0;
robot1.jo[5].p1[1] = 0.0;
robot1.jo[5].p1[2] = 0.0;
robot1.jo[6].p1[0] = 0.0;
robot1.jo[6].p1[1] = 0.0;
robot1.jo[6].p1[2] = 0.0;
robot1.jo[7].p1[0] = 0.0;
robot1.jo[7].p1[1] = 0.0;
robot1.jo[7].p1[2] = 0.0;
robot1->jo[1].p1[0] = 0.0;
robot1->jo[1].p1[1] = 0.0;
robot1->jo[1].p1[2] = 0.0;
robot1->jo[2].p1[0] = 0.0;
robot1->jo[2].p1[1] = 0.0;
robot1->jo[2].p1[2] = 0.0;
robot1->jo[3].p1[0] = 0.0;
robot1->jo[3].p1[1] = 0.0;
robot1->jo[3].p1[2] = 0.0;
robot1->jo[4].p1[0] = 0.0;
robot1->jo[4].p1[1] = 0.0;
robot1->jo[4].p1[2] = 0.0;
robot1->jo[5].p1[0] = 0.0;
robot1->jo[5].p1[1] = 0.0;
robot1->jo[5].p1[2] = 0.0;
robot1->jo[6].p1[0] = 0.0;
robot1->jo[6].p1[1] = 0.0;
robot1->jo[6].p1[2] = 0.0;
robot1->jo[7].p1[0] = 0.0;
robot1->jo[7].p1[1] = 0.0;
robot1->jo[7].p1[2] = 0.0;
robot1.jo[1].p1[0] = transformStamped1.transform.translation.x;
robot1.jo[1].p1[1] = transformStamped1.transform.translation.y;
robot1.jo[1].p1[2] = transformStamped1.transform.translation.z;
robot1->jo[1].p1[0] = transformStamped1.transform.translation.x;
robot1->jo[1].p1[1] = transformStamped1.transform.translation.y;
robot1->jo[1].p1[2] = transformStamped1.transform.translation.z;
ROS_INFO_STREAM(
"panda_link1 Position is " << " x = " << robot1.jo[1].p1[0] << ", y = " << robot1.jo[1].p1[1]
<< ", z = " << robot1.jo[1].p1[2]);
"panda_link1 Position is " << " x = " << robot1->jo[1].p1[0] << ", y = " << robot1->jo[1].p1[1]
<< ", z = " << robot1->jo[1].p1[2]);
robot1.jo[2].p1[0] = transformStamped2.transform.translation.x;
robot1.jo[2].p1[1] = transformStamped2.transform.translation.y;
robot1.jo[2].p1[2] = transformStamped2.transform.translation.z;
robot1->jo[2].p1[0] = transformStamped2.transform.translation.x;
robot1->jo[2].p1[1] = transformStamped2.transform.translation.y;
robot1->jo[2].p1[2] = transformStamped2.transform.translation.z;
ROS_INFO_STREAM(
"panda_link2 Position is " << " x = " << robot1.jo[2].p1[0] << ", y = " << robot1.jo[2].p1[1]
<< ", z = " << robot1.jo[2].p1[2]);
"panda_link2 Position is " << " x = " << robot1->jo[2].p1[0] << ", y = " << robot1->jo[2].p1[1]
<< ", z = " << robot1->jo[2].p1[2]);
robot1.jo[3].p1[0] = transformStamped3.transform.translation.x;
robot1.jo[3].p1[1] = transformStamped3.transform.translation.y;
robot1.jo[3].p1[2] = transformStamped3.transform.translation.z;
robot1->jo[3].p1[0] = transformStamped3.transform.translation.x;
robot1->jo[3].p1[1] = transformStamped3.transform.translation.y;
robot1->jo[3].p1[2] = transformStamped3.transform.translation.z;
ROS_INFO_STREAM(
"panda_link3 Position is " << " x = " << robot1.jo[3].p1[0] << ", y = " << robot1.jo[3].p1[1]
<< ", z = " << robot1.jo[3].p1[2]);
"panda_link3 Position is " << " x = " << robot1->jo[3].p1[0] << ", y = " << robot1->jo[3].p1[1]
<< ", z = " << robot1->jo[3].p1[2]);
robot1.jo[4].p1[0] = transformStamped4.transform.translation.x;
robot1.jo[4].p1[1] = transformStamped4.transform.translation.y;
robot1.jo[4].p1[2] = transformStamped4.transform.translation.z;
robot1->jo[4].p1[0] = transformStamped4.transform.translation.x;
robot1->jo[4].p1[1] = transformStamped4.transform.translation.y;
robot1->jo[4].p1[2] = transformStamped4.transform.translation.z;
ROS_INFO_STREAM(
"panda_link4 Position is " << " x = " << robot1.jo[4].p1[0] << ", y = " << robot1.jo[4].p1[1]
<< ", z = " << robot1.jo[4].p1[2]);
"panda_link4 Position is " << " x = " << robot1->jo[4].p1[0] << ", y = " << robot1->jo[4].p1[1]
<< ", z = " << robot1->jo[4].p1[2]);
robot1.jo[5].p1[0] = transformStamped5.transform.translation.x;
robot1.jo[5].p1[1] = transformStamped5.transform.translation.y;
robot1.jo[5].p1[2] = transformStamped5.transform.translation.z;
robot1->jo[5].p1[0] = transformStamped5.transform.translation.x;
robot1->jo[5].p1[1] = transformStamped5.transform.translation.y;
robot1->jo[5].p1[2] = transformStamped5.transform.translation.z;
ROS_INFO_STREAM(
"panda_link5 Position is " << " x = " << robot1.jo[5].p1[0] << ", y = " << robot1.jo[5].p1[1]
<< ", z = " << robot1.jo[5].p1[2]);
"panda_link5 Position is " << " x = " << robot1->jo[5].p1[0] << ", y = " << robot1->jo[5].p1[1]
<< ", z = " << robot1->jo[5].p1[2]);
robot1.jo[6].p1[0] = transformStamped6.transform.translation.x;
robot1.jo[6].p1[1] = transformStamped6.transform.translation.y;
robot1.jo[6].p1[2] = transformStamped6.transform.translation.z;
robot1->jo[6].p1[0] = transformStamped6.transform.translation.x;
robot1->jo[6].p1[1] = transformStamped6.transform.translation.y;
robot1->jo[6].p1[2] = transformStamped6.transform.translation.z;
ROS_INFO_STREAM(
"panda_link6 Position is " << " x = " << robot1.jo[6].p1[0] << ", y = " << robot1.jo[6].p1[1]
<< ", z = " << robot1.jo[6].p1[2]);
"panda_link6 Position is " << " x = " << robot1->jo[6].p1[0] << ", y = " << robot1->jo[6].p1[1]
<< ", z = " << robot1->jo[6].p1[2]);
robot1.jo[7].p1[0] = transformStamped7.transform.translation.x;
robot1.jo[7].p1[1] = transformStamped7.transform.translation.y;
robot1.jo[7].p1[2] = transformStamped7.transform.translation.z;
robot1->jo[7].p1[0] = transformStamped7.transform.translation.x;
robot1->jo[7].p1[1] = transformStamped7.transform.translation.y;
robot1->jo[7].p1[2] = transformStamped7.transform.translation.z;
ROS_INFO_STREAM(
"panda_link7 Position is " << " x = " << robot1.jo[7].p1[0] << ", y = " << robot1.jo[7].p1[1]
<< ", z = " << robot1.jo[7].p1[2]);
"panda_link7 Position is " << " x = " << robot1->jo[7].p1[0] << ", y = " << robot1->jo[7].p1[1]
<< ", z = " << robot1->jo[7].p1[2]);
robot1.jo[1].or2.w = 0.0;
robot1.jo[1].or2.x = 0.0;
robot1.jo[1].or2.y = 0.0;
robot1.jo[1].or2.z = 0.0;
robot1.jo[2].or2.w = 0.0;
robot1.jo[2].or2.x = 0.0;
robot1.jo[2].or2.y = 0.0;
robot1.jo[2].or2.z = 0.0;
robot1.jo[3].or2.w = 0.0;
robot1.jo[3].or2.x = 0.0;
robot1.jo[3].or2.y = 0.0;
robot1.jo[3].or2.z = 0.0;
robot1.jo[4].or2.w = 0.0;
robot1.jo[4].or2.x = 0.0;
robot1.jo[4].or2.y = 0.0;
robot1.jo[4].or2.z = 0.0;
robot1.jo[5].or2.w = 0.0;
robot1.jo[5].or2.x = 0.0;
robot1.jo[5].or2.y = 0.0;
robot1.jo[5].or2.z = 0.0;
robot1.jo[6].or2.w = 0.0;
robot1.jo[6].or2.x = 0.0;
robot1.jo[6].or2.y = 0.0;
robot1.jo[6].or2.z = 0.0;
robot1.jo[7].or2.w = 0.0;
robot1.jo[7].or2.x = 0.0;
robot1.jo[7].or2.y = 0.0;
robot1.jo[7].or2.z = 0.0;
robot1->jo[1].or2.w = 0.0;
robot1->jo[1].or2.x = 0.0;
robot1->jo[1].or2.y = 0.0;
robot1->jo[1].or2.z = 0.0;
robot1->jo[2].or2.w = 0.0;
robot1->jo[2].or2.x = 0.0;
robot1->jo[2].or2.y = 0.0;
robot1->jo[2].or2.z = 0.0;
robot1->jo[3].or2.w = 0.0;
robot1->jo[3].or2.x = 0.0;
robot1->jo[3].or2.y = 0.0;
robot1->jo[3].or2.z = 0.0;
robot1->jo[4].or2.w = 0.0;
robot1->jo[4].or2.x = 0.0;
robot1->jo[4].or2.y = 0.0;
robot1->jo[4].or2.z = 0.0;
robot1->jo[5].or2.w = 0.0;
robot1->jo[5].or2.x = 0.0;
robot1->jo[5].or2.y = 0.0;
robot1->jo[5].or2.z = 0.0;
robot1->jo[6].or2.w = 0.0;
robot1->jo[6].or2.x = 0.0;
robot1->jo[6].or2.y = 0.0;
robot1->jo[6].or2.z = 0.0;
robot1->jo[7].or2.w = 0.0;
robot1->jo[7].or2.x = 0.0;
robot1->jo[7].or2.y = 0.0;
robot1->jo[7].or2.z = 0.0;
robot1.jo[1].or2.w = transformStamped1.transform.rotation.w;
robot1.jo[1].or2.x = transformStamped1.transform.rotation.x;
robot1.jo[1].or2.y = transformStamped1.transform.rotation.y;
robot1.jo[1].or2.z = transformStamped1.transform.rotation.z;
robot1->jo[1].or2.w = transformStamped1.transform.rotation.w;
robot1->jo[1].or2.x = transformStamped1.transform.rotation.x;
robot1->jo[1].or2.y = transformStamped1.transform.rotation.y;
robot1->jo[1].or2.z = transformStamped1.transform.rotation.z;
ROS_INFO_STREAM(
"panda_link1 Orientation is " << " w = " << robot1.jo[1].or2.w << ", x = " << robot1.jo[1].or2.x
<< ", y = " << robot1.jo[1].or2.y << ", z = " << robot1.jo[1].or2.z);
"panda_link1 Orientation is " << " w = " << robot1->jo[1].or2.w << ", x = " << robot1->jo[1].or2.x
<< ", y = " << robot1->jo[1].or2.y << ", z = " << robot1->jo[1].or2.z);
robot1.jo[2].or2.w = transformStamped2.transform.rotation.w;
robot1.jo[2].or2.x = transformStamped2.transform.rotation.x;
robot1.jo[2].or2.y = transformStamped2.transform.rotation.y;
robot1.jo[2].or2.z = transformStamped2.transform.rotation.z;
robot1->jo[2].or2.w = transformStamped2.transform.rotation.w;
robot1->jo[2].or2.x = transformStamped2.transform.rotation.x;
robot1->jo[2].or2.y = transformStamped2.transform.rotation.y;
robot1->jo[2].or2.z = transformStamped2.transform.rotation.z;
ROS_INFO_STREAM(
"panda_link2 Orientation is " << " w = " << robot1.jo[2].or2.w << ", x = " << robot1.jo[2].or2.x
<< ", y = " << robot1.jo[2].or2.y << ", z = " << robot1.jo[2].or2.z);
"panda_link2 Orientation is " << " w = " << robot1->jo[2].or2.w << ", x = " << robot1->jo[2].or2.x
<< ", y = " << robot1->jo[2].or2.y << ", z = " << robot1->jo[2].or2.z);
robot1.jo[3].or2.w = transformStamped3.transform.rotation.w;
robot1.jo[3].or2.x = transformStamped3.transform.rotation.x;
robot1.jo[3].or2.y = transformStamped3.transform.rotation.y;
robot1.jo[3].or2.z = transformStamped3.transform.rotation.z;
robot1->jo[3].or2.w = transformStamped3.transform.rotation.w;
robot1->jo[3].or2.x = transformStamped3.transform.rotation.x;
robot1->jo[3].or2.y = transformStamped3.transform.rotation.y;
robot1->jo[3].or2.z = transformStamped3.transform.rotation.z;
ROS_INFO_STREAM(
"panda_link3 Orientation is " << " w = " << robot1.jo[3].or2.w << ", x = " << robot1.jo[3].or2.x
<< ", y = " << robot1.jo[3].or2.y << ", z = " << robot1.jo[3].or2.z);
"panda_link3 Orientation is " << " w = " << robot1->jo[3].or2.w << ", x = " << robot1->jo[3].or2.x
<< ", y = " << robot1->jo[3].or2.y << ", z = " << robot1->jo[3].or2.z);
robot1.jo[4].or2.w = transformStamped4.transform.rotation.w;
robot1.jo[4].or2.x = transformStamped4.transform.rotation.x;
robot1.jo[4].or2.y = transformStamped4.transform.rotation.y;
robot1.jo[4].or2.z = transformStamped4.transform.rotation.z;
robot1->jo[4].or2.w = transformStamped4.transform.rotation.w;
robot1->jo[4].or2.x = transformStamped4.transform.rotation.x;
robot1->jo[4].or2.y = transformStamped4.transform.rotation.y;
robot1->jo[4].or2.z = transformStamped4.transform.rotation.z;
ROS_INFO_STREAM(
"panda_link4 Orientation is " << " w = " << robot1.jo[4].or2.w << ", x = " << robot1.jo[4].or2.x
<< ", y = " << robot1.jo[4].or2.y << ", z = " << robot1.jo[4].or2.z);
"panda_link4 Orientation is " << " w = " << robot1->jo[4].or2.w << ", x = " << robot1->jo[4].or2.x
<< ", y = " << robot1->jo[4].or2.y << ", z = " << robot1->jo[4].or2.z);
robot1.jo[5].or2.w = transformStamped5.transform.rotation.w;
robot1.jo[5].or2.x = transformStamped5.transform.rotation.x;
robot1.jo[5].or2.y = transformStamped5.transform.rotation.y;
robot1.jo[5].or2.z = transformStamped5.transform.rotation.z;
robot1->jo[5].or2.w = transformStamped5.transform.rotation.w;
robot1->jo[5].or2.x = transformStamped5.transform.rotation.x;
robot1->jo[5].or2.y = transformStamped5.transform.rotation.y;
robot1->jo[5].or2.z = transformStamped5.transform.rotation.z;
ROS_INFO_STREAM(
"panda_link5 Orientation is " << " w = " << robot1.jo[5].or2.w << ", x = " << robot1.jo[5].or2.x
<< ", y = " << robot1.jo[5].or2.y << ", z = " << robot1.jo[5].or2.z);
"panda_link5 Orientation is " << " w = " << robot1->jo[5].or2.w << ", x = " << robot1->jo[5].or2.x
<< ", y = " << robot1->jo[5].or2.y << ", z = " << robot1->jo[5].or2.z);
robot1.jo[6].or2.w = transformStamped6.transform.rotation.w;
robot1.jo[6].or2.x = transformStamped6.transform.rotation.x;
robot1.jo[6].or2.y = transformStamped6.transform.rotation.y;
robot1.jo[6].or2.z = transformStamped6.transform.rotation.z;
robot1->jo[6].or2.w = transformStamped6.transform.rotation.w;
robot1->jo[6].or2.x = transformStamped6.transform.rotation.x;
robot1->jo[6].or2.y = transformStamped6.transform.rotation.y;
robot1->jo[6].or2.z = transformStamped6.transform.rotation.z;
ROS_INFO_STREAM(
"panda_link6 Orientation is " << " w = " << robot1.jo[6].or2.w << ", x = " << robot1.jo[6].or2.x
<< ", y = " << robot1.jo[6].or2.y << ", z = " << robot1.jo[6].or2.z);
"panda_link6 Orientation is " << " w = " << robot1->jo[6].or2.w << ", x = " << robot1->jo[6].or2.x
<< ", y = " << robot1->jo[6].or2.y << ", z = " << robot1->jo[6].or2.z);
robot1.jo[7].or2.w = transformStamped7.transform.rotation.w;
robot1.jo[7].or2.x = transformStamped7.transform.rotation.x;
robot1.jo[7].or2.y = transformStamped7.transform.rotation.y;
robot1.jo[7].or2.z = transformStamped7.transform.rotation.z;
robot1->jo[7].or2.w = transformStamped7.transform.rotation.w;
robot1->jo[7].or2.x = transformStamped7.transform.rotation.x;
robot1->jo[7].or2.y = transformStamped7.transform.rotation.y;
robot1->jo[7].or2.z = transformStamped7.transform.rotation.z;
ROS_INFO_STREAM(
"panda_link7 Orientation is " << " w = " << robot1.jo[7].or2.w << ", x = " << robot1.jo[7].or2.x
<< ", y = " << robot1.jo[7].or2.y << ", z = " << robot1.jo[7].or2.z);
"panda_link7 Orientation is " << " w = " << robot1->jo[7].or2.w << ", x = " << robot1->jo[7].or2.x
<< ", y = " << robot1->jo[7].or2.y << ", z = " << robot1->jo[7].or2.z);
}
rate.sleep();
}
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment