From d7ee3a8d9969d8a35597ba5d722e79d1e3ac3815 Mon Sep 17 00:00:00 2001
From: KingMaZito <matteo.aneddama@icloud.com>
Date: Mon, 21 Nov 2022 02:36:18 +0100
Subject: [PATCH] ...
---
.../profiles/default/devel_collisions.txt | 2 +-
src/mtc/include/impl/moveit_mediator.h | 2 +-
src/mtc/include/impl/moveit_robot.h | 1 -
src/mtc/src/impl/moveit_mediator.cpp | 37 +++++++++++++++++--
4 files changed, 35 insertions(+), 7 deletions(-)
diff --git a/.catkin_tools/profiles/default/devel_collisions.txt b/.catkin_tools/profiles/default/devel_collisions.txt
index 66bfbea..5d3a89c 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 26344
+/home/matteo/reachability/devel/./cmake.lock 26357
/home/matteo/reachability/devel/lib/libmoveit_grasps.so 79
/home/matteo/reachability/devel/lib/libmoveit_grasps_filter.so 79
diff --git a/src/mtc/include/impl/moveit_mediator.h b/src/mtc/include/impl/moveit_mediator.h
index 7a0fa09..1a6533f 100644
--- a/src/mtc/include/impl/moveit_mediator.h
+++ b/src/mtc/include/impl/moveit_mediator.h
@@ -95,7 +95,7 @@ class Moveit_mediator : public Abstract_mediator{
void build_wings(std::bitset<3>& wing,int& robot) override;
void set_wings(std::vector<std::vector<wing_BP>>& wbp) override;
- void merge_ps(moveit_msgs::PlanningScene& origin, moveit_msgs::PlanningScene* merge, Moveit_robot* mr);
+ void merge_ps(moveit_msgs::PlanningScene out, moveit_msgs::PlanningScene* in, Moveit_robot* mr);
void publish_tables();
void load_robot_description();
void rewrite_task_template(Abstract_robot* r, moveit_msgs::CollisionObject& source, tf2::Transform& target);
diff --git a/src/mtc/include/impl/moveit_robot.h b/src/mtc/include/impl/moveit_robot.h
index ae1c707..178b3f6 100644
--- a/src/mtc/include/impl/moveit_robot.h
+++ b/src/mtc/include/impl/moveit_robot.h
@@ -24,7 +24,6 @@ class Moveit_robot : public Robot{
map_.insert(std::make_pair<std::string, std::string>("eef_name", hand_n.str()));
map_.insert(std::make_pair<std::string, std::string>("hand_frame", ik_frame_n.str()));
map_.insert(std::make_pair<std::string, std::string>("hand_group_name", hand_n.str()));
-
}
inline moveit::planning_interface::MoveGroupInterface* mgi() {return mgi_;}
diff --git a/src/mtc/src/impl/moveit_mediator.cpp b/src/mtc/src/impl/moveit_mediator.cpp
index f0ed7ca..e65ebd7 100644
--- a/src/mtc/src/impl/moveit_mediator.cpp
+++ b/src/mtc/src/impl/moveit_mediator.cpp
@@ -1,6 +1,7 @@
#include "impl/moveit_mediator.h"
#include <thread>
#include <mutex>
+#include <algorithm>
thread_local moveit::task_constructor::Task task__;
thread_local moveit_task_constructor_msgs::ExecuteTaskSolutionGoal etsg_;
@@ -305,6 +306,13 @@ void Moveit_mediator::setup_task(){
for (auto p : aco.touch_links) ROS_INFO("aco touch link %s", p.c_str());
ROS_INFO("aco collis obj weight %d", 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.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);
+ ROS_INFO ("%s, %i", ps1->robot_state.joint_state.header.frame_id.c_str(), p1->robot_state.joint_state.header.seq);
+
for (auto p : ps1->world.collision_objects) ROS_INFO("world co id %s", p.id);
ROS_INFO("world om origin %f %f %f, %f %f %f %f", ps1->world.octomap.origin.position.x, ps1->world.octomap.origin.position.y, ps1->world.octomap.origin.position.z, ps1->world.octomap.origin.orientation.x, ps1->world.octomap.origin.orientation.y, ps1->world.octomap.origin.orientation.z, ps1->world.octomap.origin.orientation.w);
ROS_INFO("world om header time, id %s, seq %i", ps1->world.octomap.header.frame_id.c_str(), ps1->world.octomap.header.seq);
@@ -379,7 +387,15 @@ void Moveit_mediator::setup_task(){
for (auto p : aco.touch_links) ROS_INFO("aco touch link %s", p.c_str());
ROS_INFO("aco collis obj weight %d", aco.weight);
}
- for (auto p : ps2->world.collision_objects) ROS_INFO("world co id %s", p.id);
+ for (auto d : ps2->robot_state.joint_state.position) ROS_INFO("js position %d ", 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);
+ 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);
ROS_INFO("world om Data");
@@ -408,11 +424,24 @@ void Moveit_mediator::setup_task(){
}
-void Moveit_mediator::merge_ps(moveit_msgs::PlanningScene& origin, moveit_msgs::PlanningScene* merge, Moveit_robot* mr){
- for (auto acm : merge->allowed_collision_matrix.default_entry_names){
- // dafd
+void Moveit_mediator::merge_ps(moveit_msgs::PlanningScene out, moveit_msgs::PlanningScene* in, Moveit_robot* mr){
+ /*
+ for( auto aco : in->robot_state.attached_collision_objects) {
+ out.robot_state.attached_collision_objects.push_back(aco);
}
+ for( auto aco : in->robot_state.joint_state) {
+ out.robot_state.attached_collision_objects.push_back(aco);
+ }
+ sensor_msgs::JointState::
+
+ for (auto str : res.allowed_collision_matrix.entry_names){
+ auto entry = std::find(origin->allowed_collision_matrix.entry_names.begin(), origin->allowed_collision_matrix.entry_names.end(), str);
+ if (entry != origin->allowed_collision_matrix.entry_names.end()){
+ origin->allowed_collision_matrix.entry_values[entry - origin->allowed_collision_matrix.entry_names.begin()]
+ }
+ }
+ */
}
void Moveit_mediator::parallel_exec(Moveit_robot& mr, moveit_msgs::RobotTrajectory rt, moveit_msgs::PlanningScene ps){
--
GitLab