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
GitLab community forum
Contribute to GitLab
Provide feedback
Keyboard shortcuts
?
Snippets
Groups
Projects
Show more breadcrumbs
Nikhil Ambardar
zero
Commits
1d3649ef
Commit
1d3649ef
authored
4 years ago
by
Nikhil Ambardar
Browse files
Options
Downloads
Patches
Plain Diff
Update testappm.cpp
parent
8b18ea2d
No related branches found
No related tags found
No related merge requests found
Changes
1
Show whitespace changes
Inline
Side-by-side
Showing
1 changed file
src/testappm.cpp
+42
-181
42 additions, 181 deletions
src/testappm.cpp
with
42 additions
and
181 deletions
src/testappm.cpp
+
42
−
181
View file @
1d3649ef
...
...
@@ -24,9 +24,8 @@
#include
"geometry_msgs/PointStamped.h"
#include
<tf2_ros/transform_broadcaster.h>
#include
<tf2/LinearMath/Quaternion.h>
#include
<moveit/move_group_interface/move_group_interface.h>
#include
<moveit/planning_scene_interface/planning_scene_interface.h>
int
main
(
int
argc
,
char
**
argv
)
{
ros
::
init
(
argc
,
argv
,
"TestNode"
);
ros
::
NodeHandle
node_handle
(
"namespace_name"
);
...
...
@@ -34,18 +33,12 @@ int main(int argc, char** argv) {
spinner
.
start
();
ROS_INFO
(
"Welcome to Test Code"
);
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
;
...
...
@@ -53,38 +46,37 @@ int main(int argc, char** argv) {
joint
o5e
;
joint
o5f
;
joint
o5g
;
*/
obstacle
o6
;
robot
o7
;
robotfpe
o8
;
world
o9
;
worldobject
o10
;
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
.
jo
.
push_back
(
o5a
);
o7
.
jo
.
push_back
(
o5b
);
o7
.
jo
.
push_back
(
o5c
);
o7
.
jo
.
push_back
(
o5d
);
o7
.
jo
.
push_back
(
o5e
);
o7
.
jo
.
push_back
(
o5f
);
o7
.
jo
.
push_back
(
o5g
);
// test if the robot contains 7 joints
if
(
o7
.
jo
.
size
()
==
5
)
{
ROS_INFO
(
"SIZE IS 5"
);
}
else
{
ROS_INFO_STREAM
(
"SIZE IS NOT 5 but was "
<<
o7
.
jo
.
size
());
}
/* o7.jo1->or2= {1.2,5.3,6.5,7.4};
o7.jo2->or2= {2.5,3.6,5.8,4.9};
o7.jo3->or2= {5.1,1.2,9.9,6.8};
o7.jo4->or2= {6.3,3.4,8.5,9.6};
o7.jo5->or2= {8.2,2.6,7.9,5.5};
o7.jo6->or2= {7.3,4.4,6.7,4.0};
o7.jo7->or2= {7.3,4.4,6.7,4.0};
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->p1= {1.3, 10.1, 9.4};
o7.jo2->p1= {14.7, 11.3, 6.5};
o7.jo3->p1= {4.9, 13.8, 7.7};
o7.jo4->p1= {2.4, 12.5, 1.6};
o7.jo5->p1= {3.1, 16.2, 5.3};
o7.jo6->p1= {11.5, 15.4, 3.4};
o7.jo7->p1= {5.1, 18.3, 4.2};
*/
o9
.
c
->
robname
=
"Robot FPE"
;
o9
.
c
->
of
=
true
;
o9
.
c
->
moving
=
false
;
...
...
@@ -114,171 +106,40 @@ int main(int argc, char** argv) {
tf2_ros
::
TransformListener
tfListener
(
tfBuffer
);
ros
::
Rate
rate
(
10
);
int
i
=
1
;
int
h
=
1
;
int
i
=
0
;
int
h
o
=
0
;
while
(
node_handle
.
ok
())
{
//for (const auto &topic : ROSNODE1::topics)
{
geometry_msgs
::
TransformStamped
transformStamped
[
7
];
try
{
transformStamped
[
i
++
]
=
tfBuffer
.
lookupTransform
(
"world"
,
o7
.
jo
[
h
++
]
->
name
,
ros
::
Time
(
0
));
}
catch
(
tf2
::
TransformException
&
ex
)
{
try
{
transformStamped
[
++
i
]
=
tfBuffer
.
lookupTransform
(
"world"
,
o7
.
jo
[
++
ho
].
name
,
ros
::
Time
(
0
));
}
catch
(
tf2
::
TransformException
&
ex
)
{
ROS_WARN
(
"%s"
,
ex
.
what
());
ros
::
Duration
(
0.1
).
sleep
();
continue
;
}
if
(
h
==
7
)
ROS_INFO
(
" TEST CASE SUCCESSFUL. ROBOT FPE HAS 7 JOINTS "
);
if
(
h
o
==
7
)
ROS_INFO
_STREAM
(
" TEST CASE SUCCESSFUL. ROBOT FPE HAS 7 JOINTS "
);
else
ROS_ERROR
(
" TEST CASE
NOT SUCCESSFUL
. ROBOT FPE HAS WRONG NUMBER OF JOINTS
"
);
ROS_ERROR
_STREAM
(
" TEST CASE
FAILED
. ROBOT FPE HAS WRONG NUMBER OF JOINTS
= "
<<
ho
);
// 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"
);
ROS_ERROR
(
"ERROR IN POINTER WORLD 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