diff --git a/.catkin_tools/profiles/default/devel_collisions.txt b/.catkin_tools/profiles/default/devel_collisions.txt
index 5d3a89cc2ffe6db746820e5dd4dbcbe8513da96c..5fe0a469a7d78e0289e1efb6e99272ac49ac2943 100644
--- a/.catkin_tools/profiles/default/devel_collisions.txt
+++ b/.catkin_tools/profiles/default/devel_collisions.txt
@@ -1,4 +1,4 @@
/home/matteo/ws_panda/devel/./cmake.lock 42
-/home/matteo/reachability/devel/./cmake.lock 26357
+/home/matteo/reachability/devel/./cmake.lock 26396
/home/matteo/reachability/devel/lib/libmoveit_grasps.so 79
/home/matteo/reachability/devel/lib/libmoveit_grasps_filter.so 79
diff --git a/src/mtc/src/impl/moveit_mediator.cpp b/src/mtc/src/impl/moveit_mediator.cpp
index 430fe585e32e3a494e3114a8bfb888608bfd4ac5..c726da371f8c71f7422a0efa20e9b6b179edd619 100644
--- a/src/mtc/src/impl/moveit_mediator.cpp
+++ b/src/mtc/src/impl/moveit_mediator.cpp
@@ -273,11 +273,11 @@ void Moveit_mediator::setup_task(){
ROS_INFO("is_diff: %i", ps1->is_diff);
ROS_INFO("[Robot 1] link padding");
for (auto lp : ps1->link_padding){
- ROS_INFO("link padding name: %s, padding %d", lp.link_name.c_str(), lp.padding);
+ ROS_INFO("link padding name: %s, padding %f", lp.link_name.c_str(), lp.padding);
}
ROS_INFO("[Robot 1] link scale");
for (auto ls : ps1->link_scale){
- ROS_INFO("link scale name: %s, padding %d", ls.link_name.c_str(), ls.scale);
+ ROS_INFO("link scale name: %s, padding %f", ls.link_name.c_str(), ls.scale);
}
ROS_INFO("[Robot 1] name");
ROS_INFO("name: %s", ps1->name.c_str());
@@ -295,22 +295,22 @@ void Moveit_mediator::setup_task(){
}
ROS_INFO("aco dp Points");
for (auto p : aco.detach_posture.points){
- for (auto pa : p.accelerations) ROS_INFO("aco dp points acc: %d", pa);
- for (auto pa : p.effort) ROS_INFO("aco dp points effort: %d", pa);
+ for (auto pa : p.accelerations) ROS_INFO("aco dp points acc: %f", pa);
+ for (auto pa : p.effort) ROS_INFO("aco dp points effort: %f", pa);
ROS_INFO("aco dp points time from start");
- for (auto pa : p.velocities) ROS_INFO("aco dp points vel: %d", pa);
- for (auto pa : p.positions) ROS_INFO("aco dp points tra: %d", pa);
+ for (auto pa : p.velocities) ROS_INFO("aco dp points vel: %f", pa);
+ for (auto pa : p.positions) ROS_INFO("aco dp points tra: %f", pa);
}
ROS_INFO("aco link names %s", aco.link_name.c_str());
ROS_INFO("aco collis obj id %s", aco.object.id.c_str());
for (auto p : aco.touch_links) ROS_INFO("aco touch link %s", p.c_str());
- ROS_INFO("aco collis obj weight %d", aco.weight);
+ ROS_INFO("aco collis obj weight %f", aco.weight);
}
- for (auto d : ps1->robot_state.joint_state.position) ROS_INFO("js position %d ", d);
+ for (auto d : ps1->robot_state.joint_state.position) ROS_INFO("js position %f ", d);
for (auto d : ps1->robot_state.joint_state.name) ROS_INFO(" js name %s", d.c_str());
- for (auto d : ps1->robot_state.joint_state.velocity) ROS_INFO(" js vel %d", d);
- for (auto d : ps1->robot_state.joint_state.effort) ROS_INFO(" js e %d", d);
+ for (auto d : ps1->robot_state.joint_state.velocity) ROS_INFO(" js vel %f", d);
+ for (auto d : ps1->robot_state.joint_state.effort) ROS_INFO(" js e %f", d);
ROS_INFO ("%s, %i", ps1->robot_state.joint_state.header.frame_id.c_str(), ps1->robot_state.joint_state.header.seq);
for (auto p : ps1->world.collision_objects) ROS_INFO("world co id %s", p.id);
@@ -354,11 +354,11 @@ void Moveit_mediator::setup_task(){
ROS_INFO("[Robot 2] link padding");
for (auto lp : ps2->link_padding){
- ROS_INFO("link padding name: %s, padding %d", lp.link_name.c_str(), lp.padding);
+ ROS_INFO("link padding name: %s, padding %f", lp.link_name.c_str(), lp.padding);
}
ROS_INFO("[Robot 2] link scale");
for (auto ls : ps2->link_scale){
- ROS_INFO("link scale name: %s, padding %d", ls.link_name.c_str(), ls.scale);
+ ROS_INFO("link scale name: %s, padding %f", ls.link_name.c_str(), ls.scale);
}
ROS_INFO("[Robot 2] name");
ROS_INFO("name: %s", ps2->name.c_str());
@@ -376,25 +376,24 @@ void Moveit_mediator::setup_task(){
}
ROS_INFO("aco dp Points");
for (auto p : aco.detach_posture.points){
- for (auto pa : p.accelerations) ROS_INFO("aco dp points acc: %d", pa);
- for (auto pa : p.effort) ROS_INFO("aco dp points effort: %d", pa);
+ for (auto pa : p.accelerations) ROS_INFO("aco dp points acc: %f", pa);
+ for (auto pa : p.effort) ROS_INFO("aco dp points effort: %f", pa);
ROS_INFO("aco dp points time from start");
- for (auto pa : p.velocities) ROS_INFO("aco dp points vel: %d", pa);
- for (auto pa : p.positions) ROS_INFO("aco dp points tra: %d", pa);
+ for (auto pa : p.velocities) ROS_INFO("aco dp points vel: %f", pa);
+ for (auto pa : p.positions) ROS_INFO("aco dp points tra: %f", pa);
}
ROS_INFO("aco link names %s", aco.link_name.c_str());
ROS_INFO("aco collis obj id %s", aco.object.id.c_str());
for (auto p : aco.touch_links) ROS_INFO("aco touch link %s", p.c_str());
- ROS_INFO("aco collis obj weight %d", aco.weight);
+ ROS_INFO("aco collis obj weight %f", aco.weight);
}
- for (auto d : ps2->robot_state.joint_state.position) ROS_INFO("js position %d ", d);
+ for (auto d : ps2->robot_state.joint_state.position) ROS_INFO("js position %f ", d);
for (auto d : ps2->robot_state.joint_state.name) ROS_INFO(" js name %s", d.c_str());
- for (auto d : ps2->robot_state.joint_state.velocity) ROS_INFO(" js vel %d", d);
- for (auto d : ps2->robot_state.joint_state.effort) ROS_INFO(" js e %d", d);
+ for (auto d : ps2->robot_state.joint_state.velocity) ROS_INFO(" js vel %f", d);
+ for (auto d : ps2->robot_state.joint_state.effort) ROS_INFO(" js e %f", d);
ROS_INFO ("%s, %i", ps2->robot_state.joint_state.header.frame_id.c_str(), ps2->robot_state.joint_state.header.seq);
- ROS_INFO("rs js id %s", ps2->robot_state.joint_state.position);
for (auto p : ps2->world.collision_objects) ROS_INFO("world co id %s", p.id.c_str());
ROS_INFO("world om origin %f %f %f, %f %f %f %f", ps2->world.octomap.origin.position.x, ps2->world.octomap.origin.position.y, ps2->world.octomap.origin.position.z, ps2->world.octomap.origin.orientation.x, ps2->world.octomap.origin.orientation.y, ps2->world.octomap.origin.orientation.z, ps2->world.octomap.origin.orientation.w);
ROS_INFO("world om header time, id %s, seq %i", ps2->world.octomap.header.frame_id.c_str(), ps2->world.octomap.header.seq);