Skip to content
Snippets Groups Projects
Commit d7ee3a8d authored by KingMaZito's avatar KingMaZito
Browse files

...

parent 56ed60e0
No related branches found
No related tags found
No related merge requests found
Pipeline #15007 failed
/home/matteo/ws_panda/devel/./cmake.lock 42 /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.so 79
/home/matteo/reachability/devel/lib/libmoveit_grasps_filter.so 79 /home/matteo/reachability/devel/lib/libmoveit_grasps_filter.so 79
...@@ -95,7 +95,7 @@ class Moveit_mediator : public Abstract_mediator{ ...@@ -95,7 +95,7 @@ class Moveit_mediator : public Abstract_mediator{
void build_wings(std::bitset<3>& wing,int& robot) override; void build_wings(std::bitset<3>& wing,int& robot) override;
void set_wings(std::vector<std::vector<wing_BP>>& wbp) 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 publish_tables();
void load_robot_description(); void load_robot_description();
void rewrite_task_template(Abstract_robot* r, moveit_msgs::CollisionObject& source, tf2::Transform& target); void rewrite_task_template(Abstract_robot* r, moveit_msgs::CollisionObject& source, tf2::Transform& target);
......
...@@ -24,7 +24,6 @@ class Moveit_robot : public Robot{ ...@@ -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>("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_frame", ik_frame_n.str()));
map_.insert(std::make_pair<std::string, std::string>("hand_group_name", hand_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_;} inline moveit::planning_interface::MoveGroupInterface* mgi() {return mgi_;}
......
#include "impl/moveit_mediator.h" #include "impl/moveit_mediator.h"
#include <thread> #include <thread>
#include <mutex> #include <mutex>
#include <algorithm>
thread_local moveit::task_constructor::Task task__; thread_local moveit::task_constructor::Task task__;
thread_local moveit_task_constructor_msgs::ExecuteTaskSolutionGoal etsg_; thread_local moveit_task_constructor_msgs::ExecuteTaskSolutionGoal etsg_;
...@@ -305,6 +306,13 @@ void Moveit_mediator::setup_task(){ ...@@ -305,6 +306,13 @@ void Moveit_mediator::setup_task(){
for (auto p : aco.touch_links) ROS_INFO("aco touch link %s", p.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 %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); 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 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); 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(){ ...@@ -379,7 +387,15 @@ void Moveit_mediator::setup_task(){
for (auto p : aco.touch_links) ROS_INFO("aco touch link %s", p.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 %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 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 header time, id %s, seq %i", ps2->world.octomap.header.frame_id.c_str(), ps2->world.octomap.header.seq);
ROS_INFO("world om Data"); ROS_INFO("world om Data");
...@@ -408,11 +424,24 @@ void Moveit_mediator::setup_task(){ ...@@ -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){ void Moveit_mediator::merge_ps(moveit_msgs::PlanningScene out, moveit_msgs::PlanningScene* in, Moveit_robot* mr){
for (auto acm : merge->allowed_collision_matrix.default_entry_names){ /*
// dafd 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){ void Moveit_mediator::parallel_exec(Moveit_robot& mr, moveit_msgs::RobotTrajectory rt, moveit_msgs::PlanningScene ps){
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment