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
e175c92d
Commit
e175c92d
authored
4 years ago
by
Nikhil Ambardar
Browse files
Options
Downloads
Patches
Plain Diff
temp
parent
b37291c6
Branches
Branches containing commit
No related tags found
No related merge requests found
Changes
3
Show whitespace changes
Inline
Side-by-side
Showing
3 changed files
src/datalink/humanspace.h
+2
-2
2 additions, 2 deletions
src/datalink/humanspace.h
src/robot_models_node.cpp
+29
-4
29 additions, 4 deletions
src/robot_models_node.cpp
src/worldsafety.cpp
+52
-48
52 additions, 48 deletions
src/worldsafety.cpp
with
83 additions
and
54 deletions
src/datalink/humanspace.h
+
2
−
2
View file @
e175c92d
...
@@ -4,8 +4,8 @@
...
@@ -4,8 +4,8 @@
class
humanspace
class
humanspace
{
{
public:
public:
int
humannum
;
//
int humannum;
uint32
humannum
;
//constructor
//constructor
...
...
This diff is collapsed.
Click to expand it.
src/robot_models_node.cpp
+
29
−
4
View file @
e175c92d
...
@@ -26,6 +26,12 @@
...
@@ -26,6 +26,12 @@
#include
<tf2/LinearMath/Quaternion.h>
#include
<tf2/LinearMath/Quaternion.h>
#include
<moveit/move_group_interface/move_group_interface.h>
#include
<moveit/move_group_interface/move_group_interface.h>
#include
<moveit/planning_scene_interface/planning_scene_interface.h>
#include
<moveit/planning_scene_interface/planning_scene_interface.h>
void
updateDectectedHumans
(
const
std_msgs
::
uint32
::
ConstPtr
&
msg
)
{
ROS_INFO
(
"I heard: [%s]"
,
msg
->
data
.
c_str
());
}
int
main
(
int
argc
,
char
**
argv
)
{
int
main
(
int
argc
,
char
**
argv
)
{
ros
::
init
(
argc
,
argv
,
"ROSNODE1"
);
ros
::
init
(
argc
,
argv
,
"ROSNODE1"
);
ros
::
NodeHandle
node_handle
(
"namespace_name"
);
ros
::
NodeHandle
node_handle
(
"namespace_name"
);
...
@@ -117,9 +123,26 @@
...
@@ -117,9 +123,26 @@
o9
.
b2
->
z
=
1.6
;
o9
.
b2
->
z
=
1.6
;
o9
.
b2
->
w
=
1.9
;
o9
.
b2
->
w
=
1.9
;
ros
::
Publisher
chatter_pub
=
namespace_name
.
advertise
<
std_msgs
::
String
>
(
"chatter"
,
1000
);
ros
::
Rate
loop_rate
(
10
);
int
count
=
0
;
while
(
ros
::
ok
())
{
std_msgs
::
uint32
msg
;
std
::
stringstream
ss
;
ss
<<
"hello world "
<<
count
;
msg
.
data
=
ss
.
str
();
ROS_INFO
(
"%s"
,
msg
.
data
.
c_str
());
chatter_pub
.
publish
(
msg
);
ros
::
spinOnce
();
loop_rate
.
sleep
();
++
count
;
}
ros
::
Subscriber
sub
=
namespace_name
.
subscribe
(
"chatter"
,
1000
,
updateDectectedHumans
);
ros
::
spin
();
tf2_ros
::
Buffer
tfBuffer
;
tf2_ros
::
Buffer
tfBuffer
;
tf2_ros
::
TransformListener
tfListener
(
tfBuffer
);
tf2_ros
::
TransformListener
tfListener
(
tfBuffer
);
ros
::
Rate
rate
(
10
);
ros
::
Rate
rate
(
10
);
while
(
node_handle
.
ok
())
{
while
(
node_handle
.
ok
())
{
...
@@ -132,7 +155,8 @@
...
@@ -132,7 +155,8 @@
geometry_msgs
::
TransformStamped
transformStamped5
;
geometry_msgs
::
TransformStamped
transformStamped5
;
geometry_msgs
::
TransformStamped
transformStamped6
;
geometry_msgs
::
TransformStamped
transformStamped6
;
geometry_msgs
::
TransformStamped
transformStamped7
;
geometry_msgs
::
TransformStamped
transformStamped7
;
uint32
::
data
updateDetectedHumans
;
geometry_msgs
::
uint32
x
;
try
{
try
{
transformStamped1
=
tfBuffer
.
lookupTransform
(
"world"
,
o7
.
jo
[
1
].
name
,
ros
::
Time
(
0
));
transformStamped1
=
tfBuffer
.
lookupTransform
(
"world"
,
o7
.
jo
[
1
].
name
,
ros
::
Time
(
0
));
transformStamped2
=
tfBuffer
.
lookupTransform
(
"world"
,
o7
.
jo
[
2
].
name
,
ros
::
Time
(
0
));
transformStamped2
=
tfBuffer
.
lookupTransform
(
"world"
,
o7
.
jo
[
2
].
name
,
ros
::
Time
(
0
));
...
@@ -141,6 +165,7 @@
...
@@ -141,6 +165,7 @@
transformStamped5
=
tfBuffer
.
lookupTransform
(
"world"
,
o7
.
jo
[
5
].
name
,
ros
::
Time
(
0
));
transformStamped5
=
tfBuffer
.
lookupTransform
(
"world"
,
o7
.
jo
[
5
].
name
,
ros
::
Time
(
0
));
transformStamped6
=
tfBuffer
.
lookupTransform
(
"world"
,
o7
.
jo
[
6
].
name
,
ros
::
Time
(
0
));
transformStamped6
=
tfBuffer
.
lookupTransform
(
"world"
,
o7
.
jo
[
6
].
name
,
ros
::
Time
(
0
));
transformStamped7
=
tfBuffer
.
lookupTransform
(
"world"
,
o7
.
jo
[
7
].
name
,
ros
::
Time
(
0
));
transformStamped7
=
tfBuffer
.
lookupTransform
(
"world"
,
o7
.
jo
[
7
].
name
,
ros
::
Time
(
0
));
updateDetectedHumans
=
o9
.
a
->
humannum
;
}
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
();
...
@@ -200,8 +225,8 @@
...
@@ -200,8 +225,8 @@
ROS_INFO_STREAM
(
"panda_link6 Position is "
<<
" x = "
<<
o7
.
jo
[
6
].
p1
[
0
]
<<
", y = "
<<
o7
.
jo
[
6
].
p1
[
1
]
<<
", z = "
<<
o7
.
jo
[
6
].
p1
[
2
]);
ROS_INFO_STREAM
(
"panda_link6 Position is "
<<
" x = "
<<
o7
.
jo
[
6
].
p1
[
0
]
<<
", y = "
<<
o7
.
jo
[
6
].
p1
[
1
]
<<
", z = "
<<
o7
.
jo
[
6
].
p1
[
2
]);
o7
.
jo
[
7
].
p1
[
0
]
=
transformStamped7
.
transform
.
translation
.
x
;
o7
.
jo
[
7
].
p1
[
0
]
=
transformStamped7
.
transform
.
translation
.
x
;
o7
.
jo
[
7
].
p1
[
0
]
=
transformStamped7
.
transform
.
translation
.
y
;
o7
.
jo
[
7
].
p1
[
1
]
=
transformStamped7
.
transform
.
translation
.
y
;
o7
.
jo
[
7
].
p1
[
0
]
=
transformStamped7
.
transform
.
translation
.
z
;
o7
.
jo
[
7
].
p1
[
2
]
=
transformStamped7
.
transform
.
translation
.
z
;
ROS_INFO_STREAM
(
"panda_link7 Position is "
<<
" x = "
<<
o7
.
jo
[
7
].
p1
[
0
]
<<
", y = "
<<
o7
.
jo
[
7
].
p1
[
1
]
<<
", z = "
<<
o7
.
jo
[
7
].
p1
[
2
]);
ROS_INFO_STREAM
(
"panda_link7 Position is "
<<
" x = "
<<
o7
.
jo
[
7
].
p1
[
0
]
<<
", y = "
<<
o7
.
jo
[
7
].
p1
[
1
]
<<
", z = "
<<
o7
.
jo
[
7
].
p1
[
2
]);
o7
.
jo
[
1
].
or2
.
w
=
0.0
;
o7
.
jo
[
1
].
or2
.
w
=
0.0
;
...
...
This diff is collapsed.
Click to expand it.
src/worldsafety.cpp
+
52
−
48
View file @
e175c92d
...
@@ -64,12 +64,12 @@ int main(int argc, char** argv)
...
@@ -64,12 +64,12 @@ int main(int argc, char** argv)
o7
.
jo
.
push_back
(
o5f
);
o7
.
jo
.
push_back
(
o5f
);
o7
.
jo
.
push_back
(
o5g
);
o7
.
jo
.
push_back
(
o5g
);
// test if the robot contains 7 joints static test
/*
// test if the robot contains 7 joints static test
if (o7.jo.size() == 5) {
if (o7.jo.size() == 5) {
ROS_INFO("SIZE IS 5");
ROS_INFO("SIZE IS 5");
} else {
} else {
ROS_INFO_STREAM("SIZE IS NOT 5 but was " << o7.jo.size());
ROS_INFO_STREAM("SIZE IS NOT 5 but was " << o7.jo.size());
}
}
*/
int
yx
=
0
;
int
yx
=
0
;
o7
.
jo
[
++
yx
].
name
=
"panda_link1"
;
o7
.
jo
[
++
yx
].
name
=
"panda_link1"
;
...
@@ -129,35 +129,6 @@ int main(int argc, char** argv)
...
@@ -129,35 +129,6 @@ int main(int argc, char** argv)
// Move the (simulated) robot in gazebo
// Move the (simulated) robot in gazebo
move_group.move();*/
move_group.move();*/
tf2_ros
::
Buffer
tfBuffer
;
tf2_ros
::
TransformListener
tfListener
(
tfBuffer
);
ros
::
Rate
rate
(
10
);
int
i
=
0
;
int
ho
=
0
;
while
(
node_handle
.
ok
())
{
{
geometry_msgs
::
TransformStamped
transformStamped
[
7
];
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
;
}
//Checking if robot moved to position or not
if
(
o7
.
jo
[
7
].
p1
[
2
]
==
0.9
)
ROS_INFO_STREAM
(
" THE ROBOT HAS MOVED TO POSITION AS INTENDED "
);
else
ROS_ERROR_STREAM
(
"THE ROBOT HAS NOT MOVED TO THE POSITION AS INTENDED"
);
ros
::
shutdown
();
// World Model Safety Test Cases to check Segmentation errors
ros
::
Rate
rate
(
10
);
// test the properties of the model that do not change over time, such as the structure of you model
if
(
o7
.
jo
.
size
()
==
7
)
if
(
o7
.
jo
.
size
()
==
7
)
ROS_INFO_STREAM
(
" TEST CASE SUCCESSFUL. ROBOT FPE HAS 7 JOINTS "
);
ROS_INFO_STREAM
(
" TEST CASE SUCCESSFUL. ROBOT FPE HAS 7 JOINTS "
);
else
else
...
@@ -178,12 +149,6 @@ int main(int argc, char** argv)
...
@@ -178,12 +149,6 @@ int main(int argc, char** argv)
if
(
o9
.
b2
==
nullptr
)
if
(
o9
.
b2
==
nullptr
)
ROS_ERROR
(
"ERROR IN ABSTRACT OBJECT CLASS AGGREGATION RELATION"
);
ROS_ERROR
(
"ERROR IN ABSTRACT OBJECT CLASS AGGREGATION RELATION"
);
while
(
node_handle
.
ok
())
{
{
// update the model data like in the application model node
// test the dynamic parts here, i.e., if the model data is consistent with the position of the robot
static
const
std
::
string
PLANNING_GROUP
=
"panda_arm"
;
static
const
std
::
string
PLANNING_GROUP
=
"panda_arm"
;
moveit
::
planning_interface
::
MoveGroupInterface
move_group
(
PLANNING_GROUP
);
moveit
::
planning_interface
::
MoveGroupInterface
move_group
(
PLANNING_GROUP
);
moveit
::
planning_interface
::
PlanningSceneInterface
planning_scene_interface
;
moveit
::
planning_interface
::
PlanningSceneInterface
planning_scene_interface
;
...
@@ -205,6 +170,45 @@ int main(int argc, char** argv)
...
@@ -205,6 +170,45 @@ int main(int argc, char** argv)
// Move the (simulated) robot in gazebo
// Move the (simulated) robot in gazebo
move_group
.
move
();
move_group
.
move
();
tf2_ros
::
Buffer
tfBuffer
;
tf2_ros
::
TransformListener
tfListener
(
tfBuffer
);
ros
::
Rate
rate
(
10
);
int
i
=
0
;
int
ho
=
0
;
while
(
node_handle
.
ok
())
{
{
geometry_msgs
::
TransformStamped
transformStamped
[
7
];
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
;
}
o7
.
jo
[
7
].
p1
[
2
]
=
transformStamped
[
7
].
transform
.
translation
.
z
;
// World Model Safety Test Cases to check Segmentation errors
ros
::
Rate
rate
(
10
);
// test the properties of the model that do not change over time, such as the structure of you model
// Static Tests
while
(
node_handle
.
ok
())
{
{
// update the model data like in the application model node
// test the dynamic parts here, i.e., if the model data is consistent with the position of the robot
//Checking if robot moved to position or not
if
(
o7
.
jo
[
7
].
p1
[
2
]
==
0.9
)
ROS_INFO_STREAM
(
" THE ROBOT HAS MOVED TO POSITION AS INTENDED "
);
else
ROS_ERROR_STREAM
(
"THE ROBOT HAS NOT MOVED TO THE POSITION AS INTENDED"
);
ros
::
shutdown
();
}
}
}
}
...
...
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