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
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
5d5d220d
Commit
5d5d220d
authored
2 years ago
by
Johannes Mey
Browse files
Options
Downloads
Patches
Plain Diff
new object locator with continuous updates for objects
parent
d967675f
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/object_locator.cpp
+58
-36
58 additions, 36 deletions
src/object_locator.cpp
with
58 additions
and
36 deletions
src/object_locator.cpp
+
58
−
36
View file @
5d5d220d
...
@@ -6,6 +6,8 @@
...
@@ -6,6 +6,8 @@
\date 15.06.22
\date 15.06.22
*/
*/
#include
<cmath>
#include
<ros/ros.h>
#include
<ros/ros.h>
#include
<ros/package.h>
#include
<ros/package.h>
#include
<apriltag_ros/AprilTagDetectionArray.h>
#include
<apriltag_ros/AprilTagDetectionArray.h>
...
@@ -17,7 +19,6 @@
...
@@ -17,7 +19,6 @@
#include
"ccf/util/NodeUtil.h"
#include
"ccf/util/NodeUtil.h"
#include
"Scene.pb.h"
#include
"Scene.pb.h"
#include
"google/protobuf/text_format.h"
#include
<google/protobuf/util/json_util.h>
#include
<google/protobuf/util/json_util.h>
#include
<google/protobuf/message.h>
#include
<google/protobuf/message.h>
...
@@ -31,6 +32,8 @@ const std::string CELL_BUNDLE = "CETI_TABLE_ONE";
...
@@ -31,6 +32,8 @@ const std::string CELL_BUNDLE = "CETI_TABLE_ONE";
const
double
TABLE_HEIGHT
=
0.89
;
const
double
TABLE_HEIGHT
=
0.89
;
const
double
GRID_SIZE
=
0.05
;
const
double
GRID_SIZE
=
0.05
;
const
double
MAX_ERROR
=
0.02
;
const
double
MAX_ERROR
=
0.02
;
const
bool
RISKY_MODE
=
true
;
const
bool
SNAP_TO_GRID
=
true
;
double
distanceToGrid
(
geometry_msgs
::
Point
point
,
double
grid_size
)
double
distanceToGrid
(
geometry_msgs
::
Point
point
,
double
grid_size
)
{
{
...
@@ -57,26 +60,23 @@ int main(int argc, char** argv)
...
@@ -57,26 +60,23 @@ int main(int argc, char** argv)
ros
::
NodeHandle
n
(
"ccf"
);
ros
::
NodeHandle
n
(
"ccf"
);
ros
::
Rate
loop_rate
(
200
);
ros
::
Rate
loop_rate
(
200
);
ros
::
Rate
pause_rate
(
ros
::
Duration
(
2
));
// seconds
pause_rate
.
sleep
();
ros
::
Duration
(
2
).
sleep
();
// sleep for two seconds
DummyRobotArmController
controller
(
n
,
"object_locator"
,
"nobot"
);
DummyRobotArmController
controller
(
n
,
"object_locator"
,
"nobot"
);
auto
mqttServer
=
getPrivateParameter
<
std
::
string
>
(
"mqtt_server"
,
"localhost:1883"
);
auto
mqttServer
=
getPrivateParameter
<
std
::
string
>
(
"mqtt_server"
,
"localhost:1883"
);
std
::
unique
_ptr
<
MqttConnection
>
mqtt_connection
=
std
::
shared
_ptr
<
MqttConnection
>
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"
));
auto
observed_scene_listener_topic
=
auto
other_cell_scene_listener_topic
=
getPrivateParameter
<
std
::
string
>
(
"other_cell_listener"
,
"/ceti_cell_1/scene/update"
);
getPrivateParameter
<
std
::
string
>
(
"other_cell_listener"
,
"/ceti_cell_1"
auto
observed_scene_delta_update_topic
=
"/scene/update"
);
auto
other_cell_delta_update_topic
=
getPrivateParameter
<
std
::
string
>
(
"other_cell_delta_updates"
,
"object_locator/scene/update"
);
getPrivateParameter
<
std
::
string
>
(
"other_cell_delta_updates"
,
"object_locator/scene/update"
);
mqtt_connection
->
listen
(
other_cell_scene_listener_topic
);
controller
.
addConnection
(
mqtt_connection
);
controller
.
addConnection
(
std
::
move
(
mqtt_connection
)
);
mqtt_connection
->
listen
(
observed_scene_listener_topic
);
// subscribe to a scene
// subscribe to a scene
controller
.
addCallback
(
o
ther_cell
_scene_listener_topic
,
[
&
controller
](
auto
msg
)
{
controller
.
addCallback
(
o
bserved
_scene_listener_topic
,
[
&
controller
](
auto
msg
)
{
ROS_INFO_STREAM
(
"Updating scene from other cell."
);
ROS_INFO_STREAM
(
"Updating scene from other cell."
);
Scene
scene
;
Scene
scene
;
scene
.
ParseFromString
(
msg
);
scene
.
ParseFromString
(
msg
);
...
@@ -90,7 +90,7 @@ int main(int argc, char** argv)
...
@@ -90,7 +90,7 @@ int main(int argc, char** argv)
"objects."
"objects."
"json"
));
"json"
));
ROS_
ERROR
_STREAM
(
"object library contains "
<<
objectLibrary
.
objects_size
()
<<
" objects"
);
ROS_
DEBUG
_STREAM
(
"object library contains "
<<
objectLibrary
.
objects_size
()
<<
" objects"
);
std
::
map
<
std
::
string
,
geometry_msgs
::
Pose
>
located_poses
;
std
::
map
<
std
::
string
,
geometry_msgs
::
Pose
>
located_poses
;
...
@@ -161,11 +161,13 @@ int main(int argc, char** argv)
...
@@ -161,11 +161,13 @@ int main(int argc, char** argv)
tf2
::
Quaternion
q
{
pose
.
orientation
.
x
,
pose
.
orientation
.
y
,
pose
.
orientation
.
z
,
pose
.
orientation
.
w
};
tf2
::
Quaternion
q
{
pose
.
orientation
.
x
,
pose
.
orientation
.
y
,
pose
.
orientation
.
z
,
pose
.
orientation
.
w
};
std
::
cout
.
precision
(
3
);
std
::
cout
.
precision
(
3
);
if
(
controller
.
resolveObject
(
name
))
auto
existingObject
=
controller
.
resolveObject
(
name
);
// we need this also later to check if pose has changed
if
(
!
RISKY_MODE
&&
existingObject
)
{
{
ROS_DEBUG_STREAM
(
"skipping pose for "
<<
name
<<
" the object is already in the scene."
);
ROS_DEBUG_STREAM
(
"skipping pose for "
<<
name
<<
" the object is already in the scene."
);
continue
;
continue
;
}
}
if
(
pose
.
orientation
.
w
<
0.99
)
if
(
pose
.
orientation
.
w
<
0.99
)
{
{
ROS_WARN_STREAM
(
"skipping pose for "
<<
name
<<
" because the object is rotated "
<<
pose
.
orientation
<<
" "
ROS_WARN_STREAM
(
"skipping pose for "
<<
name
<<
" because the object is rotated "
<<
pose
.
orientation
<<
" "
...
@@ -179,21 +181,48 @@ int main(int argc, char** argv)
...
@@ -179,21 +181,48 @@ int main(int argc, char** argv)
continue
;
continue
;
}
}
double
e
=
distanceToGrid
(
pose
.
position
,
GRID_SIZE
);
double
e
=
distanceToGrid
(
pose
.
position
,
GRID_SIZE
);
ROS_
ERROR
_STREAM
(
"object "
<<
name
<<
" has an error of "
<<
e
);
ROS_
DEBUG
_STREAM
(
"object "
<<
name
<<
" has an error of "
<<
e
);
// sanitize pose
// sanitize pose
relevant_poses
[
name
]
=
pose
;
if
(
SNAP_TO_GRID
)
relevant_poses
[
name
].
position
.
z
=
TABLE_HEIGHT
;
{
relevant_poses
[
name
].
position
=
closestGridPoint
(
relevant_poses
[
name
].
position
,
GRID_SIZE
);
relevant_poses
[
name
]
=
pose
;
relevant_poses
[
name
].
orientation
.
x
=
0
;
relevant_poses
[
name
].
position
.
z
=
TABLE_HEIGHT
;
relevant_poses
[
name
].
orientation
.
y
=
0
;
relevant_poses
[
name
].
position
=
closestGridPoint
(
relevant_poses
[
name
].
position
,
GRID_SIZE
);
relevant_poses
[
name
].
orientation
.
z
=
0
;
relevant_poses
[
name
].
orientation
.
x
=
0
;
relevant_poses
[
name
].
orientation
.
w
=
1
;
relevant_poses
[
name
].
orientation
.
y
=
0
;
relevant_poses
[
name
].
orientation
.
z
=
0
;
if
(
located_poses
[
name
]
==
relevant_poses
[
name
])
relevant_poses
[
name
].
orientation
.
w
=
1
;
if
(
RISKY_MODE
&&
existingObject
)
{
double
dx
=
existingObject
->
pos
().
x
()
-
relevant_poses
[
name
].
position
.
x
;
double
dy
=
existingObject
->
pos
().
y
()
-
relevant_poses
[
name
].
position
.
y
;
double
distance
=
std
::
sqrt
(
dx
*
dx
+
dy
*
dy
);
if
(
distance
<
GRID_SIZE
/
2
)
{
ROS_DEBUG_STREAM
(
"Skipping object "
<<
name
<<
" because its location has not changed."
);
continue
;
}
else
{
ROS_DEBUG_STREAM
(
"Sending object "
<<
name
<<
" because its location has changed by "
<<
distance
);
}
}
}
if
(
RISKY_MODE
||
located_poses
[
name
]
==
relevant_poses
[
name
])
{
{
ROS_DEBUG_STREAM
(
"found the same pose for "
+
name
+
" two times in a row: "
if
(
RISKY_MODE
)
<<
relevant_poses
[
name
].
position
.
x
<<
", "
<<
relevant_poses
[
name
].
position
.
y
);
{
ROS_DEBUG_STREAM
(
"found the new pose for "
+
name
+
": "
<<
relevant_poses
[
name
].
position
.
x
<<
", "
<<
relevant_poses
[
name
].
position
.
y
);
}
else
{
ROS_DEBUG_STREAM
(
"found the same pose for "
+
name
+
" two times in a row: "
<<
relevant_poses
[
name
].
position
.
x
<<
", "
<<
relevant_poses
[
name
].
position
.
y
);
}
auto
object
=
DummyRobotArmController
::
resolveObject
(
name
,
objectLibrary
);
auto
object
=
DummyRobotArmController
::
resolveObject
(
name
,
objectLibrary
);
if
(
!
object
)
if
(
!
object
)
{
{
...
@@ -205,29 +234,22 @@ int main(int argc, char** argv)
...
@@ -205,29 +234,22 @@ int main(int argc, char** argv)
newObject
->
mutable_pos
()
->
set_x
(
located_poses
[
name
].
position
.
x
);
newObject
->
mutable_pos
()
->
set_x
(
located_poses
[
name
].
position
.
x
);
newObject
->
mutable_pos
()
->
set_y
(
located_poses
[
name
].
position
.
y
);
newObject
->
mutable_pos
()
->
set_y
(
located_poses
[
name
].
position
.
y
);
newObject
->
mutable_pos
()
->
set_z
(
located_poses
[
name
].
position
.
z
+
newObject
->
size
().
height
()
/
2
);
newObject
->
mutable_pos
()
->
set_z
(
located_poses
[
name
].
position
.
z
+
newObject
->
size
().
height
()
/
2
);
newObject
->
set_mode
(
Object_Mode_MODE_ADD
);
newObject
->
set_mode
(
existingObject
?
Object_Mode_MODE_MODIFY
:
Object_Mode_MODE_ADD
);
newObject
->
set_state
(
Object_State_STATE_STATIONARY
);
newObject
->
set_state
(
Object_State_STATE_STATIONARY
);
}
}
}
}
if
(
updateScene
.
objects_size
()
>
0
)
if
(
updateScene
.
objects_size
()
>
0
)
{
{
ROS_
ERROR
_STREAM
(
"Sending scene with "
<<
updateScene
.
objects_size
()
<<
" newly recognized objects."
);
ROS_
DEBUG
_STREAM
(
"Sending scene with "
<<
updateScene
.
objects_size
()
<<
" newly recognized
or updated
objects."
);
controller
.
sendToAll
(
o
ther_cell
_delta_update_topic
,
updateScene
.
SerializeAsString
());
controller
.
sendToAll
(
o
bserved_scene
_delta_update_topic
,
updateScene
.
SerializeAsString
());
}
}
located_poses
=
relevant_poses
;
located_poses
=
relevant_poses
;
});
});
// receive a tag position
// if no "base tag" is contained, abort
// if the contained objects are already in the robot scene, abort (we only detect new objects)
// if the object tag is not in the base scene, abort
// find the drop-of location in the scene that is closest to the
while
(
ros
::
ok
())
while
(
ros
::
ok
())
{
{
ros
::
spinOnce
();
ros
::
spinOnce
();
loop_rate
.
sleep
();
loop_rate
.
sleep
();
}
}
...
...
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