Skip to content
GitLab
Explore
Sign in
Register
Primary navigation
Search or go to…
Project
C
CCF Immersive Sorting Demo
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
Package registry
Container registry
Model registry
Operate
Environments
Terraform modules
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
CeTI
ROS
CeTI Cobotic Framework
CCF Immersive Sorting Demo
Commits
e00f6ce0
Commit
e00f6ce0
authored
3 years ago
by
Johannes Mey
Browse files
Options
Downloads
Patches
Plain Diff
improved logging
parent
d87e4eeb
Branches
Branches containing commit
No related tags found
No related merge requests found
Changes
2
Show whitespace changes
Inline
Side-by-side
Showing
2 changed files
src/dummy_sorting_controller.cpp
+36
-35
36 additions, 35 deletions
src/dummy_sorting_controller.cpp
src/moveit_sorting_controller.cpp
+85
-65
85 additions, 65 deletions
src/moveit_sorting_controller.cpp
with
121 additions
and
100 deletions
src/dummy_sorting_controller.cpp
+
36
−
35
View file @
e00f6ce0
...
@@ -56,20 +56,8 @@ int main(int argc, char** argv)
...
@@ -56,20 +56,8 @@ int main(int argc, char** argv)
ros
::
WallDuration
(
5
).
sleep
();
// wait 5 secs to init scene (to give moveit time to load)
ros
::
WallDuration
(
5
).
sleep
();
// wait 5 secs to init scene (to give moveit time to load)
controller
.
loadScene
(
getPrivateParameter
<
std
::
string
>
(
"scene"
,
ros
::
package
::
getPath
(
"ccf_immersive_sorting"
)
+
"/con"
controller
.
loadScene
(
getPrivateParameter
<
std
::
string
>
(
"scene"
,
ros
::
package
::
getPath
(
"ccf_immersive_sorting"
)
+
"fig/"
"/config/config_scene_ceti-table-placeworld.json"
));
"conf"
"ig_"
"scen"
"e_"
"ceti"
"-tab"
"le-"
"plac"
"ewor"
"ld."
"jso"
"n"
));
std
::
optional
<
std
::
string
>
currentlyPickedBox
{};
std
::
optional
<
std
::
string
>
currentlyPickedBox
{};
...
@@ -83,32 +71,31 @@ int main(int argc, char** argv)
...
@@ -83,32 +71,31 @@ int main(int argc, char** argv)
const
PickAndPlace
&
pick_place
=
command
.
pickandplace
();
const
PickAndPlace
&
pick_place
=
command
.
pickandplace
();
if
(
command
.
has_pickandplace
())
if
(
command
.
has_pickandplace
())
{
{
ROS_INFO_STREAM
(
"Got Command to pick object "
<<
pick_place
.
idpick
()
<<
" and place it into "
ROS_INFO_STREAM
(
"[COMMAND] Pick object "
<<
pick_place
.
idpick
()
<<
" and place it at "
<<
pick_place
.
idplace
());
<<
pick_place
.
idplace
());
Object
*
pick_object
=
controller
.
resolveObject
(
pick_place
.
idpick
());
Object
*
pick_object
=
controller
.
resolveObject
(
pick_place
.
idpick
());
if
(
!
pick_object
)
if
(
!
pick_object
)
{
{
ROS_ERROR_STREAM
(
"Selected unknown pick object '"
<<
pick_place
.
idpick
()
<<
"'"
);
ROS_ERROR_STREAM
(
"
[COMMAND] FAILED!
Selected unknown pick object '"
<<
pick_place
.
idpick
()
<<
"'"
);
return
;
return
;
}
}
Object
*
place_object
=
controller
.
resolveObject
(
pick_place
.
idplace
());
Object
*
place_object
=
controller
.
resolveObject
(
pick_place
.
idplace
());
if
(
!
place_object
)
if
(
!
place_object
)
{
{
ROS_ERROR_STREAM
(
"Selected unknown place object '"
<<
pick_place
.
idplace
()
<<
"'"
);
ROS_ERROR_STREAM
(
"
[COMMAND] FAILED!
Selected unknown place object '"
<<
pick_place
.
idplace
()
<<
"'"
);
return
;
return
;
}
}
if
(
pick_object
->
type
()
!=
Object
::
BOX
)
if
(
pick_object
->
type
()
!=
Object
::
BOX
)
{
{
ROS_WARN_STREAM
(
"Selected unsupported pick object of type "
<<
pick_object
->
type
());
ROS_WARN_STREAM
(
"
[COMMAND] FAILED!
Selected unsupported pick object of type "
<<
pick_object
->
type
());
}
}
if
(
place_object
->
type
()
!=
Object
::
BIN
&&
place_object
->
type
()
!=
Object
::
DROP_OFF_LOCATION
&&
if
(
place_object
->
type
()
!=
Object
::
BIN
&&
place_object
->
type
()
!=
Object
::
DROP_OFF_LOCATION
&&
place_object
->
type
()
!=
Object
::
COLLABORATION_ZONE
)
place_object
->
type
()
!=
Object
::
COLLABORATION_ZONE
)
{
{
ROS_WARN_STREAM
(
"Selected unsupported place object of type "
<<
place_object
->
type
());
ROS_WARN_STREAM
(
"
[COMMAND] FAILED!
Selected unsupported place object of type "
<<
place_object
->
type
());
}
}
if
(
command
.
pickandplace
().
idrobot
()
!=
controller
.
getRobotName
())
if
(
command
.
pickandplace
().
idrobot
()
!=
controller
.
getRobotName
())
{
{
ROS_
WARN
_STREAM
(
"Ignoring command pick-place command for robot "
ROS_
DEBUG
_STREAM
(
"
[COMMAND] IGNORED!
Ignoring command pick-place command for robot "
<<
command
.
pickandplace
().
idrobot
()
<<
" (this is "
<<
controller
.
getRobotName
()
<<
")"
);
<<
command
.
pickandplace
().
idrobot
()
<<
" (this is "
<<
controller
.
getRobotName
()
<<
")"
);
return
;
return
;
}
}
...
@@ -116,18 +103,19 @@ int main(int argc, char** argv)
...
@@ -116,18 +103,19 @@ int main(int argc, char** argv)
Object
*
robot
=
controller
.
resolveObject
(
command
.
pickandplace
().
idrobot
());
Object
*
robot
=
controller
.
resolveObject
(
command
.
pickandplace
().
idrobot
());
if
(
!
robot
)
if
(
!
robot
)
{
{
ROS_ERROR_STREAM
(
"Inconsistent scene, robot '"
<<
command
.
evacuate
().
idrobot
()
<<
"' not found!"
);
ROS_ERROR_STREAM
(
"[COMMAND] FAILED! Inconsistent scene, robot '"
<<
command
.
evacuate
().
idrobot
()
<<
"' not found!"
);
return
;
return
;
}
}
if
(
place_object
->
type
()
==
Object
::
BIN
)
if
(
place_object
->
type
()
==
Object
::
BIN
)
{
{
if
(
!
controller
.
pickAndDrop
(
*
robot
,
*
pick_object
,
*
place_object
,
false
))
if
(
!
controller
.
pickAndDrop
(
*
robot
,
*
pick_object
,
*
place_object
,
false
))
{
{
ROS_WARN_STREAM
(
"Unable to remove box '"
<<
pick_place
.
idpick
()
<<
"'!"
);
ROS_WARN_STREAM
(
"
[COMMAND] FAILED!
Unable to remove box '"
<<
pick_place
.
idpick
()
<<
"'!"
);
}
}
else
else
{
{
ROS_INFO_STREAM
(
"
Job is done
! '"
<<
pick_place
.
idpick
()
<<
"' is no more."
);
ROS_INFO_STREAM
(
"
[COMMAND] SUCCESS
! '"
<<
pick_place
.
idpick
()
<<
"' is no more."
);
controller
.
sendScene
();
controller
.
sendScene
();
}
}
}
}
...
@@ -135,28 +123,31 @@ int main(int argc, char** argv)
...
@@ -135,28 +123,31 @@ int main(int argc, char** argv)
{
{
if
(
!
controller
.
pickAndPlace
(
*
robot
,
*
pick_object
,
*
place_object
,
false
))
if
(
!
controller
.
pickAndPlace
(
*
robot
,
*
pick_object
,
*
place_object
,
false
))
{
{
ROS_WARN_STREAM
(
"Unable to move box '"
<<
pick_place
.
idpick
()
<<
"'!"
);
ROS_WARN_STREAM
(
"
[COMMAND] FAILED!
Unable to move box '"
<<
pick_place
.
idpick
()
<<
"'!"
);
}
}
else
else
{
{
ROS_INFO_STREAM
(
"
Job is done
!
'
"
<<
pick_place
.
idpick
()
<<
"' is at its new location."
);
ROS_INFO_STREAM
(
"
[COMMAND] SUCCESS
! "
<<
pick_place
.
idpick
()
<<
"' is at its new location."
);
controller
.
sendScene
();
controller
.
sendScene
();
}
}
}
}
}
}
else
if
(
command
.
has_evacuate
())
else
if
(
command
.
has_evacuate
())
{
{
ROS_INFO_STREAM
(
"[COMMAND] Evacuate "
<<
command
.
evacuate
().
idrobot
()
<<
" from zone "
<<
command
.
evacuate
().
idcollaborationzone
());
if
(
command
.
evacuate
().
idrobot
()
!=
controller
.
getRobotName
())
if
(
command
.
evacuate
().
idrobot
()
!=
controller
.
getRobotName
())
{
{
ROS_
WARN
_STREAM
(
"
Ignoring command pick-place command for robot "
<<
command
.
evacuate
().
idrobot
()
<<
" (this is
"
ROS_
DEBUG
_STREAM
(
"
[COMMAND] IGNORED! Ignoring evacuate command for robot
"
<<
controller
.
getRobotName
()
<<
")"
);
<<
command
.
evacuate
().
idrobot
()
<<
" (this is "
<<
controller
.
getRobotName
()
<<
")"
);
return
;
return
;
}
}
Object
*
robot
=
controller
.
resolveObject
(
command
.
evacuate
().
idrobot
());
Object
*
robot
=
controller
.
resolveObject
(
command
.
evacuate
().
idrobot
());
if
(
!
robot
)
if
(
!
robot
)
{
{
ROS_ERROR_STREAM
(
"Inconsistent scene, robot '"
<<
command
.
evacuate
().
idrobot
()
<<
"' not found!"
);
ROS_ERROR_STREAM
(
"[COMMAND] FAILED! Inconsistent scene, robot '"
<<
command
.
evacuate
().
idrobot
()
<<
"' not found!"
);
return
;
return
;
}
}
...
@@ -165,28 +156,38 @@ int main(int argc, char** argv)
...
@@ -165,28 +156,38 @@ int main(int argc, char** argv)
pose
.
position
.
x
=
robot
->
pos
().
x
()
+
.4
;
pose
.
position
.
x
=
robot
->
pos
().
x
()
+
.4
;
pose
.
position
.
y
=
robot
->
pos
().
y
();
pose
.
position
.
y
=
robot
->
pos
().
y
();
pose
.
position
.
z
=
robot
->
pos
().
z
()
+
.4
;
pose
.
position
.
z
=
robot
->
pos
().
z
()
+
.4
;
controller
.
moveToPose
(
*
robot
,
pose
,
false
);
if
(
controller
.
moveToPose
(
*
robot
,
pose
,
false
))
{
ROS_INFO_STREAM
(
"[COMMAND] SUCCESS! Evacuation complete!"
);
}
else
{
ROS_ERROR_STREAM
(
"[COMMAND] FAILED! Evacuation did not complete."
);
}
}
}
else
if
(
command
.
has_configchange
())
else
if
(
command
.
has_configchange
())
{
{
Object
*
zone
=
controller
.
resolveObject
(
command
.
configchange
().
idcollaborationzone
());
Object
*
zone
=
controller
.
resolveObject
(
command
.
configchange
().
idcollaborationzone
());
ROS_INFO_STREAM
(
"[COMMAND] Change config of zone "
<<
zone
<<
" to owner "
<<
command
.
configchange
().
idrobotnewowner
());
if
(
!
zone
)
if
(
!
zone
)
{
{
ROS_ERROR_STREAM
(
"Unable to configure unknown collaboration zone '"
<<
command
.
evacuate
().
idrobot
()
<<
"'."
);
ROS_ERROR_STREAM
(
"[COMMAND] FAILED! Unable to configure unknown collaboration zone '"
<<
command
.
evacuate
().
idrobot
()
<<
"'."
);
return
;
return
;
}
}
Object
*
robot
=
controller
.
resolveObject
(
command
.
configchange
().
idrobotnewowner
());
Object
*
robot
=
controller
.
resolveObject
(
command
.
configchange
().
idrobotnewowner
());
if
(
!
robot
)
if
(
!
robot
)
{
{
ROS_WARN_STREAM
(
"Collaboration zone active for unknown robot '"
<<
command
.
evacuate
().
idrobot
()
ROS_WARN_STREAM
(
"
[COMMAND] INFO:
Collaboration zone active for unknown robot '"
<<
command
.
evacuate
().
idrobot
()
<<
"', so blocked in all."
);
<<
"', so blocked in all."
);
}
}
controller
.
configureCollaborationZone
(
*
zone
,
command
.
configchange
().
idrobotnewowner
());
controller
.
configureCollaborationZone
(
*
zone
,
command
.
configchange
().
idrobotnewowner
());
}
}
else
else
{
{
ROS_WARN_STREAM
(
"
Ignoring command that is not PickAndPlace but
"
<<
command
.
msg_case
());
ROS_WARN_STREAM
(
"
[COMMAND] IGNORED! Ignoring command of unknown type
"
<<
command
.
msg_case
());
}
}
};
};
controller
.
reactToCommandMessage
(
pick_place_callback
);
controller
.
reactToCommandMessage
(
pick_place_callback
);
...
...
This diff is collapsed.
Click to expand it.
src/moveit_sorting_controller.cpp
+
85
−
65
View file @
e00f6ce0
...
@@ -25,7 +25,6 @@ using CetiRosToolbox::getPrivateParameter;
...
@@ -25,7 +25,6 @@ using CetiRosToolbox::getPrivateParameter;
int
main
(
int
argc
,
char
**
argv
)
int
main
(
int
argc
,
char
**
argv
)
{
{
GOOGLE_PROTOBUF_VERIFY_VERSION
;
GOOGLE_PROTOBUF_VERIFY_VERSION
;
ros
::
init
(
argc
,
argv
,
NODE_NAME
);
ros
::
init
(
argc
,
argv
,
NODE_NAME
);
...
@@ -39,8 +38,8 @@ int main(int argc, char **argv)
...
@@ -39,8 +38,8 @@ int main(int argc, char **argv)
MoveItRobotArmController
controller
{
n
,
NODE_NAME
,
robotName
};
MoveItRobotArmController
controller
{
n
,
NODE_NAME
,
robotName
};
auto
mqttServer
=
getPrivateParameter
<
std
::
string
>
(
"mqtt_server"
,
"localhost:1883"
);
auto
mqttServer
=
getPrivateParameter
<
std
::
string
>
(
"mqtt_server"
,
"localhost:1883"
);
std
::
unique_ptr
<
MqttConnection
>
std
::
unique_ptr
<
MqttConnection
>
mqtt_connection
=
mqtt_connection
=
std
::
make_unique
<
MqttConnection
>
(
mqttServer
,
ros
::
this_node
::
getName
());
std
::
make_unique
<
MqttConnection
>
(
mqttServer
,
ros
::
this_node
::
getName
());
mqtt_connection
->
listen
(
getParameter
<
std
::
string
>
(
n
,
"topics/selection"
,
"selection"
));
mqtt_connection
->
listen
(
getParameter
<
std
::
string
>
(
n
,
"topics/selection"
,
"selection"
));
auto
commandTopic
=
getParameter
<
std
::
string
>
(
n
,
"topics/command"
,
"/ceti_cell_placeworld/command"
);
auto
commandTopic
=
getParameter
<
std
::
string
>
(
n
,
"topics/command"
,
"/ceti_cell_placeworld/command"
);
mqtt_connection
->
listen
(
commandTopic
);
mqtt_connection
->
listen
(
commandTopic
);
...
@@ -62,84 +61,93 @@ int main(int argc, char **argv)
...
@@ -62,84 +61,93 @@ int main(int argc, char **argv)
std
::
optional
<
std
::
string
>
currentlyPickedBox
{};
std
::
optional
<
std
::
string
>
currentlyPickedBox
{};
ros
::
Subscriber
sub
=
n
.
subscribe
<
std_msgs
::
Empty
>
(
"send_scene"
,
1000
,
[
&
controller
](
ros
::
Subscriber
sub
=
const
std_msgs
::
EmptyConstPtr
&
msg
)
n
.
subscribe
<
std_msgs
::
Empty
>
(
"send_scene"
,
1000
,
[
&
controller
](
const
std_msgs
::
EmptyConstPtr
&
msg
)
{
{
ROS_INFO_STREAM
(
"Sending scene manually (triggered by a message to the 'send_scene' topic)"
);
ROS_INFO_STREAM
(
"Sending scene manually (triggered by a message to the 'send_scene' topic)"
);
controller
.
sendScene
();
controller
.
sendScene
();
});
});
auto
pick_place_callback
=
[
&
controller
](
const
Command
&
command
)
auto
pick_place_callback
=
[
&
controller
](
const
Command
&
command
)
{
{
const
PickAndPlace
&
pick_place
=
command
.
pickandplace
();
const
PickAndPlace
&
pick_place
=
command
.
pickandplace
();
if
(
command
.
has_pickandplace
())
{
if
(
command
.
has_pickandplace
())
ROS_INFO_STREAM
(
"Got Command to pick object "
<<
pick_place
.
idpick
()
<<
" and place it into "
<<
pick_place
.
idplace
());
{
ROS_INFO_STREAM
(
"[COMMAND] Pick object "
<<
pick_place
.
idpick
()
<<
" and place it at "
<<
pick_place
.
idplace
());
Object
*
pick_object
=
controller
.
resolveObject
(
pick_place
.
idpick
());
Object
*
pick_object
=
controller
.
resolveObject
(
pick_place
.
idpick
());
if
(
!
pick_object
)
if
(
!
pick_object
)
{
{
ROS_ERROR_STREAM
(
ROS_ERROR_STREAM
(
"[COMMAND] FAILED! Selected unknown pick object '"
<<
pick_place
.
idpick
()
<<
"'"
);
"Selected unknown pick object '"
<<
pick_place
.
idpick
()
<<
"'"
);
return
;
return
;
}
}
Object
*
place_object
=
controller
.
resolveObject
(
pick_place
.
idplace
());
Object
*
place_object
=
controller
.
resolveObject
(
pick_place
.
idplace
());
if
(
!
place_object
)
if
(
!
place_object
)
{
{
ROS_ERROR_STREAM
(
ROS_ERROR_STREAM
(
"[COMMAND] FAILED! Selected unknown place object '"
<<
pick_place
.
idplace
()
<<
"'"
);
"Selected unknown place object '"
<<
pick_place
.
idplace
()
<<
"'"
);
return
;
return
;
}
}
if
(
pick_object
->
type
()
!=
Object
::
BOX
)
if
(
pick_object
->
type
()
!=
Object
::
BOX
)
{
{
ROS_WARN_STREAM
(
ROS_WARN_STREAM
(
"[COMMAND] FAILED! Selected unsupported pick object of type "
<<
pick_object
->
type
());
"Selected unsupported pick object of type "
<<
pick_object
->
type
());
}
}
if
(
place_object
->
type
()
!=
Object
::
BIN
&&
place_object
->
type
()
!=
Object
::
DROP_OFF_LOCATION
&&
place_object
->
type
()
!=
Object
::
COLLABORATION_ZONE
)
if
(
place_object
->
type
()
!=
Object
::
BIN
&&
place_object
->
type
()
!=
Object
::
DROP_OFF_LOCATION
&&
place_object
->
type
()
!=
Object
::
COLLABORATION_ZONE
)
{
{
ROS_WARN_STREAM
(
ROS_WARN_STREAM
(
"[COMMAND] FAILED! Selected unsupported place object of type "
<<
place_object
->
type
());
"Selected unsupported place object of type "
<<
place_object
->
type
());
}
}
if
(
command
.
pickandplace
().
idrobot
()
!=
controller
.
getRobotName
())
{
if
(
command
.
pickandplace
().
idrobot
()
!=
controller
.
getRobotName
())
ROS_WARN_STREAM
(
{
"Ignoring command pick-place command for robot "
<<
command
.
pickandplace
().
idrobot
()
<<
" (this is "
<<
controller
.
getRobotName
()
<<
")"
);
ROS_DEBUG_STREAM
(
"[COMMAND] IGNORED! Ignoring command pick-place command for robot "
<<
command
.
pickandplace
().
idrobot
()
<<
" (this is "
<<
controller
.
getRobotName
()
<<
")"
);
return
;
return
;
}
}
Object
*
robot
=
controller
.
resolveObject
(
command
.
pickandplace
().
idrobot
());
Object
*
robot
=
controller
.
resolveObject
(
command
.
pickandplace
().
idrobot
());
if
(
!
robot
)
if
(
!
robot
)
{
{
ROS_ERROR_STREAM
(
ROS_ERROR_STREAM
(
"[COMMAND] FAILED! Inconsistent scene, robot '"
<<
command
.
evacuate
().
idrobot
()
"Inconsistent scene, robot '"
<<
command
.
evacuate
().
idrobot
()
<<
"' not found!"
);
<<
"' not found!"
);
return
;
return
;
}
}
if
(
place_object
->
type
()
==
Object
::
BIN
)
{
if
(
place_object
->
type
()
==
Object
::
BIN
)
if
(
!
controller
.
pickAndDrop
(
*
robot
,
*
pick_object
,
*
place_object
,
false
))
{
{
ROS_WARN_STREAM
(
"Unable to remove box '"
<<
pick_place
.
idpick
()
<<
"'!"
);
if
(
!
controller
.
pickAndDrop
(
*
robot
,
*
pick_object
,
*
place_object
,
false
))
}
else
{
{
ROS_INFO_STREAM
(
"Job is done! '"
<<
pick_place
.
idpick
()
<<
"' is no more."
);
ROS_WARN_STREAM
(
"[COMMAND] FAILED! Unable to remove box '"
<<
pick_place
.
idpick
()
<<
"'!"
);
}
else
{
ROS_INFO_STREAM
(
"[COMMAND] SUCCESS! '"
<<
pick_place
.
idpick
()
<<
"' is no more."
);
controller
.
sendScene
();
controller
.
sendScene
();
}
}
}
else
if
(
place_object
->
type
()
==
Object
::
DROP_OFF_LOCATION
||
place_object
->
type
()
==
Object
::
COLLABORATION_ZONE
)
{
}
if
(
!
controller
.
pickAndPlace
(
*
robot
,
*
pick_object
,
*
place_object
,
false
))
{
else
if
(
place_object
->
type
()
==
Object
::
DROP_OFF_LOCATION
||
place_object
->
type
()
==
Object
::
COLLABORATION_ZONE
)
ROS_WARN_STREAM
(
"Unable to move box '"
<<
pick_place
.
idpick
()
<<
"'!"
);
{
}
else
{
if
(
!
controller
.
pickAndPlace
(
*
robot
,
*
pick_object
,
*
place_object
,
false
))
ROS_INFO_STREAM
(
"Job is done! '"
<<
pick_place
.
idpick
()
<<
"' is at its new location."
);
{
ROS_WARN_STREAM
(
"[COMMAND] FAILED! Unable to move box '"
<<
pick_place
.
idpick
()
<<
"'!"
);
}
else
{
ROS_INFO_STREAM
(
"[COMMAND] SUCCESS! "
<<
pick_place
.
idpick
()
<<
"' is at its new location."
);
controller
.
sendScene
();
controller
.
sendScene
();
}
}
}
}
}
else
if
(
command
.
has_evacuate
())
{
}
else
if
(
command
.
has_evacuate
())
{
if
(
command
.
evacuate
().
idrobot
()
!=
controller
.
getRobotName
())
{
ROS_INFO_STREAM
(
"[COMMAND] Evacuate "
<<
command
.
evacuate
().
idrobot
()
<<
" from zone "
ROS_WARN_STREAM
(
<<
command
.
evacuate
().
idcollaborationzone
());
"Ignoring command pick-place command for robot "
<<
command
.
evacuate
().
idrobot
()
<<
" (this is "
<<
controller
.
getRobotName
()
<<
")"
);
if
(
command
.
evacuate
().
idrobot
()
!=
controller
.
getRobotName
())
{
ROS_DEBUG_STREAM
(
"[COMMAND] IGNORED! Ignoring evacuate command for robot "
<<
command
.
evacuate
().
idrobot
()
<<
" (this is "
<<
controller
.
getRobotName
()
<<
")"
);
return
;
return
;
}
}
Object
*
robot
=
controller
.
resolveObject
(
command
.
evacuate
().
idrobot
());
Object
*
robot
=
controller
.
resolveObject
(
command
.
evacuate
().
idrobot
());
if
(
!
robot
)
if
(
!
robot
)
{
{
ROS_ERROR_STREAM
(
ROS_ERROR_STREAM
(
"[COMMAND] FAILED! Inconsistent scene, robot '"
<<
command
.
evacuate
().
idrobot
()
"Inconsistent scene, robot '"
<<
command
.
evacuate
().
idrobot
()
<<
"' not found!"
);
<<
"' not found!"
);
return
;
return
;
}
}
...
@@ -148,31 +156,43 @@ int main(int argc, char **argv)
...
@@ -148,31 +156,43 @@ int main(int argc, char **argv)
pose
.
position
.
x
=
robot
->
pos
().
x
()
+
.4
;
pose
.
position
.
x
=
robot
->
pos
().
x
()
+
.4
;
pose
.
position
.
y
=
robot
->
pos
().
y
();
pose
.
position
.
y
=
robot
->
pos
().
y
();
pose
.
position
.
z
=
robot
->
pos
().
z
()
+
.4
;
pose
.
position
.
z
=
robot
->
pos
().
z
()
+
.4
;
controller
.
moveToPose
(
*
robot
,
pose
,
false
);
if
(
controller
.
moveToPose
(
*
robot
,
pose
,
false
))
{
}
else
if
(
command
.
has_configchange
())
{
ROS_INFO_STREAM
(
"[COMMAND] SUCCESS! Evacuation complete!"
);
}
else
{
ROS_ERROR_STREAM
(
"[COMMAND] FAILED! Evacuation did not complete."
);
}
}
else
if
(
command
.
has_configchange
())
{
Object
*
zone
=
controller
.
resolveObject
(
command
.
configchange
().
idcollaborationzone
());
Object
*
zone
=
controller
.
resolveObject
(
command
.
configchange
().
idcollaborationzone
());
ROS_INFO_STREAM
(
"[COMMAND] Change config of zone "
<<
zone
->
id
()
<<
" to owner "
<<
command
.
configchange
().
idrobotnewowner
());
if
(
!
zone
)
if
(
!
zone
)
{
{
ROS_ERROR_STREAM
(
"Unable to configure unknown collaboration zone '"
<<
command
.
evacuate
().
idrobot
()
<<
"'."
);
ROS_ERROR_STREAM
(
"[COMMAND] FAILED! Unable to configure unknown collaboration zone '"
<<
command
.
evacuate
().
idrobot
()
<<
"'."
);
return
;
return
;
}
}
Object
*
robot
=
controller
.
resolveObject
(
command
.
configchange
().
idrobotnewowner
());
Object
*
robot
=
controller
.
resolveObject
(
command
.
configchange
().
idrobotnewowner
());
if
(
!
robot
)
if
(
!
robot
)
{
{
ROS_WARN_STREAM
(
ROS_WARN_STREAM
(
"[COMMAND] INFO: Collaboration zone active for unknown robot '"
<<
command
.
evacuate
().
idrobot
()
"Collaboration zone active for unknown robot '"
<<
command
.
evacuate
().
idrobot
()
<<
"', so blocked in all."
);
<<
"', so blocked in all."
);
}
}
controller
.
configureCollaborationZone
(
*
zone
,
command
.
configchange
().
idrobotnewowner
());
controller
.
configureCollaborationZone
(
*
zone
,
command
.
configchange
().
idrobotnewowner
());
}
else
{
}
ROS_WARN_STREAM
(
"Ignoring command that is not PickAndPlace but "
<<
command
.
msg_case
());
else
{
ROS_WARN_STREAM
(
"[COMMAND] IGNORED! Ignoring command of unknown type "
<<
command
.
msg_case
());
}
}
};
};
controller
.
reactToCommandMessage
(
pick_place_callback
);
controller
.
reactToCommandMessage
(
pick_place_callback
);
auto
sceneUpdateCallback
=
[
&
currentlyPickedBox
,
&
controller
]()
auto
sceneUpdateCallback
=
[
&
currentlyPickedBox
,
&
controller
]()
{
{
if
(
currentlyPickedBox
.
has_value
())
if
(
currentlyPickedBox
.
has_value
())
{
{
auto
resolved
=
controller
.
resolveObject
(
currentlyPickedBox
.
value
());
auto
resolved
=
controller
.
resolveObject
(
currentlyPickedBox
.
value
());
...
...
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