Skip to content
GitLab
Explore
Sign in
Register
Primary navigation
Search or go to…
Project
M
multi_cell_builder
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
Matteo Anedda
multi_cell_builder
Commits
88948f3f
Commit
88948f3f
authored
1 year ago
by
KingMaZito
Browse files
Options
Downloads
Patches
Plain Diff
callBack implementation for pickup/drop/place tested
parent
b8075244
No related branches found
No related tags found
No related merge requests found
Changes
3
Show whitespace changes
Inline
Side-by-side
Showing
3 changed files
include/mediator/mg_mediator.h
+4
-0
4 additions, 0 deletions
include/mediator/mg_mediator.h
results/dummy/-2045918175/-2045918175.yaml
+2
-2
2 additions, 2 deletions
results/dummy/-2045918175/-2045918175.yaml
src/mediator/mg_mediator.cpp
+311
-71
311 additions, 71 deletions
src/mediator/mg_mediator.cpp
with
317 additions
and
73 deletions
include/mediator/mg_mediator.h
+
4
−
0
View file @
88948f3f
...
@@ -77,8 +77,12 @@ class MGMediator: public AbstractMediator, virtual Controller{
...
@@ -77,8 +77,12 @@ class MGMediator: public AbstractMediator, virtual Controller{
void
publishTables
();
void
publishTables
();
moveit
::
task_constructor
::
Task
pickUp
(
const
std
::
string
&
obj
,
const
std
::
string
&
robotId
);
moveit
::
task_constructor
::
Task
pickUp
(
const
std
::
string
&
obj
,
const
std
::
string
&
robotId
);
moveit
::
task_constructor
::
Task
place
(
const
std
::
string
&
obj
,
const
std
::
string
&
robotId
,
const
std
::
string
&
target
);
moveit
::
task_constructor
::
Task
place
(
const
std
::
string
&
obj
,
const
std
::
string
&
robotId
,
const
std
::
string
&
target
);
moveit
::
task_constructor
::
Task
drop
(
const
std
::
string
&
obj
,
const
std
::
string
&
robotId
,
const
std
::
string
&
drop
);
void
processSharedStructure
();
void
processSharedStructure
();
void
setupMqtt
();
};
};
#endif
#endif
\ No newline at end of file
This diff is collapsed.
Click to expand it.
results/dummy/-2045918175/-2045918175.yaml
+
2
−
2
View file @
88948f3f
...
@@ -21,9 +21,9 @@
...
@@ -21,9 +21,9 @@
{
'
id'
:
'
table1_left_panel'
,
'
pos'
:
{
'
x'
:
-0.000000
,
'
y'
:
-0.652502
,
'
z'
:
0.885000
}
,
'
size'
:
{
'
length'
:
0.700000
,
'
width'
:
0.500000
,
'
height'
:
0.010000
}
,
'
orientation'
:
{
'
x'
:
0.000000
,
'
y'
:
0.000000
,
'
z'
:
0.000001
,
'
w'
:
1.000000
}
,
'
color'
:
{
'
r'
:
0.15
,
'
g'
:
0.15
,
'
b'
:
0.15
}
},
{
'
id'
:
'
table1_left_panel'
,
'
pos'
:
{
'
x'
:
-0.000000
,
'
y'
:
-0.652502
,
'
z'
:
0.885000
}
,
'
size'
:
{
'
length'
:
0.700000
,
'
width'
:
0.500000
,
'
height'
:
0.010000
}
,
'
orientation'
:
{
'
x'
:
0.000000
,
'
y'
:
0.000000
,
'
z'
:
0.000001
,
'
w'
:
1.000000
}
,
'
color'
:
{
'
r'
:
0.15
,
'
g'
:
0.15
,
'
b'
:
0.15
}
},
{
'
id'
:
'
table2_right_panel'
,
'
pos'
:
{
'
x'
:
-0.000007
,
'
y'
:
1.957498
,
'
z'
:
0.885000
}
,
'
size'
:
{
'
length'
:
0.700000
,
'
width'
:
0.500000
,
'
height'
:
0.010000
}
,
'
orientation'
:
{
'
x'
:
0.000000
,
'
y'
:
0.000000
,
'
z'
:
0.000001
,
'
w'
:
1.000000
}
,
'
color'
:
{
'
r'
:
0.15
,
'
g'
:
0.15
,
'
b'
:
0.15
}
},
{
'
id'
:
'
table2_right_panel'
,
'
pos'
:
{
'
x'
:
-0.000007
,
'
y'
:
1.957498
,
'
z'
:
0.885000
}
,
'
size'
:
{
'
length'
:
0.700000
,
'
width'
:
0.500000
,
'
height'
:
0.010000
}
,
'
orientation'
:
{
'
x'
:
0.000000
,
'
y'
:
0.000000
,
'
z'
:
0.000001
,
'
w'
:
1.000000
}
,
'
color'
:
{
'
r'
:
0.15
,
'
g'
:
0.15
,
'
b'
:
0.15
}
},
{
'
id'
:
'
A'
,
'
type'
:
'
BOX'
,
'
pos'
:
{
'
x'
:
-0.3
,
'
y'
:
-0.7
,
'
z'
:
0.9355
},
'
size'
:
{
'
length'
:
0.0318
,
'
width'
:
0.0636
,
'
height'
:
0.091
},
'
orientation'
:
{
'
x'
:
0
,
'
y'
:
0
,
'
z'
:
0
,
'
w'
:
1
},
'
color'
:
{
'
b'
:
1
}
},
{
'
id'
:
'
A'
,
'
type'
:
'
BOX'
,
'
pos'
:
{
'
x'
:
-0.3
,
'
y'
:
-0.7
,
'
z'
:
0.9355
},
'
size'
:
{
'
length'
:
0.0318
,
'
width'
:
0.0636
,
'
height'
:
0.091
},
'
orientation'
:
{
'
x'
:
0
,
'
y'
:
0
,
'
z'
:
0
,
'
w'
:
1
},
'
color'
:
{
'
b'
:
1
}
},
{
'
id'
:
'
ABIN'
,
'
type'
:
'
OBSTACLE
'
,
'
pos'
:
{
'
x'
:
-0.
25
,
'
y'
:
-0.65
,
'
z'
:
0.9355
},
'
size'
:
{
'
length'
:
0.0330
,
'
width'
:
0.066
,
'
height'
:
0.091
},
'
orientation'
:
{
'
x'
:
0
,
'
y'
:
0
,
'
z'
:
0
,
'
w'
:
1
},
'
color'
:
{
'
b'
:
1
}
},
{
'
id'
:
'
ABIN'
,
'
type'
:
'
BIN
'
,
'
pos'
:
{
'
x'
:
-0.
1
,
'
y'
:
-0.65
,
'
z'
:
0.9355
},
'
size'
:
{
'
length'
:
0.0330
,
'
width'
:
0.066
,
'
height'
:
0.091
},
'
orientation'
:
{
'
x'
:
0
,
'
y'
:
0
,
'
z'
:
0
,
'
w'
:
1
},
'
color'
:
{
'
b'
:
1
}
},
{
'
id'
:
'
B'
,
'
type'
:
'
BOX'
,
'
pos'
:
{
'
x'
:
-0.3
,
'
y'
:
0.7
,
'
z'
:
0.9355
},
'
size'
:
{
'
length'
:
0.0318
,
'
width'
:
0.0636
,
'
height'
:
0.091
},
'
orientation'
:
{
'
x'
:
0
,
'
y'
:
0
,
'
z'
:
0
,
'
w'
:
1
},
'
color'
:
{
'
g'
:
1
}
},
{
'
id'
:
'
B'
,
'
type'
:
'
BOX'
,
'
pos'
:
{
'
x'
:
-0.3
,
'
y'
:
0.7
,
'
z'
:
0.9355
},
'
size'
:
{
'
length'
:
0.0318
,
'
width'
:
0.0636
,
'
height'
:
0.091
},
'
orientation'
:
{
'
x'
:
0
,
'
y'
:
0
,
'
z'
:
0
,
'
w'
:
1
},
'
color'
:
{
'
g'
:
1
}
},
{
'
id'
:
'
BBIN'
,
'
type'
:
'
OBSTACLE
'
,
'
pos'
:
{
'
x'
:
-0.25
,
'
y'
:
1.7
,
'
z'
:
0.9355
},
'
size'
:
{
'
length'
:
0.033
,
'
width'
:
0.066
,
'
height'
:
0.091
},
'
orientation'
:
{
'
x'
:
0
,
'
y'
:
0
,
'
z'
:
0
,
'
w'
:
1
},
'
color'
:
{
'
g'
:
1
}
},
{
'
id'
:
'
BBIN'
,
'
type'
:
'
BIN
'
,
'
pos'
:
{
'
x'
:
-0.25
,
'
y'
:
1.7
,
'
z'
:
0.9355
},
'
size'
:
{
'
length'
:
0.033
,
'
width'
:
0.066
,
'
height'
:
0.091
},
'
orientation'
:
{
'
x'
:
0
,
'
y'
:
0
,
'
z'
:
0
,
'
w'
:
1
},
'
color'
:
{
'
g'
:
1
}
},
{
'
id'
:
'
arm1'
,
'
type'
:
'
ARM'
,
'
pos'
:
{
'
x'
:
-0.220002
,
'
y'
:
-0.000003
,
'
z'
:
0.89
},
'
size'
:
{
},
'
orientation'
:
{
'
x'
:
0.000000
,
'
y'
:
0.000000
,
'
z'
:
0.000001
,
'
w'
:
1.000000
},
'
color'
:
{
'
r'
:
1.00
,
'
g'
:
1.00
,
'
b'
:
1.00
}
},
{
'
id'
:
'
arm1'
,
'
type'
:
'
ARM'
,
'
pos'
:
{
'
x'
:
-0.220002
,
'
y'
:
-0.000003
,
'
z'
:
0.89
},
'
size'
:
{
},
'
orientation'
:
{
'
x'
:
0.000000
,
'
y'
:
0.000000
,
'
z'
:
0.000001
,
'
w'
:
1.000000
},
'
color'
:
{
'
r'
:
1.00
,
'
g'
:
1.00
,
'
b'
:
1.00
}
},
{
'
id'
:
'
arm2'
,
'
type'
:
'
ARM'
,
'
pos'
:
{
'
x'
:
-0.220005
,
'
y'
:
1.304997
,
'
z'
:
0.89
},
'
size'
:
{
},
'
orientation'
:
{
'
x'
:
0.000000
,
'
y'
:
0.000000
,
'
z'
:
0.000001
,
'
w'
:
1.000000
},
'
color'
:
{
'
r'
:
1.00
,
'
g'
:
1.00
,
'
b'
:
1.00
}
},
{
'
id'
:
'
arm2'
,
'
type'
:
'
ARM'
,
'
pos'
:
{
'
x'
:
-0.220005
,
'
y'
:
1.304997
,
'
z'
:
0.89
},
'
size'
:
{
},
'
orientation'
:
{
'
x'
:
0.000000
,
'
y'
:
0.000000
,
'
z'
:
0.000001
,
'
w'
:
1.000000
},
'
color'
:
{
'
r'
:
1.00
,
'
g'
:
1.00
,
'
b'
:
1.00
}
},
]}
]}
\ No newline at end of file
This diff is collapsed.
Click to expand it.
src/mediator/mg_mediator.cpp
+
311
−
71
View file @
88948f3f
#include
"mediator/mg_mediator.h"
#include
"mediator/mg_mediator.h"
#include
"yaml-cpp/yaml.h"
std
::
vector
<
moveit_task_constructor_msgs
::
ExecuteTaskSolutionGoal
>
shared_structure
;
std
::
vector
<
moveit_task_constructor_msgs
::
ExecuteTaskSolutionGoal
>
shared_structure
;
std
::
mutex
shared_mutex
;
std
::
mutex
shared_mutex
;
std
::
condition_variable
cv
;
std
::
condition_variable
cv
;
...
@@ -28,7 +30,10 @@ void MGMediator::processSharedStructure() {
...
@@ -28,7 +30,10 @@ void MGMediator::processSharedStructure() {
--
it
;
// Adjust the iterator after erasing
--
it
;
// Adjust the iterator after erasing
}
}
}
}
}
// The lock is automatically released here when `lock` goes out of scope
}
sendToAll
(
"payload/panda_arm1"
,
"penis"
);
// The lock is automatically released here when `lock` goes out of scope
// Print the sizes of the sub_trajectory vectors
// Print the sizes of the sub_trajectory vectors
// if (!shared_structure.empty()) {
// if (!shared_structure.empty()) {
...
@@ -37,7 +42,7 @@ void MGMediator::processSharedStructure() {
...
@@ -37,7 +42,7 @@ void MGMediator::processSharedStructure() {
// }
// }
// }
// }
//
ROS_INFO("Do something");
ROS_INFO
(
"Do something"
);
peak
.
clear
();
peak
.
clear
();
...
@@ -55,20 +60,85 @@ MGMediator::MGMediator(std::shared_ptr<ros::NodeHandle> const& nh)
...
@@ -55,20 +60,85 @@ MGMediator::MGMediator(std::shared_ptr<ros::NodeHandle> const& nh)
,
mgi_
(
std
::
make_shared
<
moveit
::
planning_interface
::
MoveGroupInterface
>
(
"panda_arms"
))
,
mgi_
(
std
::
make_shared
<
moveit
::
planning_interface
::
MoveGroupInterface
>
(
"panda_arms"
))
,
planning_scene_diff_publisher_
(
std
::
make_shared
<
ros
::
Publisher
>
(
nh_
->
advertise
<
moveit_msgs
::
PlanningScene
>
(
"planning_scene"
,
1
))){
,
planning_scene_diff_publisher_
(
std
::
make_shared
<
ros
::
Publisher
>
(
nh_
->
advertise
<
moveit_msgs
::
PlanningScene
>
(
"planning_scene"
,
1
))){
// MQTT HANDLE
// addCallback("command/place", [&](const std::string& data) {
// std::vector<std::tuple<std::string, std::string, std::string>> place_command{std::make_tuple("A", "panda_arm1", "target"), std::make_tuple("B", "panda_arm2", "target") };
// for (const auto& tuple: place_command) {
// moveit::task_constructor::Task mgt = place(std::get<0>(tuple), std::get<1>(tuple), std::get<2>(tuple));
// if(mgt.plan(1)) {
// //mgt.introspection().publishSolution(*mgt.solutions().front());
// //mgt.execute(*mgt.solutions().front());
// moveit_task_constructor_msgs::ExecuteTaskSolutionGoal e;
// mgt.solutions().front()->fillMessage(e.solution, &mgt.introspection());
// {
// std::lock_guard<std::mutex> lock(shared_mutex);
// shared_structure.push_back(std::move(e));
// }
// // Notify the consuming thread
// cv.notify_one();
// }
// }
// });
// moveit::task_constructor::Task mgt2 = place("A", "panda_arm1", "dafd");
// if(mgt2.plan(1)) {
// //mgt.introspection().publishSolution(*mgt.solutions().front());
// mgt2.execute(*mgt2.solutions().front());
// }
robot_model_loader
::
RobotModelLoaderPtr
robot_model_loader
;
robot_model_loader
=
std
::
make_shared
<
robot_model_loader
::
RobotModelLoader
>
(
"robot_description"
);
robot_model_
=
robot_model_loader
->
getModel
();
planning_scene_monitor_
=
std
::
make_shared
<
planning_scene_monitor
::
PlanningSceneMonitor
>
(
"robot_description"
);
visual_tools_
=
std
::
make_shared
<
moveit_visual_tools
::
MoveItVisualTools
>
(
"world"
,
"/rviz_visual_tools"
);
visual_tools_
->
loadMarkerPub
();
visual_tools_
->
waitForMarkerPub
();
visual_tools_
->
loadRemoteControl
();
visual_tools_
->
setPlanningSceneMonitor
(
planning_scene_monitor_
);
visual_tools_
->
trigger
();
sampling_planner_
->
setProperty
(
"goal_joint_tolerance"
,
1e-5
);
// cartesian
cartesian_planner_
->
setMaxVelocityScaling
(
1.0
);
cartesian_planner_
->
setMaxAccelerationScaling
(
1.0
);
cartesian_planner_
->
setStepSize
(
.01
);
}
void
MGMediator
::
setupMqtt
(){
auto
mqtt
=
std
::
make_shared
<
MqttConnection
>
(
"localhost"
,
"op"
);
auto
mqtt
=
std
::
make_shared
<
MqttConnection
>
(
"localhost"
,
"op"
);
mqtt
->
listen
(
"command/place"
);
mqtt
->
listen
(
"command/pickup"
);
addConnection
(
std
::
move
(
mqtt
));
for
(
const
auto
&
rob
:
robots_
){
std
::
stringstream
ss
;
ss
<<
rob
.
first
<<
"/command"
;
mqtt
->
listen
(
ss
.
str
());
addCallback
(
"command/pickup"
,
[
&
](
const
std
::
string
&
data
)
{
addCallback
(
ss
.
str
(),
[
&
](
const
std
::
string
&
data
)
{
std
::
vector
<
std
::
pair
<
std
::
string
,
std
::
string
>>
pickup_command
{
std
::
make_pair
(
"A"
,
"panda_arm1"
),
std
::
make_pair
(
"B"
,
"panda_arm2"
)
};
YAML
::
Node
node
;
for
(
const
auto
&
pair
:
pickup_command
)
{
try
{
moveit
::
task_constructor
::
Task
mgt
=
pickUp
(
pair
.
first
,
pair
.
second
);
node
=
YAML
::
Load
(
data
);
for
(
YAML
::
const_iterator
it
=
node
.
begin
();
it
!=
node
.
end
();
++
it
)
{
std
::
stringstream
sss
;
sss
<<
"Key: "
<<
it
->
first
.
as
<
std
::
string
>
()
<<
", Value: "
<<
it
->
second
.
as
<
std
::
string
>
();
ROS_INFO
(
"%s"
,
sss
.
str
().
c_str
());
}
}
catch
(
const
YAML
::
RepresentationException
&
e
){
ROS_INFO
(
"Bad Command"
);
return
;}
if
(
node
[
"mode"
].
as
<
std
::
string
>
()
==
"PICKUP"
){
moveit
::
task_constructor
::
Task
mgt
=
pickUp
(
node
[
"object"
].
as
<
std
::
string
>
(),
node
[
"robot"
].
as
<
std
::
string
>
());
if
(
mgt
.
plan
(
1
))
{
if
(
mgt
.
plan
(
1
))
{
//mgt.introspection().publishSolution(*mgt.solutions().front());
//mgt.introspection().publishSolution(*mgt.solutions().front());
//
mgt.execute(*mgt.solutions().front());
mgt
.
execute
(
*
mgt
.
solutions
().
front
());
moveit_task_constructor_msgs
::
ExecuteTaskSolutionGoal
e
;
moveit_task_constructor_msgs
::
ExecuteTaskSolutionGoal
e
;
mgt
.
solutions
().
front
()
->
fillMessage
(
e
.
solution
,
&
mgt
.
introspection
());
mgt
.
solutions
().
front
()
->
fillMessage
(
e
.
solution
,
&
mgt
.
introspection
());
...
@@ -81,15 +151,12 @@ MGMediator::MGMediator(std::shared_ptr<ros::NodeHandle> const& nh)
...
@@ -81,15 +151,12 @@ MGMediator::MGMediator(std::shared_ptr<ros::NodeHandle> const& nh)
cv
.
notify_one
();
cv
.
notify_one
();
}
}
}
}
});
addCallback
(
"command/place"
,
[
&
](
const
std
::
string
&
data
)
{
if
(
node
[
"mode"
].
as
<
std
::
string
>
()
==
"PLACE"
){
std
::
vector
<
std
::
tuple
<
std
::
string
,
std
::
string
,
std
::
string
>>
place_command
{
std
::
make_tuple
(
"A"
,
"panda_arm1"
,
"target"
),
std
::
make_tuple
(
"B"
,
"panda_arm2"
,
"target"
)
};
moveit
::
task_constructor
::
Task
mgt
=
place
(
node
[
"object"
].
as
<
std
::
string
>
(),
node
[
"robot"
].
as
<
std
::
string
>
(),
node
[
"place"
].
as
<
std
::
string
>
());
for
(
const
auto
&
tuple
:
place_command
)
{
moveit
::
task_constructor
::
Task
mgt
=
place
(
std
::
get
<
0
>
(
tuple
),
std
::
get
<
1
>
(
tuple
),
std
::
get
<
2
>
(
tuple
));
if
(
mgt
.
plan
(
1
))
{
if
(
mgt
.
plan
(
1
))
{
//mgt.introspection().publishSolution(*mgt.solutions().front());
//mgt.introspection().publishSolution(*mgt.solutions().front());
//
mgt.execute(*mgt.solutions().front());
mgt
.
execute
(
*
mgt
.
solutions
().
front
());
moveit_task_constructor_msgs
::
ExecuteTaskSolutionGoal
e
;
moveit_task_constructor_msgs
::
ExecuteTaskSolutionGoal
e
;
mgt
.
solutions
().
front
()
->
fillMessage
(
e
.
solution
,
&
mgt
.
introspection
());
mgt
.
solutions
().
front
()
->
fillMessage
(
e
.
solution
,
&
mgt
.
introspection
());
...
@@ -100,44 +167,38 @@ MGMediator::MGMediator(std::shared_ptr<ros::NodeHandle> const& nh)
...
@@ -100,44 +167,38 @@ MGMediator::MGMediator(std::shared_ptr<ros::NodeHandle> const& nh)
// Notify the consuming thread
// Notify the consuming thread
cv
.
notify_one
();
cv
.
notify_one
();
}
}
}
}
});
// moveit::task_constructor::Task mgt2 = place("A", "panda_arm1", "dafd");
// if(mgt2.plan(1)) {
// //mgt.introspection().publishSolution(*mgt.solutions().front());
// mgt2.execute(*mgt2.solutions().front());
// }
robot_model_loader
::
RobotModelLoaderPtr
robot_model_loader
;
robot_model_loader
=
std
::
make_shared
<
robot_model_loader
::
RobotModelLoader
>
(
"robot_description"
);
robot_model_
=
robot_model_loader
->
getModel
();
planning_scene_monitor_
=
std
::
make_shared
<
planning_scene_monitor
::
PlanningSceneMonitor
>
(
"robot_description"
);
if
(
node
[
"mode"
].
as
<
std
::
string
>
()
==
"DROP"
){
visual_tools_
=
std
::
make_shared
<
moveit_visual_tools
::
MoveItVisualTools
>
(
"world"
,
"/rviz_visual_tools"
);
moveit
::
task_constructor
::
Task
mgt
=
drop
(
node
[
"object"
].
as
<
std
::
string
>
(),
node
[
"robot"
].
as
<
std
::
string
>
(),
node
[
"drop"
].
as
<
std
::
string
>
());
visual_tools_
->
loadMarkerPub
();
if
(
mgt
.
plan
(
1
))
{
//mgt.introspection().publishSolution(*mgt.solutions().front());
mgt
.
execute
(
*
mgt
.
solutions
().
front
());
moveit_task_constructor_msgs
::
ExecuteTaskSolutionGoal
e
;
mgt
.
solutions
().
front
()
->
fillMessage
(
e
.
solution
,
&
mgt
.
introspection
());
visual_tools_
->
waitForMarkerPub
();
{
std
::
lock_guard
<
std
::
mutex
>
lock
(
shared_mutex
);
shared_structure
.
push_back
(
std
::
move
(
e
));
}
visual_tools_
->
loadRemoteControl
();
// Notify the consuming thread
visual_tools_
->
setPlanningSceneMonitor
(
planning_scene_monitor_
);
cv
.
notify_one
();
visual_tools_
->
trigger
();
}
}
});
}
addConnection
(
std
::
move
(
mqtt
));
sampling_planner_
->
setProperty
(
"goal_joint_tolerance"
,
1e-5
);
// cartesian
cartesian_planner_
->
setMaxVelocityScaling
(
1.0
);
cartesian_planner_
->
setMaxAccelerationScaling
(
1.0
);
cartesian_planner_
->
setStepSize
(
.01
);
}
}
void
MGMediator
::
mediate
(){
void
MGMediator
::
mediate
(){
setupMqtt
();
setPanel
();
setPanel
();
publishTables
();
publishTables
();
...
@@ -181,7 +242,7 @@ moveit::task_constructor::Task MGMediator::pickUp(const std::string& obj, const
...
@@ -181,7 +242,7 @@ moveit::task_constructor::Task MGMediator::pickUp(const std::string& obj, const
ROS_INFO
(
"ss1 %s"
,
support_surface1
.
c_str
());
ROS_INFO
(
"ss1 %s"
,
support_surface1
.
c_str
());
std
::
string
name
=
"Pick
&Place
"
;
std
::
string
name
=
"Pick"
;
task_
.
stages
()
->
setName
(
name
+
std
::
to_string
(
static_cast
<
int
>
(
ros
::
Time
::
now
().
toNSec
())));
task_
.
stages
()
->
setName
(
name
+
std
::
to_string
(
static_cast
<
int
>
(
ros
::
Time
::
now
().
toNSec
())));
task_
.
loadRobotModel
();
task_
.
loadRobotModel
();
task_
.
setRobotModel
(
mr
->
mgi
()
->
getRobotModel
());
task_
.
setRobotModel
(
mr
->
mgi
()
->
getRobotModel
());
...
@@ -359,7 +420,7 @@ moveit::task_constructor::Task MGMediator::place(const std::string& obj, const s
...
@@ -359,7 +420,7 @@ moveit::task_constructor::Task MGMediator::place(const std::string& obj, const s
std
::
string
name
=
"
Pick&
Place"
;
std
::
string
name
=
"Place"
;
task_
.
stages
()
->
setName
(
name
+
std
::
to_string
(
static_cast
<
int
>
(
ros
::
Time
::
now
().
toNSec
())));
task_
.
stages
()
->
setName
(
name
+
std
::
to_string
(
static_cast
<
int
>
(
ros
::
Time
::
now
().
toNSec
())));
task_
.
loadRobotModel
();
task_
.
loadRobotModel
();
task_
.
setRobotModel
(
mr
->
mgi
()
->
getRobotModel
());
task_
.
setRobotModel
(
mr
->
mgi
()
->
getRobotModel
());
...
@@ -514,6 +575,157 @@ moveit::task_constructor::Task MGMediator::place(const std::string& obj, const s
...
@@ -514,6 +575,157 @@ moveit::task_constructor::Task MGMediator::place(const std::string& obj, const s
return
task_
;
return
task_
;
}
}
moveit
::
task_constructor
::
Task
MGMediator
::
drop
(
const
std
::
string
&
obj
,
const
std
::
string
&
robotId
,
const
std
::
string
&
target
){
AbstractRobotDecorator
*
mr
=
nullptr
;
try
{
mr
=
robots_
.
at
(
robotId
).
get
();
}
catch
(
std
::
out_of_range
&
oor
){
ROS_INFO
(
"Robot not found"
);
ros
::
shutdown
();
}
moveit_msgs
::
CollisionObject
target_bin
=
psi_
->
getObjects
().
at
(
target
);
const
std
::
string
object
=
obj
;
moveit
::
task_constructor
::
Task
task_
;
tf2
::
Transform
t
(
tf2
::
Quaternion
(
target_bin
.
pose
.
orientation
.
x
,
target_bin
.
pose
.
orientation
.
y
,
target_bin
.
pose
.
orientation
.
z
,
target_bin
.
pose
.
orientation
.
w
),
tf2
::
Vector3
(
target_bin
.
pose
.
position
.
x
,
target_bin
.
pose
.
position
.
y
,
(
3
*
target_bin
.
primitives
[
0
].
dimensions
[
2
])
+
target_bin
.
pose
.
position
.
z
));
std
::
string
support_surface1
=
"nichts"
;
// Just some value
std
::
string
support_surface2
;
for
(
const
auto
&
s_r
:
robots_
){
std
::
string
str
;
std
::
bitset
<
3
>
panel_mask
(
7
);
if
(
s_r
.
second
->
checkSingleObjectCollision
(
t
,
str
,
panel_mask
))
support_surface2
=
str
;
}
ROS_INFO
(
"ss1 %s"
,
support_surface2
.
c_str
());
std
::
string
name
=
"Drop"
;
task_
.
stages
()
->
setName
(
name
+
std
::
to_string
(
static_cast
<
int
>
(
ros
::
Time
::
now
().
toNSec
())));
task_
.
loadRobotModel
();
task_
.
setRobotModel
(
mr
->
mgi
()
->
getRobotModel
());
task_
.
setProperty
(
"group"
,
mr
->
name
());
task_
.
setProperty
(
"eef"
,
mr
->
map
()[
"eef_name"
]);
task_
.
setProperty
(
"hand"
,
mr
->
map
()[
"hand_group_name"
]);
task_
.
setProperty
(
"hand_grasping_frame"
,
mr
->
map
()[
"hand_frame"
]);
task_
.
setProperty
(
"ik_frame"
,
mr
->
map
()[
"hand_frame"
]);
moveit
::
task_constructor
::
Stage
*
current_state_ptr
=
nullptr
;
{
auto
current_state
=
std
::
make_unique
<
moveit
::
task_constructor
::
stages
::
CurrentState
>
(
"current state"
);
auto
applicability_filter
=
std
::
make_unique
<
moveit
::
task_constructor
::
stages
::
PredicateFilter
>
(
"applicability test"
,
std
::
move
(
current_state
));
applicability_filter
->
setPredicate
([
object
](
const
moveit
::
task_constructor
::
SolutionBase
&
s
,
std
::
string
&
comment
)
{
if
(
s
.
start
()
->
scene
()
->
getCurrentState
().
hasAttachedBody
(
object
))
{
comment
=
"object with id '"
+
object
+
"' is already attached and cannot be picked"
;
// psst, I changed the return values and it works... dont tell this mama
return
true
;
}
return
false
;
});
current_state_ptr
=
applicability_filter
.
get
();
task_
.
add
(
std
::
move
(
applicability_filter
));
}
{
auto
stage
=
std
::
make_unique
<
moveit
::
task_constructor
::
stages
::
Connect
>
(
"move to place"
,
moveit
::
task_constructor
::
stages
::
Connect
::
GroupPlannerVector
{
{
mr
->
name
(),
sampling_planner_
}
});
stage
->
setTimeout
(
5.0
);
stage
->
properties
().
configureInitFrom
(
moveit
::
task_constructor
::
Stage
::
PARENT
);
task_
.
add
(
std
::
move
(
stage
));
}
{
auto
place
=
std
::
make_unique
<
moveit
::
task_constructor
::
SerialContainer
>
(
"place object"
);
task_
.
properties
().
exposeTo
(
place
->
properties
(),
{
"eef"
,
"hand"
,
"group"
});
place
->
properties
().
configureInitFrom
(
moveit
::
task_constructor
::
Stage
::
PARENT
,
{
"eef"
,
"hand"
,
"group"
});
{
// Generate Place Pose
auto
stage
=
std
::
make_unique
<
moveit
::
task_constructor
::
stages
::
GeneratePlacePose
>
(
"generate place pose"
);
stage
->
properties
().
configureInitFrom
(
moveit
::
task_constructor
::
Stage
::
PARENT
,
{
"ik_frame"
});
stage
->
properties
().
set
(
"marker_ns"
,
"place_pose"
);
stage
->
setObject
(
object
);
// Set target pose
geometry_msgs
::
PoseStamped
p
;
p
.
header
.
frame_id
=
"world"
;
p
.
pose
.
position
.
x
=
t
.
getOrigin
().
getX
();
p
.
pose
.
position
.
y
=
t
.
getOrigin
().
getY
();
p
.
pose
.
position
.
z
=
t
.
getOrigin
().
getZ
();
p
.
pose
.
orientation
.
x
=
t
.
getRotation
().
getX
();
p
.
pose
.
orientation
.
y
=
t
.
getRotation
().
getY
();
p
.
pose
.
orientation
.
z
=
t
.
getRotation
().
getZ
();
p
.
pose
.
orientation
.
w
=
t
.
getRotation
().
getW
();
stage
->
setPose
(
p
);
stage
->
setMonitoredStage
(
current_state_ptr
);
// Hook into attach_object_stage
// Compute IK
Eigen
::
Quaterniond
eigen
=
Eigen
::
AngleAxisd
(
1.571f
,
Eigen
::
Vector3d
::
UnitX
())
*
Eigen
::
AngleAxisd
(
0.785f
,
Eigen
::
Vector3d
::
UnitY
())
*
Eigen
::
AngleAxisd
(
1.571f
,
Eigen
::
Vector3d
::
UnitZ
());
Eigen
::
Translation3d
trans
(
0.1f
,
0
,
0
);
Eigen
::
Isometry3d
ik
=
eigen
*
trans
;
auto
wrapper
=
std
::
make_unique
<
moveit
::
task_constructor
::
stages
::
ComputeIK
>
(
"place pose IK"
,
std
::
move
(
stage
));
wrapper
->
setMaxIKSolutions
(
2
);
wrapper
->
setIKFrame
(
ik
,
mr
->
map
()[
"hand_frame"
]);
wrapper
->
properties
().
configureInitFrom
(
moveit
::
task_constructor
::
Stage
::
PARENT
,
{
"eef"
,
"group"
});
wrapper
->
properties
().
configureInitFrom
(
moveit
::
task_constructor
::
Stage
::
INTERFACE
,
{
"target_pose"
});
place
->
insert
(
std
::
move
(
wrapper
));
}
{
auto
stage
=
std
::
make_unique
<
moveit
::
task_constructor
::
stages
::
MoveTo
>
(
"open hand"
,
sampling_planner_
);
stage
->
setGroup
(
mr
->
map
()[
"eef_name"
]);
stage
->
setGoal
(
"open"
);
place
->
insert
(
std
::
move
(
stage
));
}
{
auto
stage
=
std
::
make_unique
<
moveit
::
task_constructor
::
stages
::
ModifyPlanningScene
>
(
"detach object"
);
stage
->
detachObject
(
object
,
mr
->
map
()[
"hand_frame"
]);
place
->
insert
(
std
::
move
(
stage
));
}
{
auto
stage
=
std
::
make_unique
<
moveit
::
task_constructor
::
stages
::
MoveTo
>
(
"close hand"
,
sampling_planner_
);
stage
->
setGroup
(
mr
->
map
()[
"eef_name"
]);
stage
->
setGoal
(
"close"
);
place
->
insert
(
std
::
move
(
stage
));
}
// Add place container to task
task_
.
add
(
std
::
move
(
place
));
}
{
auto
stage
=
std
::
make_unique
<
moveit
::
task_constructor
::
stages
::
MoveTo
>
(
"move home"
,
sampling_planner_
);
stage
->
properties
().
configureInitFrom
(
moveit
::
task_constructor
::
Stage
::
PARENT
,
{
"group"
});
stage
->
setGoal
(
"ready"
);
stage
->
restrictDirection
(
moveit
::
task_constructor
::
stages
::
MoveTo
::
FORWARD
);
task_
.
add
(
std
::
move
(
stage
));
{
auto
stage
=
std
::
make_unique
<
moveit
::
task_constructor
::
stages
::
ModifyPlanningScene
>
(
"allow collision (hand,object)"
);
stage
->
allowCollisions
(
object
,
task_
.
getRobotModel
()
->
getJointModelGroup
(
mr
->
map
()[
"eef_name"
])
->
getLinkModelNamesWithCollisionGeometry
(),
false
);
task_
.
add
(
std
::
move
(
stage
));
}
}
return
task_
;
}
//! connect robot and initialize Moveit components
//! connect robot and initialize Moveit components
/*!
/*!
Set up acm_ and rs_ members to track, merge, and publish changes during execution.
Set up acm_ and rs_ members to track, merge, and publish changes during execution.
...
@@ -598,6 +810,20 @@ void MGMediator::publishTables(){
...
@@ -598,6 +810,20 @@ void MGMediator::publishTables(){
ss
<<
",{
\"
id
\"
:
\"
"
<<
c
.
Name
<<
"
\"
,
\"
type
\"
:
\"
BOX
\"
,
\"
pos
\"
: {
\"
x
\"
: "
<<
c
.
Pose
.
position
.
x
<<
",
\"
y
\"
: "
<<
c
.
Pose
.
position
.
y
<<
",
\"
z
\"
: "
<<
c
.
Pose
.
position
.
x
<<
"},
\"
size
\"
: {
\"
length
\"
: "
<<
c
.
x_depth
<<
",
\"
width
\"
: "
<<
c
.
y_width
<<
",
\"
height
\"
: "
<<
c
.
z_heigth
<<
"},
\"
orientation
\"
: {
\"
x
\"
: "
<<
c
.
Pose
.
orientation
.
x
<<
",
\"
y
\"
: "
<<
c
.
Pose
.
orientation
.
y
<<
" ,
\"
z
\"
: "
<<
c
.
Pose
.
orientation
.
z
<<
",
\"
w
\"
: "
<<
c
.
Pose
.
orientation
.
w
<<
"},"
;
ss
<<
",{
\"
id
\"
:
\"
"
<<
c
.
Name
<<
"
\"
,
\"
type
\"
:
\"
BOX
\"
,
\"
pos
\"
: {
\"
x
\"
: "
<<
c
.
Pose
.
position
.
x
<<
",
\"
y
\"
: "
<<
c
.
Pose
.
position
.
y
<<
",
\"
z
\"
: "
<<
c
.
Pose
.
position
.
x
<<
"},
\"
size
\"
: {
\"
length
\"
: "
<<
c
.
x_depth
<<
",
\"
width
\"
: "
<<
c
.
y_width
<<
",
\"
height
\"
: "
<<
c
.
z_heigth
<<
"},
\"
orientation
\"
: {
\"
x
\"
: "
<<
c
.
Pose
.
orientation
.
x
<<
",
\"
y
\"
: "
<<
c
.
Pose
.
orientation
.
y
<<
" ,
\"
z
\"
: "
<<
c
.
Pose
.
orientation
.
z
<<
",
\"
w
\"
: "
<<
c
.
Pose
.
orientation
.
w
<<
"},"
;
ss
<<
"
\"
color
\"
: {
\"
r
\"
: 0.2,
\"
g
\"
: 0.2,
\"
b
\"
: 0.2}}"
;
ss
<<
"
\"
color
\"
: {
\"
r
\"
: 0.2,
\"
g
\"
: 0.2,
\"
b
\"
: 0.2}}"
;
}
}
// acm_.insert(std::pair<std::string, std::vector<uint8_t>>(item.id, std::vector<uint8_t>()));
}
for
(
auto
&
c
:
cuboid_reader_
->
cuboidObstacle
()){
tf2
::
Transform
trans
(
tf2
::
Quaternion
(
0
,
0
,
0
,
1
),
tf2
::
Vector3
(
c
.
Pose
.
position
.
x
,
c
.
Pose
.
position
.
y
,
c
.
Pose
.
position
.
z
));
if
(
pair
.
second
->
checkSingleObjectCollision
(
trans
,
str
,
panel_mask
))
{
ss
<<
",{
\"
id
\"
:
\"
"
<<
c
.
Name
<<
"
\"
,
\"
type
\"
:
\"
BIN
\"
,
\"
pos
\"
: {
\"
x
\"
: "
<<
c
.
Pose
.
position
.
x
<<
",
\"
y
\"
: "
<<
c
.
Pose
.
position
.
y
<<
",
\"
z
\"
: "
<<
c
.
Pose
.
position
.
x
<<
"},
\"
size
\"
: {
\"
length
\"
: "
<<
c
.
x_depth
<<
",
\"
width
\"
: "
<<
c
.
y_width
<<
",
\"
height
\"
: "
<<
c
.
z_heigth
<<
"},
\"
orientation
\"
: {
\"
x
\"
: "
<<
c
.
Pose
.
orientation
.
x
<<
",
\"
y
\"
: "
<<
c
.
Pose
.
orientation
.
y
<<
" ,
\"
z
\"
: "
<<
c
.
Pose
.
orientation
.
z
<<
",
\"
w
\"
: "
<<
c
.
Pose
.
orientation
.
w
<<
"},"
;
ss
<<
"
\"
color
\"
: {
\"
r
\"
: 0.2,
\"
g
\"
: 0.2,
\"
b
\"
: 0.2}}"
;
}
}
ss
<<
"]}"
;
for
(
auto
&
c
:
cuboid_reader_
->
cuboidObstacle
()){
moveit_msgs
::
CollisionObject
item
;
moveit_msgs
::
CollisionObject
item
;
item
.
id
=
c
.
Name
;
item
.
id
=
c
.
Name
;
...
@@ -621,18 +847,32 @@ void MGMediator::publishTables(){
...
@@ -621,18 +847,32 @@ void MGMediator::publishTables(){
item
.
operation
=
item
.
ADD
;
item
.
operation
=
item
.
ADD
;
psi_
->
applyCollisionObject
(
item
);
psi_
->
applyCollisionObject
(
item
);
// acm_.insert(std::pair<std::string, std::vector<uint8_t>>(item.id, std::vector<uint8_t>()));
}
}
for
(
auto
&
c
:
cuboid_reader_
->
cuboidObstacle
()){
for
(
auto
&
c
:
cuboid_reader_
->
cuboidBox
()){
tf2
::
Transform
trans
(
tf2
::
Quaternion
(
0
,
0
,
0
,
1
),
tf2
::
Vector3
(
c
.
Pose
.
position
.
x
,
c
.
Pose
.
position
.
y
,
c
.
Pose
.
position
.
z
));
moveit_msgs
::
CollisionObject
item
;
if
(
pair
.
second
->
checkSingleObjectCollision
(
trans
,
str
,
panel_mask
))
{
item
.
id
=
c
.
Name
;
ss
<<
",{
\"
id
\"
:
\"
"
<<
c
.
Name
<<
"
\"
,
\"
type
\"
:
\"
BIN
\"
,
\"
pos
\"
: {
\"
x
\"
: "
<<
c
.
Pose
.
position
.
x
<<
",
\"
y
\"
: "
<<
c
.
Pose
.
position
.
y
<<
",
\"
z
\"
: "
<<
c
.
Pose
.
position
.
x
<<
"},
\"
size
\"
: {
\"
length
\"
: "
<<
c
.
x_depth
<<
",
\"
width
\"
: "
<<
c
.
y_width
<<
",
\"
height
\"
: "
<<
c
.
z_heigth
<<
"},
\"
orientation
\"
: {
\"
x
\"
: "
<<
c
.
Pose
.
orientation
.
x
<<
",
\"
y
\"
: "
<<
c
.
Pose
.
orientation
.
y
<<
" ,
\"
z
\"
: "
<<
c
.
Pose
.
orientation
.
z
<<
",
\"
w
\"
: "
<<
c
.
Pose
.
orientation
.
w
<<
"},"
;
item
.
header
.
frame_id
=
"world"
;
ss
<<
"
\"
color
\"
: {
\"
r
\"
: 0.2,
\"
g
\"
: 0.2,
\"
b
\"
: 0.2}}"
;
item
.
header
.
stamp
=
ros
::
Time
::
now
();
}
item
.
primitives
.
resize
(
1
);
item
.
primitives
[
0
].
type
=
item
.
primitives
[
0
].
BOX
;
item
.
primitives
[
0
].
dimensions
.
resize
(
3
);
item
.
primitives
[
0
].
dimensions
[
0
]
=
c
.
x_depth
;
item
.
primitives
[
0
].
dimensions
[
1
]
=
c
.
y_width
;
item
.
primitives
[
0
].
dimensions
[
2
]
=
c
.
z_heigth
;
item
.
primitive_poses
.
resize
(
1
);
item
.
primitive_poses
[
0
].
position
.
x
=
c
.
Pose
.
position
.
x
;
item
.
primitive_poses
[
0
].
position
.
y
=
c
.
Pose
.
position
.
y
;
item
.
primitive_poses
[
0
].
position
.
z
=
c
.
Pose
.
position
.
z
;
item
.
primitive_poses
[
0
].
orientation
.
x
=
c
.
Pose
.
orientation
.
x
;
item
.
primitive_poses
[
0
].
orientation
.
y
=
c
.
Pose
.
orientation
.
y
;
item
.
primitive_poses
[
0
].
orientation
.
z
=
c
.
Pose
.
orientation
.
z
;
item
.
primitive_poses
[
0
].
orientation
.
w
=
c
.
Pose
.
orientation
.
w
;
item
.
operation
=
item
.
ADD
;
psi_
->
applyCollisionObject
(
item
);
}
}
ss
<<
"]}"
;
std
::
stringstream
rss
;
std
::
stringstream
rss
;
rss
<<
"World/panda_arm1panda_arm2/"
<<
pair
.
first
;
rss
<<
"World/panda_arm1panda_arm2/"
<<
pair
.
first
;
...
...
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