Skip to content
GitLab
Explore
Sign in
Register
Primary navigation
Search or go to…
Project
Z
zero
Manage
Activity
Members
Labels
Plan
Issues
Issue boards
Milestones
Wiki
Code
Merge requests
Repository
Branches
Commits
Tags
Repository graph
Compare revisions
Snippets
Build
Pipelines
Jobs
Pipeline schedules
Artifacts
Deploy
Releases
Container registry
Model registry
Operate
Environments
Monitor
Incidents
Analyze
Value stream analytics
Contributor analytics
CI/CD analytics
Repository analytics
Model experiments
Help
Help
Support
GitLab documentation
Compare GitLab plans
Community forum
Contribute to GitLab
Provide feedback
Keyboard shortcuts
?
Snippets
Groups
Projects
Show more breadcrumbs
Nikhil Ambardar
zero
Commits
85bc26d4
Commit
85bc26d4
authored
4 years ago
by
Nikhil Ambardar
Browse files
Options
Downloads
Patches
Plain Diff
Update testappm.cpp
parent
0769ee2d
No related branches found
No related tags found
No related merge requests found
Changes
1
Hide whitespace changes
Inline
Side-by-side
Showing
1 changed file
src/testappm.cpp
+168
-30
168 additions, 30 deletions
src/testappm.cpp
with
168 additions
and
30 deletions
src/testappm.cpp
+
168
−
30
View file @
85bc26d4
...
...
@@ -25,18 +25,27 @@
#include
<tf2_ros/transform_broadcaster.h>
#include
<tf2/LinearMath/Quaternion.h>
int
main
(
int
argc
,
char
**
argv
)
{
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
;
static
const
std
::
string
PLANNING_GROUP
=
"panda_arm"
;
moveit
::
planning_interface
::
MoveGroupInterface
move_group
(
PLANNING_GROUP
);
moveit
::
planning_interface
::
PlanningSceneInterface
planning_scene_interface
;
ros
::
Duration
(
3.0
).
sleep
();
abstractobject
o1
;
graspobject
o2
;
hand
o3
;
humanspace
o4
;
/*
joint o5a;
joint o5b;
joint o5c;
...
...
@@ -44,20 +53,21 @@
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"
;
int
yx
=
0
;
o7
.
jo
[
++
yx
]
->
name
=
"panda_link1"
;
o7
.
jo
[
++
yx
]
->
name
=
"panda_link2"
;
o7
.
jo
[
++
yx
]
->
name
=
"panda_link3"
;
o7
.
jo
[
++
yx
]
->
name
=
"panda_link4"
;
o7
.
jo
[
++
yx
]
->
name
=
"panda_link5"
;
o7
.
jo
[
++
yx
]
->
name
=
"panda_link6"
;
o7
.
jo
[
++
yx
]
->
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};
...
...
@@ -104,43 +114,171 @@
tf2_ros
::
TransformListener
tfListener
(
tfBuffer
);
ros
::
Rate
rate
(
10
);
int
i
=
0
;
int
i
=
1
;
int
h
=
1
;
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
));
try
{
transformStamped
[
i
++
]
=
tfBuffer
.
lookupTransform
(
"world"
,
o7
.
jo
[
h
++
]
->
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"
);
if
(
h
==
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"
);
ROS_ERROR
(
" TEST CASE NOT SUCCESSFUL. ROBOT FPE HAS WRONG NUMBER OF JOINTS "
);
// TEST CASES FOR ALL AGGREGATION RELATIONS POINTERS
if
(
o7
.
jo
[
1
]
||
o7
.
jo
[
2
]
||
o7
.
jo
[
3
]
||
o7
.
jo
[
4
]
||
o7
.
jo
[
5
]
||
o7
.
jo
[
6
]
||
o7
.
jo
[
7
]
==
nullptr
)
ROS_ERROR
(
"ERROR IN JOINTS POINTER AGGREGATION RELATIONSHIP"
);
if
(
o8
.
h
==
nullptr
)
ROS_ERROR
(
"ERROR IN HAND POINTER IN ROBOT FPE CLASS AGGREGATION RELATION"
);
if
(
o9
.
a
==
nullptr
)
ROS_ERROR
(
"ERROR IN POINTER HUMANSPACE AGGREGATION"
);
if
(
o9
.
c
==
nullptr
)
ROS_ERROR
(
"ERROR IN ROBOT CLASS AGGREGATION"
);
if
(
o9
.
b1
==
nullptr
)
ROS_ERROR
(
"ERROR IN ABSTRACT OBJECT CLASS AGGREGATION RELATION"
);
if
(
o9
.
b2
==
nullptr
)
ROS_ERROR
(
"ERROR IN ABSTRACT OBJECT CLASS AGGREGATION RELATION"
);
/* o7.jo1->p1[0]=0.0;
o7.jo1->p1[1]=0.0;
o7.jo1->p1[2]=0.0;
o7.jo2->p1[0]=0.0;
o7.jo2->p1[1]=0.0;
o7.jo2->p1[2]=0.0;
o7.jo3->p1[0]=0.0;
o7.jo3->p1[1]=0.0;
o7.jo3->p1[2]=0.0;
o7.jo4->p1[0]=0.0;
o7.jo4->p1[1]=0.0;
o7.jo4->p1[2]=0.0;
o7.jo5->p1[0]=0.0;
o7.jo5->p1[1]=0.0;
o7.jo5->p1[2]=0.0;
o7.jo6->p1[0]=0.0;
o7.jo6->p1[1]=0.0;
o7.jo6->p1[2]=0.0;
o7.jo7->p1[0]=0.0;
o7.jo7->p1[1]=0.0;
o7.jo7->p1[2]=0.0;
o7.jo1->p1[0]=transformStamped1.transform.translation.x;
o7.jo1->p1[1]=transformStamped1.transform.translation.y;
o7.jo1->p1[2]=transformStamped1.transform.translation.z;
ROS_INFO_STREAM("panda_link1 Position is " <<" x = "<< o7.jo1->p1[0] << ", y = "<< o7.jo1->p1[1]<<", z = "<<o7.jo1->p1[2]);
o7.jo2->p1[0]=transformStamped2.transform.translation.x;
o7.jo2->p1[1]=transformStamped2.transform.translation.y;
o7.jo2->p1[2]=transformStamped2.transform.translation.z;
ROS_INFO_STREAM("panda_link2 Position is " <<" x = "<< o7.jo2->p1[0] << ", y = "<< o7.jo2->p1[1]<<", z = "<<o7.jo2->p1[2]);
o7.jo3->p1[0]=transformStamped3.transform.translation.x;
o7.jo3->p1[1]=transformStamped3.transform.translation.y;
o7.jo3->p1[2]=transformStamped3.transform.translation.z;
ROS_INFO_STREAM("panda_link3 Position is " <<" x = "<< o7.jo3->p1[0] << ", y = "<< o7.jo3->p1[1]<<", z = "<<o7.jo3->p1[2]);
o7.jo4->p1[0]=transformStamped4.transform.translation.x;
o7.jo4->p1[1]=transformStamped4.transform.translation.y;
o7.jo4->p1[2]=transformStamped4.transform.translation.z;
ROS_INFO_STREAM("panda_link4 Position is " <<" x = "<< o7.jo4->p1[0] << ", y = "<< o7.jo4->p1[1]<<", z = "<<o7.jo4->p1[2]);
o7.jo5->p1[0]=transformStamped5.transform.translation.x;
o7.jo5->p1[1]=transformStamped5.transform.translation.y;
o7.jo5->p1[2]=transformStamped5.transform.translation.z;
ROS_INFO_STREAM("panda_link5 Position is " <<" x = "<< o7.jo5->p1[0] << ", y = "<< o7.jo5->p1[1]<<", z = "<<o7.jo5->p1[2]);
o7.jo6->p1[0]=transformStamped6.transform.translation.x;
o7.jo6->p1[1]=transformStamped6.transform.translation.y;
o7.jo6->p1[2]=transformStamped6.transform.translation.z;
ROS_INFO_STREAM("panda_link6 Position is " <<" x = "<< o7.jo6->p1[0] << ", y = "<< o7.jo6->p1[1]<<", z = "<<o7.jo6->p1[2]);
o7.jo7->p1[0]=transformStamped7.transform.translation.x;
o7.jo7->p1[0]=transformStamped7.transform.translation.y;
o7.jo7->p1[0]=transformStamped7.transform.translation.z;
ROS_INFO_STREAM("panda_link7 Position is " <<" x = "<< o7.jo7->p1[0] << ", y = "<< o7.jo7->p1[1]<<", z = "<<o7.jo7->p1[2]);
o7.jo1->or2.w=0.0;
o7.jo1->or2.x=0.0;
o7.jo1->or2.y=0.0;
o7.jo1->or2.z=0.0;
o7.jo2->or2.w=0.0;
o7.jo2->or2.x=0.0;
o7.jo2->or2.y=0.0;
o7.jo2->or2.z=0.0;
o7.jo3->or2.w=0.0;
o7.jo3->or2.x=0.0;
o7.jo3->or2.y=0.0;
o7.jo3->or2.z=0.0;
o7.jo4->or2.w=0.0;
o7.jo4->or2.x=0.0;
o7.jo4->or2.y=0.0;
o7.jo4->or2.z=0.0;
o7.jo5->or2.w=0.0;
o7.jo5->or2.x=0.0;
o7.jo5->or2.y=0.0;
o7.jo5->or2.z=0.0;
o7.jo6->or2.w=0.0;
o7.jo6->or2.x=0.0;
o7.jo6->or2.y=0.0;
o7.jo6->or2.z=0.0;
o7.jo7->or2.w=0.0;
o7.jo7->or2.x=0.0;
o7.jo7->or2.y=0.0;
o7.jo7->or2.z=0.0;
o7.jo1->or2.w=transformStamped1.transform.rotation.w;
o7.jo1->or2.x=transformStamped1.transform.rotation.x;
o7.jo1->or2.y=transformStamped1.transform.rotation.y;
o7.jo1->or2.z=transformStamped1.transform.rotation.z;
ROS_INFO_STREAM("panda_link1 Orientation is " <<" w = "<< o7.jo1->or2.w<<", x = "<<o7.jo1->or2.x<<", y = "<<o7.jo1->or2.y<<", z = "<<o7.jo1->or2.z);
o7.jo2->or2.w=transformStamped2.transform.rotation.w;
o7.jo2->or2.x=transformStamped2.transform.rotation.x;
o7.jo2->or2.y=transformStamped2.transform.rotation.y;
o7.jo2->or2.z=transformStamped2.transform.rotation.z;
ROS_INFO_STREAM("panda_link2 Orientation is " <<" w = "<< o7.jo2->or2.w<<", x = "<< o7.jo2->or2.x<<", y = "<< o7.jo2->or2.y<<", z = "<< o7.jo2->or2.z);
o7.jo3->or2.w=transformStamped3.transform.rotation.w;
o7.jo3->or2.x=transformStamped3.transform.rotation.x;
o7.jo3->or2.y=transformStamped3.transform.rotation.y;
o7.jo3->or2.z=transformStamped3.transform.rotation.z;
ROS_INFO_STREAM("panda_link3 Orientation is " <<" w = "<< o7.jo3->or2.w<<", x = "<<o7.jo3->or2.x<<", y = "<<o7.jo3->or2.y<<", z = "<<o7.jo3->or2.z);
o7.jo4->or2.w=transformStamped4.transform.rotation.w;
o7.jo4->or2.x=transformStamped4.transform.rotation.x;
o7.jo4->or2.y=transformStamped4.transform.rotation.y;
o7.jo4->or2.z=transformStamped4.transform.rotation.z;
ROS_INFO_STREAM("panda_link4 Orientation is " <<" w = "<<o7.jo4->or2.w<<", x = "<<o7.jo4->or2.x<<", y = "<<o7.jo4->or2.y<<", z = "<<o7.jo4->or2.z);
o7.jo5->or2.w=transformStamped5.transform.rotation.w;
o7.jo5->or2.x=transformStamped5.transform.rotation.x;
o7.jo5->or2.y=transformStamped5.transform.rotation.y;
o7.jo5->or2.z=transformStamped5.transform.rotation.z;
ROS_INFO_STREAM("panda_link5 Orientation is " <<" w = "<< o7.jo5->or2.w<<", x = "<<o7.jo5->or2.x<<", y = "<<o7.jo5->or2.y<<", z = "<<o7.jo5->or2.z);
o7.jo6->or2.w=transformStamped6.transform.rotation.w;
o7.jo6->or2.x=transformStamped6.transform.rotation.x;
o7.jo6->or2.y=transformStamped6.transform.rotation.y;
o7.jo6->or2.z=transformStamped6.transform.rotation.z;
ROS_INFO_STREAM("panda_link6 Orientation is " <<" w = "<< o7.jo6->or2.w<<", x = "<<o7.jo6->or2.x<<", y = "<<o7.jo6->or2.y<<", z = "<<o7.jo6->or2.z);
o7.jo7->or2.w=transformStamped7.transform.rotation.w;
o7.jo7->or2.x=transformStamped7.transform.rotation.x;
o7.jo7->or2.y=transformStamped7.transform.rotation.y;
o7.jo7->or2.z=transformStamped7.transform.rotation.z;
ROS_INFO_STREAM("panda_link7 Orientation is " <<" w = "<<o7.jo7->or2.w<<", x = "<<o7.jo7->or2.x<<", y = "<<o7.jo7->or2.y<<", z = "<<o7.jo7->or2.z);
*/
}
rate
.
sleep
();
}
return
0
;
}
}
\ No newline at end of file
This diff is collapsed.
Click to expand it.
Preview
0%
Loading
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Save comment
Cancel
Please
register
or
sign in
to comment