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

searching for double planning solutions

parent 03cda6f4
Branches
No related tags found
No related merge requests found
Pipeline #14949 failed
Showing
with 80 additions and 41 deletions
/home/matteo/ws_panda/devel/./cmake.lock 42
/home/matteo/reachability/devel/./cmake.lock 17431
/home/matteo/reachability/devel/./cmake.lock 21138
/home/matteo/reachability/devel/lib/libmoveit_grasps.so 79
/home/matteo/reachability/devel/lib/libmoveit_grasps_filter.so 79
......@@ -35,11 +35,14 @@
<build_depend>moveit_msgs</build_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>YAML_CPP</build_depend>
<build_depend>filesystem</build_depend>
<build_depend>pcl_ros</build_depend>
<build_depend>yaml_to_mtc</build_depend>
<run_depend>filesystem</run_depend>
<run_depend>YAML_CPP</run_depend>
<run_depend>pcl_ros</run_depend>
<run_depend>std_msgs</run_depend>
......
......@@ -35,6 +35,7 @@
<exec_depend>interactive_markers</exec_depend>
<exec_depend>eigen_conversions</exec_depend>
<exec_depend>YAML_CPP</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
......
......@@ -41,7 +41,9 @@
"${workspaceFolder}/moveit_task_constructor/core/include",
"${workspaceFolder}/moveit_task_constructor/"
],
"name": "ROS"
"name": "ROS",
"cStandard": "c17",
"cppStandard": "c++17"
}
],
"version": 4
......
......@@ -79,6 +79,8 @@
"typeindex": "cpp",
"typeinfo": "cpp",
"variant": "cpp",
"any": "cpp"
"any": "cpp",
"filesystem": "cpp",
"codecvt": "cpp"
}
}
\ No newline at end of file
cmake_minimum_required(VERSION 3.0.2)
project(mtc)
# C++ 11
add_compile_options(-std=c++14)
add_compile_options(-std=c++17)
# Warnings
add_definitions(-W -Wall -Wextra
......
......@@ -33,7 +33,8 @@ class Abstract_mediator {
std::vector<std::vector<std::vector<tf2::Transform>>> relative_bounds_;
ros::Publisher* pub_;
std::vector<std::vector<pcl::PointXYZ>> result_vector_;
std::vector<std::vector<wing_BP>> wings_;
std::vector<std::vector<Abstract_robot_element*>> wings_;
std::string dirname_;
......@@ -41,25 +42,24 @@ class Abstract_mediator {
public:
Abstract_mediator(std::vector<std::vector<tf2::Transform>> objects, ros::Publisher* pub) : objects_(objects), pub_(pub){
std::vector<wing_BP> wings;
wings.push_back({"right_panel", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0.6525f,0.4425f -0.005f)), right_size});
wings.push_back({"right_panel", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0.6525f,0.4425f -0.005f)), right_size});
wings.push_back({"right_panel", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0.6525f,0.4425f -0.005f)), right_size});
wings_.push_back(wings);
wings_.push_back(wings);
}
inline void connect_robots(Abstract_robot* robot) {robots_.push_back(robot); ROS_INFO("%s connected...", robot->name().c_str());}
inline void set_result_vector(std::vector<std::vector<pcl::PointXYZ>>& res) {result_vector_ = res;}
inline std::vector<std::vector<wing_BP>> wings() {return wings_;}
inline void set_wings(std::vector<std::vector<wing_BP>>& w) {wings_ = w;}
inline std::vector<std::vector<Abstract_robot_element*>> wings() {return wings_;}
inline std::vector<std::vector<pcl::PointXYZ>>& result_vector() {return result_vector_;}
inline std::vector<Abstract_robot*> robots(){return robots_;}
inline void set_dirname(std::string& dirn) {dirname_ = dirn;}
inline std::string& dirname() {return dirname_;}
pcl::PointCloud< pcl::PointXYZ >::Ptr vector_to_cloud(std::vector<pcl::PointXYZ>& vector);
std::vector<pcl::PointXYZ> generate_Ground(const tf2::Vector3 origin, const float diameter, float resolution);
virtual void set_wings(std::vector<std::vector<wing_BP>>& wbp)=0;
virtual bool check_collision( const int& robot)=0;
virtual void mediate()=0;
virtual void build_wings(std::bitset<3>& wing, int& robot)=0;
......
......@@ -35,7 +35,7 @@ class Abstract_robot {
public:
Abstract_robot(std::string name, tf2::Transform tf, tf2::Vector3 size) : name_(name), tf_(tf), size_(size){
Abstract_robot(std::string& name, tf2::Transform tf, tf2::Vector3 size) : name_(name), tf_(tf), size_(size){
root_tf_= tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-0.22f, 0, tf.getOrigin().getZ()));
bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3( 0.4f, 0.4f, tf.getOrigin().getZ()))); // ++
......
......@@ -10,9 +10,6 @@
#include "impl/robot.h"
class Mediator : public Abstract_mediator{
protected:
visualization_msgs::MarkerArray* rviz_markers_ = new visualization_msgs::MarkerArray;
public:
Mediator(std::vector<std::vector<tf2::Transform>> objects, ros::Publisher* pub) : Abstract_mediator(objects, pub){};
......@@ -20,8 +17,12 @@ class Mediator : public Abstract_mediator{
std::vector<pcl::PointXYZ> generate_Ground(const tf2::Vector3 origin, const float diameter, float resolution);
void approximation(Robot* robot);
void write_file(Robot* A, Robot* B);
void publish(Robot* r);
void setup_rviz();
void calculate(std::vector<tf2::Transform>& ground_per_robot);
void set_wings(std::vector<std::vector<wing_BP>>& wbp) override;
bool check_collision(const int& robot) override;
void mediate() override;
void build_wings(std::bitset<3>& wings, int& robot) override;
......
......@@ -23,6 +23,7 @@ class Moveit_mediator : public Abstract_mediator{
robot_model_loader::RobotModelLoader* robot_model_loader_;
robot_model::RobotModelPtr kinematic_model_;
planning_scene::PlanningScene* ps_;
XmlRpc::XmlRpcValue properties_;
public:
Moveit_mediator(std::vector<std::vector<tf2::Transform>> objects, ros::Publisher* pub, ros::NodeHandle nh) : Abstract_mediator(objects, pub), nh_(nh){
......@@ -30,13 +31,17 @@ class Moveit_mediator : public Abstract_mediator{
planning_scene_diff_publisher_ = nh.advertise<moveit_msgs::PlanningScene>("planning_scene", 1);
};
void setup_task();
bool check_collision(const int& robot) override;
void mediate() override;
void build_wings(std::bitset<3>& wing,int& robot) override;
void set_wings(std::vector<std::vector<wing_BP>>& wbp) override;
void publish_tables(moveit::planning_interface::PlanningSceneInterface& psi);
void load_robot_description();
void rewrite_task_template(Abstract_robot* r, moveit_msgs::CollisionObject& target);
void rewrite_task_template(Abstract_robot* r, moveit_msgs::CollisionObject& source, tf2::Transform& target);
};
#endif
\ No newline at end of file
......@@ -6,15 +6,37 @@
class Moveit_robot : public Robot{
private:
protected:
moveit::planning_interface::MoveGroupInterface* mgi_;
std::map<std::string, std::pair <std::string, std::string>> map_;
public:
Moveit_robot(std::string name, tf2::Transform tf, tf2::Vector3 size) : Robot(name, tf, size){
Moveit_robot(std::string& name, tf2::Transform tf, tf2::Vector3 size) : Robot(name, tf, size){
mgi_ = new moveit::planning_interface::MoveGroupInterface(name);
ROS_INFO("planning frame: %s", mgi_->getPlanningFrame().c_str());
std::stringstream group, eef, hand_grasping_frame, ik_frame, hand, hand_n, ik_frame_n, name_n;
name_n << name;
group << "group_" << name;
eef << "eef_" << name;
hand_grasping_frame << "hand_grasping_frame_" << name;
ik_frame << "ik_frame_" << name;
hand << "hand_" << name;
hand_n << "hand_" << name.back();
ik_frame_n << "panda_" << name.back() << "_link8";
map_.insert(std::make_pair<std::string, std::pair<std::string, std::string>>("group", std::make_pair<std::string, std::string>(group.str(), name_n.str())));
map_.insert(std::make_pair<std::string, std::pair<std::string, std::string>>("eef", std::make_pair<std::string, std::string>(eef.str(), hand_n.str())));
map_.insert(std::make_pair<std::string, std::pair<std::string, std::string>>("hand_grasping_frame", std::make_pair<std::string, std::string>(hand_grasping_frame.str(), ik_frame_n.str())));
map_.insert(std::make_pair<std::string, std::pair<std::string, std::string>>("ik_frame", std::make_pair<std::string, std::string>(ik_frame.str(), ik_frame_n.str())));
map_.insert(std::make_pair<std::string, std::pair<std::string, std::string>>("hand", std::make_pair<std::string, std::string>(hand.str(), hand_n.str())));
}
inline moveit::planning_interface::MoveGroupInterface* mgi() {return mgi_;}
inline std::map<std::string, std::pair <std::string, std::string>>& map(){return map_;};
};
......
......@@ -20,17 +20,12 @@ class Robot : public Abstract_robot{
protected:
std::vector<Abstract_robot_element*> observers_;
std::vector<Abstract_robot_element*> access_fields_;
std::vector<visualization_msgs::MarkerArray*> rviz_markers_;
std::vector<moveit_msgs::CollisionObject*> coll_markers_;
public:
Robot(std::string name, tf2::Transform tf, tf2::Vector3 size) : Abstract_robot(name, tf, size){
generate_access_fields();
}
Robot(std::string& name, tf2::Transform tf, tf2::Vector3 size) : Abstract_robot(name, tf, size){generate_access_fields();}
inline void add_rviz_markers(visualization_msgs::MarkerArray* marker) {rviz_markers_.push_back(marker);}
inline std::vector<visualization_msgs::MarkerArray*> rviz_markers(){return rviz_markers_;}
inline std::vector<Abstract_robot_element*>& observers() { return observers_;}
inline std::vector<Abstract_robot_element*>& access_fields() { return access_fields_;}
......
......@@ -11,16 +11,19 @@ class Wing : public Abstract_robot_element{
std::vector<tf2::Transform> bounds_;
public:
Wing(std::string& name, tf2::Transform& tf, tf2::Vector3& size) : size_(size), name_(name){
Wing(std::string name, tf2::Transform tf, tf2::Vector3 size) : size_(size), name_(name){
relative_tf_ = tf;
bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(size_.getX()/2.f, size_.getY()/2.f,0))); //++
bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(size_.getX()/2.f, size_.getY()/-2.f,0))); //+-
bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(size_.getX()/-2.f, size_.getY()/-2.f,0))); //--
bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(size_.getX()/-2.f, size_.getY()/2.f,0))); //-+
}
virtual ~Wing() {}
inline std::string& name() {return name_;}
inline void set_name(std::string& str) {name_ = str;}
inline void set_name(std::string str) {name_ = str;}
inline void set_set(tf2::Vector3& vec) {size_ = vec;}
inline tf2::Vector3 size(){ return size_;}
inline std::vector<tf2::Transform>& bounds() {return bounds_;}
void update(tf2::Transform& tf) override {this->calc_world_tf(tf);}
......
......@@ -12,11 +12,11 @@
class Wing_moveit_decorator : public Abstract_robot_element_decorator{
private:
moveit_msgs::CollisionObject* markers_;
moveit_msgs::CollisionObject* markers_ = new moveit_msgs::CollisionObject();
public:
Wing_moveit_decorator(Abstract_robot_element* next, moveit_msgs::CollisionObject* markers) : Abstract_robot_element_decorator(next), markers_(markers){};
Wing_moveit_decorator(Abstract_robot_element* next) : Abstract_robot_element_decorator(next){};
inline void set_markers(moveit_msgs::CollisionObject* markers) { markers_= markers;}
inline moveit_msgs::CollisionObject* markers() { return markers_;}
......
......@@ -8,13 +8,13 @@
class Wing_rviz_decorator : public Abstract_robot_element_decorator{
private:
visualization_msgs::MarkerArray* markers_;
visualization_msgs::Marker* marker_ = new visualization_msgs::Marker();
public:
Wing_rviz_decorator(Abstract_robot_element* next, visualization_msgs::MarkerArray* markers) : Abstract_robot_element_decorator(next), markers_(markers){};
inline void set_markers(visualization_msgs::MarkerArray* markers) { markers_= markers;}
inline visualization_msgs::MarkerArray* markers() { return markers_;}
Wing_rviz_decorator(Abstract_robot_element* next) : Abstract_robot_element_decorator(next){};
inline void set_markers(visualization_msgs::Marker* markers) { marker_= markers;}
inline visualization_msgs::Marker* markers() { return marker_;}
void update(tf2::Transform& tf) override;
void input_filter(tf2::Transform& tf) override;
void output_filter() override;
......
<launch>
<rosparam command="load" file="$(find mtc)/maps/dummy.yaml"/>
<rosparam command="load" file="$(find mtc)/descriptions/dummy.yaml"/>
<rosparam command="load" file="$(find mtc)/resources/dummy.yaml"/>
<arg name="map" default="dummy" />
<arg name="description" default="dummy" />
<arg name="resource" default="dummy" />
<rosparam command="load" file="$(find mtc)/maps/$(arg map).yaml"/>
<rosparam command="load" file="$(find mtc)/descriptions/$(arg description).yaml"/>
<rosparam command="load" file="$(find mtc)/resources/$(arg resource).yaml"/>
<rosparam param="resource_name" subst_value="True"> $(arg resource)</rosparam>
<node pkg="mtc" type="base_routine" name="base_routine" output="screen" required="true"/>
</launch>
\ No newline at end of file
<launch>
<!--<include file="$(find panda_moveit_config)/launch/demo.launch"></include> -->
<rosparam command="load" file="$(find mtc)/results/dummy.yaml"/>
<rosparam command="load" file="$(find mtc)/mtc_task_file/dummy.yaml" />
<rosparam command="load" file="$(find mtc)/mtc_templates/dummy.yaml" />
<rosparam command="load" file="$(find mtc)/maps/dummy.yaml"/>
<rosparam command="load" file="$(find mtc)/descriptions/dummy.yaml"/>
......
File moved
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment