diff --git a/.catkin_tools/profiles/default/devel_collisions.txt b/.catkin_tools/profiles/default/devel_collisions.txt index cfc16c0b608b2ee28bcf67b6ccfbed7c153c4bd7..d9a9bc8c771fcb9d5dd6d9dbcdf9924cbb791b1e 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 9409 +/home/matteo/reachability/devel/./cmake.lock 10354 /home/matteo/reachability/devel/lib/libmoveit_grasps.so 79 /home/matteo/reachability/devel/lib/libmoveit_grasps_filter.so 79 diff --git a/src/.vscode/c_cpp_properties.json b/src/.vscode/c_cpp_properties.json index b8cbfe4c2f0e734868ebbe3992df094c146a68d2..060596d6214c5aa77b28e1fc301552d48a0df3b1 100644 --- a/src/.vscode/c_cpp_properties.json +++ b/src/.vscode/c_cpp_properties.json @@ -1,41 +1,42 @@ { - "configurations": [ - { - "browse": { - "databaseFilename": "", - "limitSymbolsToIncludedHeaders": true - }, - "includePath": [ - "/home/matteo/ws_moveit/devel/include/**", - "/opt/ros/noetic/include/**", - "/home/matteo/ws_moveit/src/moveit/moveit_planners/chomp/chomp_motion_planner/include/**", - "/home/matteo/ws_moveit/src/eigen_stl_containers/include/**", - "/home/matteo/ws_moveit/src/geometric_shapes/include/**", - "/home/matteo/ws_moveit/src/moveit/moveit_planners/chomp/chomp_interface/include/**", - "/home/matteo/ws_moveit/src/moveit_resources/prbt_ikfast_manipulator_plugin/include/**", - "/home/matteo/ws_moveit/src/moveit/moveit_ros/benchmarks/include/**", - "/home/matteo/ws_moveit/src/moveit/moveit_plugins/moveit_ros_control_interface/include/**", - "/home/matteo/ws_moveit/src/moveit/moveit_ros/move_group/include/**", - "/home/matteo/ws_moveit/src/moveit/moveit_ros/occupancy_map_monitor/include/**", - "/home/matteo/ws_moveit/src/moveit/moveit_ros/robot_interaction/include/**", - "/home/matteo/ws_moveit/src/moveit/moveit_ros/moveit_servo/include/**", - "/home/matteo/ws_moveit/src/moveit/moveit_setup_assistant/include/**", - "/home/matteo/ws_moveit/src/moveit/moveit_plugins/moveit_simple_controller_manager/include/**", - "/home/matteo/ws_moveit/src/moveit_visual_tools/include/**", - "/home/matteo/ws_moveit/src/octomap_msgs/include/**", - "/home/matteo/ws_moveit/src/moveit/moveit_planners/pilz_industrial_motion_planner/include/**", - "/home/matteo/ws_moveit/src/moveit/moveit_planners/pilz_industrial_motion_planner_testutils/include/**", - "/home/matteo/ws_moveit/src/pybind11_catkin/include/**", - "/home/matteo/ws_moveit/src/random_numbers/include/**", - "/home/matteo/ws_panda/src/reachability/include/**", - "/home/matteo/ws_moveit/src/rosparam_shortcuts/include/**", - "/home/matteo/ws_moveit/src/rviz_visual_tools/include/**", - "/home/matteo/ws_moveit/src/srdfdom/include/**", - "/home/matteo/ws_moveit/src/warehouse_ros/include/**", - "/usr/include/**" - ], - "name": "ROS" - } - ], - "version": 4 + "configurations": [ + { + "browse": { + "databaseFilename": "", + "limitSymbolsToIncludedHeaders": true + }, + "includePath": [ + "/home/matteo/ws_moveit/devel/include/**", + "/opt/ros/noetic/include/**", + "/home/matteo/ws_moveit/src/moveit/moveit_planners/chomp/chomp_motion_planner/include/**", + "/home/matteo/ws_moveit/src/eigen_stl_containers/include/**", + "/home/matteo/ws_moveit/src/geometric_shapes/include/**", + "/home/matteo/ws_moveit/src/moveit/moveit_planners/chomp/chomp_interface/include/**", + "/home/matteo/ws_moveit/src/moveit_resources/prbt_ikfast_manipulator_plugin/include/**", + "/home/matteo/ws_moveit/src/moveit/moveit_ros/benchmarks/include/**", + "/home/matteo/ws_moveit/src/moveit/moveit_plugins/moveit_ros_control_interface/include/**", + "/home/matteo/ws_moveit/src/moveit/moveit_ros/move_group/include/**", + "/home/matteo/ws_moveit/src/moveit/moveit_ros/occupancy_map_monitor/include/**", + "/home/matteo/ws_moveit/src/moveit/moveit_ros/robot_interaction/include/**", + "/home/matteo/ws_moveit/src/moveit/moveit_ros/moveit_servo/include/**", + "/home/matteo/ws_moveit/src/moveit/moveit_setup_assistant/include/**", + "/home/matteo/ws_moveit/src/moveit/moveit_plugins/moveit_simple_controller_manager/include/**", + "/home/matteo/ws_moveit/src/moveit_visual_tools/include/**", + "/home/matteo/ws_moveit/src/octomap_msgs/include/**", + "/home/matteo/ws_moveit/src/moveit/moveit_planners/pilz_industrial_motion_planner/include/**", + "/home/matteo/ws_moveit/src/moveit/moveit_planners/pilz_industrial_motion_planner_testutils/include/**", + "/home/matteo/ws_moveit/src/pybind11_catkin/include/**", + "/home/matteo/ws_moveit/src/random_numbers/include/**", + "/home/matteo/ws_panda/src/reachability/include/**", + "/home/matteo/ws_moveit/src/rosparam_shortcuts/include/**", + "/home/matteo/ws_moveit/src/rviz_visual_tools/include/**", + "/home/matteo/ws_moveit/src/srdfdom/include/**", + "/home/matteo/ws_moveit/src/warehouse_ros/include/**", + "/usr/include/**", + "${workspaceFolder}/mtc/include" + ], + "name": "ROS" + } + ], + "version": 4 } \ No newline at end of file diff --git a/src/.vscode/settings.json b/src/.vscode/settings.json index 54461b2903b674b2df2278896f09b579cc1e41c5..299c8d481c8cd286e9e0768bff3de4d3cce6f730 100644 --- a/src/.vscode/settings.json +++ b/src/.vscode/settings.json @@ -8,5 +8,76 @@ "/home/matteo/ws_panda/devel/lib/python3/dist-packages", "/home/matteo/ws_moveit/devel/lib/python3/dist-packages", "/opt/ros/noetic/lib/python3/dist-packages" - ] + ], + "files.associations": { + "cctype": "cpp", + "clocale": "cpp", + "cmath": "cpp", + "csignal": "cpp", + "cstdarg": "cpp", + "cstddef": "cpp", + "cstdio": "cpp", + "cstdlib": "cpp", + "cstring": "cpp", + "ctime": "cpp", + "cwchar": "cpp", + "cwctype": "cpp", + "array": "cpp", + "atomic": "cpp", + "strstream": "cpp", + "bit": "cpp", + "*.tcc": "cpp", + "bitset": "cpp", + "chrono": "cpp", + "complex": "cpp", + "condition_variable": "cpp", + "cstdint": "cpp", + "deque": "cpp", + "forward_list": "cpp", + "list": "cpp", + "map": "cpp", + "set": "cpp", + "unordered_map": "cpp", + "unordered_set": "cpp", + "vector": "cpp", + "exception": "cpp", + "algorithm": "cpp", + "functional": "cpp", + "iterator": "cpp", + "memory": "cpp", + "memory_resource": "cpp", + "numeric": "cpp", + "optional": "cpp", + "random": "cpp", + "ratio": "cpp", + "regex": "cpp", + "string": "cpp", + "string_view": "cpp", + "system_error": "cpp", + "tuple": "cpp", + "type_traits": "cpp", + "utility": "cpp", + "hash_map": "cpp", + "hash_set": "cpp", + "fstream": "cpp", + "initializer_list": "cpp", + "iomanip": "cpp", + "iosfwd": "cpp", + "iostream": "cpp", + "istream": "cpp", + "limits": "cpp", + "mutex": "cpp", + "new": "cpp", + "ostream": "cpp", + "shared_mutex": "cpp", + "sstream": "cpp", + "stdexcept": "cpp", + "streambuf": "cpp", + "thread": "cpp", + "cfenv": "cpp", + "cinttypes": "cpp", + "typeindex": "cpp", + "typeinfo": "cpp", + "variant": "cpp" + } } \ No newline at end of file diff --git a/src/mtc/include/impl/abstract_map_loader.h b/src/mtc/include/impl/abstract_map_loader.h index fbdbe8ebfe465c22b4d4669e60253f6c9c55d4c5..dfb04241392a4804a5fc003b0cdac8ff814e4f56 100644 --- a/src/mtc/include/impl/abstract_map_loader.h +++ b/src/mtc/include/impl/abstract_map_loader.h @@ -26,18 +26,18 @@ class Abstract_map_loader{ Abstract_map_loader() = default; ~Abstract_map_loader() = default; - virtual void base_calculation()=0; + virtual std::vector<std::vector<pcl::PointXYZ>> base_calculation()=0; - inline std::vector<tf2::Transform> inv_map() {return inv_map_;} - inline std::vector<tf2::Transform> map() {return map_;} + inline std::vector<tf2::Transform>& inv_map() {return inv_map_;} + inline std::vector<tf2::Transform>& map() {return map_;} inline void set_inv_map(std::vector<tf2::Transform>& list) {inv_map_ = list;} inline void set_map(std::vector<tf2::Transform>& list) {map_ = list;} inline void set_task_grasps(std::vector<std::vector<tf2::Transform>>& lists_in_list) {task_grasps_ = lists_in_list;} - inline std::vector<std::vector<tf2::Transform>> task_grasps() {return task_grasps_;} - inline std::vector<std::vector<std::vector<tf2::Quaternion>>> target_rot() {return target_rot_;} + inline std::vector<std::vector<tf2::Transform>>& task_grasps() {return task_grasps_;} + inline std::vector<std::vector<std::vector<tf2::Quaternion>>>& target_rot() {return target_rot_;} inline void set_target_rot (std::vector<std::vector<std::vector<tf2::Quaternion>>>& rots) {target_rot_ = rots;} inline void set_target_cloud(std::vector<std::vector<pcl::PointCloud< pcl::PointXYZ >::Ptr>>& cloud) {target_cloud_ = cloud;} - inline std::vector<std::vector<pcl::PointCloud< pcl::PointXYZ >::Ptr>> target_cloud () { return target_cloud_;} + inline std::vector<std::vector<pcl::PointCloud< pcl::PointXYZ >::Ptr>>& target_cloud () { return target_cloud_;} inline void set_strategy(Abstract_strategy* some_stratergy) {strategy_ = some_stratergy;} inline Abstract_strategy* strategy() {return strategy_;} diff --git a/src/mtc/include/impl/abstract_mediator.h b/src/mtc/include/impl/abstract_mediator.h index c9849ca7ebf4cc32f334d7d68156aec7cece313a..eae099d568d03f4499689f6cd928cdee33082822 100644 --- a/src/mtc/include/impl/abstract_mediator.h +++ b/src/mtc/include/impl/abstract_mediator.h @@ -6,6 +6,10 @@ #include "impl/abstract_robot_element.h" #include "impl/robot.h" +#include "octomap/octomap.h" +#include <pcl/point_cloud.h> +#include <pcl/octree/octree.h> + #define box_size tf2::Vector3(0.0318f, 0.0636f, 0.091f) class Abstract_mediator{ @@ -14,16 +18,38 @@ class Abstract_mediator{ std::vector<std::vector<tf2::Transform>> objects_; std::vector<std::vector<std::vector<tf2::Transform>>> relative_bounds_; ros::Publisher* pub_; + std::vector<std::vector<pcl::PointXYZ>> result_vector_; + std::vector<Abstract_robot_element*> wings_; + public: - Abstract_mediator(std::vector<std::vector<tf2::Transform>> objects, ros::Publisher* pub) : objects_(objects), pub_(pub){} - inline void connect_robots(Abstract_robot* robot) { - robots_.push_back(robot); ROS_INFO("%s connected...", robot->name().c_str()); + Abstract_mediator(std::vector<std::vector<tf2::Transform>> objects, ros::Publisher* pub) : objects_(objects), pub_(pub){ + wings_.push_back(new Wing(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0.6525f,0.4425f -0.005f)), right)); + wings_.push_back(new Wing(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,-0.6525f,0.4425f -0.005f)), left)); + wings_.push_back(new Wing(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0.6525f,0,0.4425f-0.005f)), mid)); + } + 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<pcl::PointXYZ>>& result_vector() {return result_vector_;} + 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); + void add_wing(Robot* robot) { + if (robot->observers().size() == 0){ + Abstract_robot_element* rviz_right = new Wing_rviz_decorator(wings_[0], robot->rviz_markers()[0]); + robot->register_observers(rviz_right); + } else if (robot->observers().size() == 1){ + Abstract_robot_element* rviz_left = new Wing_rviz_decorator(wings_[1], robot->rviz_markers()[0]); + robot->register_observers(rviz_left); + } else if (robot->observers().size() == 2){ + Abstract_robot_element* rviz_mid = new Wing_rviz_decorator(wings_[2], robot->rviz_markers()[0]); + robot->register_observers(rviz_mid); + } else { + ROS_INFO("[Warning] %s got to many wings", robot->name().c_str()); + } } - - virtual void check_collision()=0; + virtual bool check_collision( const int& robot)=0; virtual void mediate()=0; }; diff --git a/src/mtc/include/impl/abstract_robot.h b/src/mtc/include/impl/abstract_robot.h index 003c4ec3142499af5c5ce1db5207082cae987aa7..d8ab4d874fe319b91686d42ecc687dd759679e8e 100644 --- a/src/mtc/include/impl/abstract_robot.h +++ b/src/mtc/include/impl/abstract_robot.h @@ -15,20 +15,34 @@ class Abstract_robot { protected: std::string name_; tf2::Transform tf_; + tf2::Transform root_tf_; std::vector<tf2::Transform> bounds_; + std::vector<tf2::Transform> robot_root_bounds_; + public: Abstract_robot(std::string name, tf2::Transform tf) : name_(name), tf_(tf) { + 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()))); // ++ bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-0.4f, 0.4f, tf.getOrigin().getZ()))); // +- bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-0.4f, -0.4f, tf.getOrigin().getZ()))); //-+ bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3( 0.4f, -0.4f, tf.getOrigin().getZ()))); // -- + + robot_root_bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0.113f, 0.095f, 0))); // ++ + robot_root_bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-0.113f, 0.095f, 0))); // +- + robot_root_bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-0.113f, -0.095f, 0))); + robot_root_bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0.113f, -0.095f, 0))); + }; inline std::string& name() { return name_;} inline tf2::Transform& tf() { return tf_;} - inline void rotate(float deg) {tf2::Quaternion rot; rot.setRPY(0,0,deg); tf2::Transform t(rot, tf2::Vector3(0,0,0)); tf_= tf_* t;}; - inline void translate(tf2::Vector3 t) {tf2::Transform tf(tf2::Quaternion(0,0,0,1), t); tf_= tf_* tf;}; + inline tf2::Transform& root_tf() { return root_tf_;} + inline void set_tf(tf2::Transform& t) { tf_ = t;} + inline std::vector<tf2::Transform>& robot_root_bounds() { return robot_root_bounds_;} + inline void rotate(float deg) {tf2::Quaternion rot; rot.setRPY(0,0,deg); tf2::Transform t(rot, tf2::Vector3(0,0,0)); tf_= tf_* t;} + inline void translate(tf2::Vector3 t) {tf2::Transform tf(tf2::Quaternion(0,0,0,1), t); tf_*=tf;} virtual void notify()= 0; virtual bool check_single_object_collision(tf2::Transform& obj)= 0; diff --git a/src/mtc/include/impl/map_loader.h b/src/mtc/include/impl/map_loader.h index cb51695b255834996e608c52bc1ea271ba4b8695..e73ab705b922020788a05ddd088f5e3a4690db34 100644 --- a/src/mtc/include/impl/map_loader.h +++ b/src/mtc/include/impl/map_loader.h @@ -9,7 +9,7 @@ class Map_loader : public Abstract_map_loader { public: Map_loader(XmlRpc::XmlRpcValue& map_data, XmlRpc::XmlRpcValue& target_data); - void base_calculation() override; + std::vector<std::vector<pcl::PointXYZ>> base_calculation() override; }; diff --git a/src/mtc/include/impl/mediator.h b/src/mtc/include/impl/mediator.h index e566d6c7b19a7243455894e10231833d722c6c49..87eacd85b74bb582049b56f373d814999c171c12 100644 --- a/src/mtc/include/impl/mediator.h +++ b/src/mtc/include/impl/mediator.h @@ -8,67 +8,17 @@ class Mediator : public Abstract_mediator{ protected: - visualization_msgs::MarkerArray* rviz_markers_ = new visualization_msgs::MarkerArray(); + 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){}; - void check_collision() override { - //for (int i = 0; i < objects_.size(); i++) { - for(int j = 0; j < objects_[0].size(); j++){ - visualization_msgs::Marker marker; - marker.header.frame_id = "map"; - marker.header.stamp = ros::Time(); - marker.ns = "objects"; - marker.id = j; - marker.type = visualization_msgs::Marker::CUBE; - marker.action = visualization_msgs::Marker::ADD; - marker.pose.position.x = objects_[0][j].getOrigin().getX(); - marker.pose.position.y = objects_[0][j].getOrigin().getY(); - marker.pose.position.z = objects_[0][j].getOrigin().getZ(); - marker.pose.orientation.x = objects_[0][j].getRotation().getX(); - marker.pose.orientation.y = objects_[0][j].getRotation().getY(); - marker.pose.orientation.z = objects_[0][j].getRotation().getZ(); - marker.pose.orientation.w = objects_[0][j].getRotation().getW(); - marker.scale.x = box_size.getX(); - marker.scale.y = box_size.getY(); - marker.scale.z = box_size.getZ(); - if(robots_[0]->check_single_object_collision(objects_[0][j])){ - ROS_INFO("true"); - marker.color.r = 0; - marker.color.g = 1.0; - marker.color.b = 0; - marker.color.a = 1; - } else { - ROS_INFO("false"); - marker.color.r = 1; - marker.color.g = 0; - marker.color.b = 0; - marker.color.a = 1.0; - } - rviz_markers_->markers.push_back(marker); - } - //} - ROS_INFO("alle punkte durch"); - } + bool check_collision(const int& robot) override; + void mediate() override; - void mediate() override { - ros::Rate loop_rate(1); - for (Abstract_robot* robot : robots_){ - Robot* ceti = dynamic_cast<Robot*>(robot); - while (ros::ok()){ - for (visualization_msgs::MarkerArray* markers : ceti->rviz_markers()) markers->markers.clear(); - rviz_markers_->markers.clear(); + 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); + void approximation(Robot* ceti1); - ceti->rotate(-M_PI/4); - ceti->notify(); - check_collision(); - for (visualization_msgs::MarkerArray* markers : ceti->rviz_markers())pub_->publish(*markers); - pub_->publish(*rviz_markers_); - loop_rate.sleep(); - } - } - } }; - #endif \ No newline at end of file diff --git a/src/mtc/include/impl/robot.h b/src/mtc/include/impl/robot.h index d67cb8019d8b1eafc21d461c10ec800be377c871..e4c347187620c820f092be3a9d2d4dfd3f34684d 100644 --- a/src/mtc/include/impl/robot.h +++ b/src/mtc/include/impl/robot.h @@ -20,6 +20,10 @@ class Robot : public Abstract_robot{ public: Robot(std::string name, tf2::Transform tf) : Abstract_robot(name, tf){ + generate_access_fields(); + } + + void generate_access_fields(){ for (int i = 0; i <= 2; i++){ for (int j = 0; j <= 2; j++){ if(i == 0 && j == 0) {continue;} @@ -54,14 +58,25 @@ class Robot : public Abstract_robot{ void notify() override {for(Abstract_robot_element* wing : observers_) wing->update(tf_); for(Abstract_robot_element* field : access_fields_) field->update(tf_);} bool check_single_object_collision(tf2::Transform& obj) override { + + tf2::Transform A = tf_ * root_tf_ * robot_root_bounds_[0]; + tf2::Transform B = tf_ * root_tf_ * robot_root_bounds_[1]; + tf2::Transform C = tf_ * root_tf_ * robot_root_bounds_[2]; + tf2::Transform D = tf_ * root_tf_ * robot_root_bounds_[3]; + + float full_area = area_calculation(A,B,C) + area_calculation(A,D,C); + float sum = area_calculation(A,obj,D) + area_calculation(D,obj,C) + area_calculation(C,obj,B) + area_calculation(obj, B, A); + if ((std::floor(sum*100)/100.f) <= full_area) {return false; } + + for (int i = 0; i < bounds_.size(); i++){ tf2::Transform A = tf_ * bounds_[0]; tf2::Transform B = tf_ * bounds_[1]; tf2::Transform C = tf_ * bounds_[2]; tf2::Transform D = tf_ * bounds_[3]; - float full_area = area_calculation(A,B,C) + area_calculation(A,D,C); - float sum = area_calculation(A,obj,D) + area_calculation(D,obj,C) + area_calculation(C,obj,B) + area_calculation(obj, B, A); + full_area = area_calculation(A,B,C) + area_calculation(A,D,C); + sum = area_calculation(A,obj,D) + area_calculation(D,obj,C) + area_calculation(C,obj,B) + area_calculation(obj, B, A); if ((std::floor(sum*100)/100.f) <= full_area) return true; @@ -77,10 +92,8 @@ class Robot : public Abstract_robot{ tf2::Transform C = ceti->world_tf() * ceti->bounds()[2]; tf2::Transform D = ceti->world_tf() * ceti->bounds()[3]; - float full_area = area_calculation(A,B,C) + area_calculation(A,D,C); - float sum = area_calculation(A,obj,D) + area_calculation(D,obj,C) + area_calculation(C,obj,B) + area_calculation(obj, B, A); - - ROS_INFO("sum %f", sum); + full_area = area_calculation(A,B,C) + area_calculation(A,D,C); + sum = area_calculation(A,obj,D) + area_calculation(D,obj,C) + area_calculation(C,obj,B) + area_calculation(obj, B, A); if ((std::floor(sum*100)/100.f) <= full_area) { return true; @@ -91,6 +104,13 @@ class Robot : public Abstract_robot{ } return false; } + void reset(){ + observers_.clear(); + access_fields_.clear(); + generate_access_fields(); + tf_ = tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0,0.4425f)); + } + inline std::vector<Abstract_robot_element*>& observers() { return observers_;} inline std::vector<Abstract_robot_element*>& access_fields() { return access_fields_;} @@ -133,4 +153,6 @@ class Robot : public Abstract_robot{ }; + + #endif \ No newline at end of file diff --git a/src/mtc/src/base_routine.cpp b/src/mtc/src/base_routine.cpp index 5394ae70714f7389f1e55c0b97a60a7ecbe0accb..1933442b8ecfea7adbe294fd7e27c1d8cb0d1e84 100644 --- a/src/mtc/src/base_routine.cpp +++ b/src/mtc/src/base_routine.cpp @@ -14,13 +14,12 @@ #include "impl/base_by_rotation.h" #include <xmlrpcpp/XmlRpc.h> -#include "impl/map_loader.cpp" +#include "impl/abstract_mediator.cpp" #include "impl/base_by_rotation.cpp" -#include "impl/wing_rviz_decorator.cpp" #include "impl/field_rviz_decorator.cpp" - - - +#include "impl/map_loader.cpp" +#include "impl/mediator.cpp" +#include "impl/wing_rviz_decorator.cpp" int main(int argc, char **argv){ @@ -36,82 +35,51 @@ int main(int argc, char **argv){ Abstract_map_loader* map_loader = new Map_loader(map, task); Abstract_strategy* strategy = new Base_by_rotation(); - - - visualization_msgs::MarkerArray* wings = new visualization_msgs::MarkerArray(); - visualization_msgs::MarkerArray* fields = new visualization_msgs::MarkerArray(); + map_loader->set_strategy(strategy); + std::vector<std::vector<pcl::PointXYZ>> results = map_loader->base_calculation(); - Abstract_robot* robo = new Robot(std::string("Robot_arm1"), tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0,0.4425f))); - Abstract_robot_element* right_wing = new Wing(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0.6525f,robo->tf().getOrigin().getZ()-0.005f)), right); - Abstract_robot_element* left_wing = new Wing(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,-0.6525f,robo->tf().getOrigin().getZ()-0.005f)), left); - Abstract_robot_element* mid_wing = new Wing(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0.6525f,0,robo->tf().getOrigin().getZ()-0.005f)), mid); + Abstract_mediator* mediator = new Mediator(map_loader->task_grasps(), new ros::Publisher(n.advertise< visualization_msgs::MarkerArray >("visualization_marker_array", 1))); + mediator->set_result_vector(results); + + visualization_msgs::MarkerArray* wings1 = new visualization_msgs::MarkerArray(); + visualization_msgs::MarkerArray* wings2 = new visualization_msgs::MarkerArray(); + visualization_msgs::MarkerArray* fields1 = new visualization_msgs::MarkerArray(); + visualization_msgs::MarkerArray* fields2 = new visualization_msgs::MarkerArray(); - Abstract_robot_element* rviz_right = new Wing_rviz_decorator(right_wing, wings); - Abstract_robot_element* rviz_left = new Wing_rviz_decorator(left_wing, wings); - Abstract_robot_element* rviz_mid = new Wing_rviz_decorator(mid_wing, wings); + Abstract_robot* robo = new Robot(std::string("Robot_arm1"), tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0,0.4425f))); Robot* ceti = dynamic_cast<Robot*>(robo); - ceti->add_rviz_markers(wings); - ceti->add_rviz_markers(fields); - ceti->register_observers(rviz_right); - ceti->register_observers(rviz_left); - ceti->register_observers(rviz_mid); + ceti->add_rviz_markers(wings1); + ceti->add_rviz_markers(fields1); - /* - for (int i = 0; i < ceti->access_fields().size(); i++){ - ceti->access_fields()[i] = new Field_rviz_decorator(ceti->access_fields()[i], fields); - } + mediator->connect_robots(robo); + + Abstract_robot* robo2 = new Robot(std::string("Robot_arm2"), tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0,0.4425f))); + Robot* ceti2 = dynamic_cast<Robot*>(robo2); + ceti2->add_rviz_markers(wings2); + ceti2->add_rviz_markers(fields2); + + mediator->connect_robots(robo2); + mediator->mediate(); - */ /* - for (int i = 0; i < map_loader->task_grasps()[0].size(); i++) { - Abstract_object* obj = new Abstract_object(tf2::Transform(tf2::Quaternion(0,0,0,1), map_loader->task_grasps()[0][i]), box_size); - - visualization_msgs::Marker marker; - marker.header.frame_id = "map"; - marker.header.stamp = ros::Time(); - marker.ns = *((char*)(&objects1)); - marker.id = *((int*)(&obj)); - marker.type = visualization_msgs::Marker::CUBE; - marker.action = visualization_msgs::Marker::ADD; - marker.pose.position.x = map_loader->task_grasps()[0][i].getX(); - marker.pose.position.y = map_loader->task_grasps()[0][i].getY(); - marker.pose.position.z = map_loader->task_grasps()[0][i].getZ() +0.025f; - marker.pose.orientation.x = 0; - marker.pose.orientation.y = 0; - marker.pose.orientation.z = 0; - marker.pose.orientation.w = 1; - marker.scale.x = box_size.getX(); - marker.scale.y = box_size.getY(); - marker.scale.z = box_size.getZ(); - marker.color.r = 1.0; - marker.color.g = 0.0; - marker.color.b = 0.0; - marker.color.a = 1.0; - objects1->markers.push_back(marker); - objects.push_back(obj); + for (int i = 0; i < ceti->access_fields1().size(); i++){ + ceti->access_fields1()[i] = new Field_rviz_decorator(ceti->access_fields1()[i], fields1); } */ - //map_loader->set_strategy(strategy); - //map_loader->base_calculation(); + //free(rviz_right); + //free(rviz_mid); + //free(rviz_left); - Abstract_mediator* mediator = new Mediator(map_loader->task_grasps(), new ros::Publisher(n.advertise< visualization_msgs::MarkerArray >("visualization_marker_array", 1))); - mediator->connect_robots(robo); - mediator->mediate(); - - free(rviz_right); - free(rviz_mid); - free(rviz_left); - // free(map_loader); - // free(strategy); + free(map_loader); + free(strategy); free(robo); - free(right_wing); - free(left_wing); - free(mid_wing); + + free(robo2); return 0; } \ No newline at end of file diff --git a/src/mtc/src/impl/abstract_mediator.cpp b/src/mtc/src/impl/abstract_mediator.cpp new file mode 100644 index 0000000000000000000000000000000000000000..fc6b6c21a5c92da41d4378c5da7f9f6c4a3b67e0 --- /dev/null +++ b/src/mtc/src/impl/abstract_mediator.cpp @@ -0,0 +1,22 @@ +#include "impl/abstract_mediator.h" + + +std::vector<pcl::PointXYZ> Abstract_mediator::generate_Ground(const tf2::Vector3 origin, const float diameter, float resolution){ + std::vector<pcl::PointXYZ> ground_plane; + + for (float x = origin.getX() - diameter * 1.5; x <= origin.getX() + diameter * 1.5; x += resolution){ + for (float y = origin.getY() - diameter * 1.5; y <= origin.getY() + diameter * 1.5; y += resolution){ + pcl::PointXYZ point(x,y, 0.4425f); + ground_plane.push_back(point); + } + } + return ground_plane; +} + +pcl::PointCloud< pcl::PointXYZ >::Ptr Abstract_mediator::vector_to_cloud(std::vector<pcl::PointXYZ>& vector){ + pcl::PointCloud< pcl::PointXYZ >::Ptr task_voxel(new pcl::PointCloud< pcl::PointXYZ >); + for(pcl::PointXYZ& point : vector) + task_voxel->push_back(point); + + return task_voxel; +} \ No newline at end of file diff --git a/src/mtc/src/impl/base_by_rotation.cpp b/src/mtc/src/impl/base_by_rotation.cpp index 5c656a13065f9a241c6c70c61b7f68a6a9934311..f16328cf5d95ac94ae97fa517e9cbce4a07a76cf 100644 --- a/src/mtc/src/impl/base_by_rotation.cpp +++ b/src/mtc/src/impl/base_by_rotation.cpp @@ -3,25 +3,22 @@ void Base_by_rotation::inv_map_creation(Abstract_map_loader* var) { ROS_INFO("starting base calculation strategy..."); - ROS_INFO("condition based inv_map calculation..."); + ROS_INFO("condition based [inv_map] calculation..."); std::vector<tf2::Transform> trans; for(int i = 0; i < var->map().size(); i++) { for (int x = 0; x < var->target_rot().size(); x++) { for (int y = 0; y < var->target_rot()[x].size(); y++) { for(int p = 0; p < var->target_rot()[x][y].size(); p++){ - if (var->target_rot()[x][y][p].angle(var->map()[i].getRotation()) < 0.436332f) { - ROS_INFO("%f", var->target_rot()[x][y][p].angle(var->map()[i].getRotation())); + if (var->target_rot()[x][y][p].angle(var->map()[i].getRotation()) < 0.349066f) { trans.push_back(tf2::Transform(var->target_rot()[x][y][p], var->map()[i].getOrigin()).inverse()); break; - } - + } } } } - } var->set_inv_map(trans); - ROS_INFO("inv map caculated..."); + ROS_INFO("caculated [inv_map] contains %i entrys...", var->inv_map().size()); }; void Base_by_rotation::cloud_calculation(Abstract_map_loader* var) { @@ -37,19 +34,19 @@ void Base_by_rotation::cloud_calculation(Abstract_map_loader* var) { } // Maybe OpenMP - ROS_INFO("start cloud calculation..."); + ROS_INFO("start [cloud] calculation..."); + tf2::Transform root(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0,0.4425f)); for (int i = 0; i < target_clouds.size(); i++){ for (int j = 0; j < target_clouds[i].size(); j++) { for (int y = 0; y < var->inv_map().size(); y++) { - //ROS_INFO("%li %li", y, j ); for (int x = 0; x < var->target_rot()[i][j].size(); x++) { tf2::Transform target = var->task_grasps()[i][j]; target.setRotation(var->target_rot()[i][j][x]); - tf2::Transform base = target * var->inv_map()[y]; + tf2::Transform base = target * (var->inv_map()[y] * root); target_clouds[i][j]->push_back(pcl::PointXYZ(base.getOrigin().getX(), base.getOrigin().getY(), base.getOrigin().getZ())); } } } } - ROS_INFO("calculation done..."); var->set_target_cloud(target_clouds); + ROS_INFO("[cloud]calculation done..."); } \ No newline at end of file diff --git a/src/mtc/src/impl/map_loader.cpp b/src/mtc/src/impl/map_loader.cpp index c8466eda75e4f7cd2f82309ddbb77ce351bb0b14..268208bbbce9cf81fb03af505c95bcc16eedaced 100644 --- a/src/mtc/src/impl/map_loader.cpp +++ b/src/mtc/src/impl/map_loader.cpp @@ -8,8 +8,7 @@ Map_loader::Map_loader(XmlRpc::XmlRpcValue& map, XmlRpc::XmlRpcValue& task){ std::regex reg("\\s+"); std::vector<tf2::Transform> _map; - ROS_INFO("template construction..."); - ROS_INFO("load map data..."); + ROS_INFO("load [map] data..."); for (int i = 0; i < map.size();i++){ std::string str = map[i]; std::sregex_token_iterator to_it(str.begin(), str.end(),reg,-1); @@ -20,11 +19,9 @@ Map_loader::Map_loader(XmlRpc::XmlRpcValue& map, XmlRpc::XmlRpcValue& task){ _map.push_back(tf2::Transform(so.normalize(),t)); } this->set_map(_map); + ROS_INFO("[map] saved with %i entries...", _map.size()); - - ROS_INFO("map saved..."); - - ROS_INFO("saving tasks..."); + ROS_INFO("saving [target] positions..."); std::vector<std::vector<tf2::Transform>> task_; for(XmlRpc::XmlRpcValue::ValueStruct::const_iterator it = task.begin(); it != task.end(); ++it){ std::vector<tf2::Transform> task_per_robot; @@ -36,11 +33,12 @@ Map_loader::Map_loader(XmlRpc::XmlRpcValue& map, XmlRpc::XmlRpcValue& task){ } task_.push_back(task_per_robot); } - task_grasps_ = task_; - ROS_INFO("targets saved..."); + this->set_task_grasps(task_); + + for (int i = 0; i < task_.size(); i++) ROS_INFO("[target] for Robot %i saved with %i entries...", i, task_[i].size()); } -void Map_loader::base_calculation(){ +std::vector<std::vector<pcl::PointXYZ>> Map_loader::base_calculation(){ ROS_INFO("calculating target orientation basic set..."); std::vector<tf2::Quaternion> basic_rot; @@ -79,7 +77,7 @@ void Map_loader::base_calculation(){ ROS_INFO("start cloud quantization..."); for(int i = 0; i < target_cloud_.size();i++){ for(int j = 0; j < target_cloud_[i].size();j++){ - pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(0.4f); + pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(0.2f); octree.setInputCloud(target_cloud_[i][j]); octree.addPointsFromInputCloud(); double min_x, min_y, min_z, max_x, max_y, max_z; @@ -95,20 +93,37 @@ void Map_loader::base_calculation(){ } } } - for(int i = 0; i < base_target_map.size(); i++) for(int j = 0; j < base_target_map[i].size(); j++) - for(int k = 0; k < base_target_map[i][j].size(); k++) ROS_INFO("%i ", base_target_map[i][j][k]); + + + + + std::vector<std::vector<pcl::PointXYZ>> resulting; + for(int i = 0; i < base_target_map.size(); i++) { + std::vector<pcl::PointXYZ> points_per_robot; + for(int j = 0; j < base_target_map[i].size(); j++){ + if (base_target_map[i][j].size() == task_grasps_[i].size()) { + points_per_robot.push_back(voxelization[j]); + } + } + if (!points_per_robot.empty()) resulting.push_back(points_per_robot); + } + + for (int i = 0; i < resulting.size(); i++) { + ROS_INFO("Robot %i got %i base positions to ckeck", i, resulting[i].size()); + } + return resulting; } std::vector<pcl::PointXYZ> Abstract_map_loader::create_pcl_box(){ tf2::Vector3 origin(0,0,0); - float resolution = 0.125f; + float resolution = 0.4f; float diameter = 3.0f; unsigned char depth = 16; std::vector<pcl::PointXYZ> box; octomap::OcTree* tree = new octomap::OcTree(resolution/2); - for (float x = origin.getX() - diameter * 1.5 ; x <= origin.getX() + diameter * 1.5 ; x += resolution){ - for (float y = origin.getY() - diameter * 1.5 ; y <= origin.getY() + diameter * 1.5 ; y += resolution){ + for (float x = origin.getX() - diameter * 5 ; x <= origin.getX() + diameter * 5 ; x += resolution){ + for (float y = origin.getY() - diameter * 5 ; y <= origin.getY() + diameter * 5 ; y += resolution){ for (float z = origin.getZ() - diameter * 1.5 ; z <= origin.getZ() + diameter * 1.5 ; z += resolution){ octomap::point3d point(x,y,z); tree->updateNode(point, true); @@ -123,3 +138,4 @@ std::vector<pcl::PointXYZ> Abstract_map_loader::create_pcl_box(){ return box; } + diff --git a/src/mtc/src/impl/mediator.cpp b/src/mtc/src/impl/mediator.cpp new file mode 100644 index 0000000000000000000000000000000000000000..90a54184effe09e1a3a50b1575dea952d72b2ac4 --- /dev/null +++ b/src/mtc/src/impl/mediator.cpp @@ -0,0 +1,163 @@ +#include "impl/mediator.h" +#include "impl/abstract_mediator.h" + + + +bool Mediator::check_collision( const int& robot){ + bool succ = true; + for(int j = 0; j < objects_[robot].size(); j++){ + visualization_msgs::Marker marker; + marker.header.frame_id = "map"; + marker.header.stamp = ros::Time(); + marker.ns = "objects "; + marker.id = j; + marker.type = visualization_msgs::Marker::CUBE; + marker.action = visualization_msgs::Marker::ADD; + marker.pose.position.x = objects_[robot][j].getOrigin().getX(); + marker.pose.position.y = objects_[robot][j].getOrigin().getY(); + marker.pose.position.z = objects_[robot][j].getOrigin().getZ(); + marker.pose.orientation.x = objects_[robot][j].getRotation().getX(); + marker.pose.orientation.y = objects_[robot][j].getRotation().getY(); + marker.pose.orientation.z = objects_[robot][j].getRotation().getZ(); + marker.pose.orientation.w = objects_[robot][j].getRotation().getW(); + marker.scale.x = box_size.getX(); + marker.scale.y = box_size.getY(); + marker.scale.z = box_size.getZ(); + if(robots_[robot]->check_single_object_collision(objects_[robot][j])){ + marker.color.r = 0; + marker.color.g = 1.0; + marker.color.b = 0; + marker.color.a = 1; + } else { + marker.color.r = 1; + marker.color.g = 0; + marker.color.b = 0; + marker.color.a = 1.0; + succ = false; + } + rviz_markers_->markers.push_back(marker); + } + return succ; + +} + +void Mediator::mediate(){ + ROS_INFO("assigne result to first robot..."); + std::vector<pcl::PointXYZ> grCenter = Abstract_mediator::generate_Ground(tf2::Vector3(0,0,0), 3.0f, 0.1f); + std::vector<std::vector<tf2::Transform>> filter_per_robot; + + pcl::octree::OctreePointCloudSearch< pcl::PointXYZ >first_cloud(0.4f); + first_cloud.setInputCloud(Abstract_mediator::vector_to_cloud(result_vector_[0])); + first_cloud.addPointsFromInputCloud(); + std::vector<tf2::Transform> ground_per_robot; + for(pcl::PointXYZ& p : grCenter){ + double min_x, min_y, min_z, max_x, max_y, max_z; + first_cloud.getBoundingBox(min_x, min_y, min_z, max_x, max_y, max_z); + bool isInBox = (p.x >= min_x && p.x <= max_x) && (p.y >= min_y && p.y <= max_y) && (p.z >= min_z && p.z <= max_z); + if (isInBox && first_cloud.isVoxelOccupiedAtPoint(p)) { + std::vector< int > pointIdxVec; + if (first_cloud.voxelSearch(p, pointIdxVec)) ground_per_robot.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(p.x, p.y, p.z))); + } + } + + for (int i = objects_[0].size()-1; i > 0; i--){ + if(objects_[0][i].getOrigin().distance(objects_[1].back().getOrigin()) == 0) objects_[1].pop_back(); + } + + /* + for (int i = 0; i < result_vector_.size();i++){ + result_clouds.push_back(pcl::octree::OctreePointCloudSearch< pcl::PointXYZ >(0.4f)); + result_clouds[i].setInputCloud(Abstract_mediator::vector_to_cloud(result_vector_[i])); + result_clouds[i].addPointsFromInputCloud(); + std::vector<tf2::Transform> ground_per_robot; + for(pcl::PointXYZ& p : grCenter){ + double min_x, min_y, min_z, max_x, max_y, max_z; + result_clouds[i].getBoundingBox(min_x, min_y, min_z, max_x, max_y, max_z); + bool isInBox = (p.x >= min_x && p.x <= max_x) && (p.y >= min_y && p.y <= max_y) && (p.z >= min_z && p.z <= max_z); + if (isInBox && result_clouds[i].isVoxelOccupiedAtPoint(p)) { + std::vector< int > pointIdxVec; + if (result_clouds[i].voxelSearch(p, pointIdxVec)) ground_per_robot.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(p.x, p.y, p.z))); + } + } + filter_per_robot.push_back(ground_per_robot); + } + + + ros::Rate loop_rate(10); + for (int x = 0; x < robots_.size(); x++){ + Robot* ceti = dynamic_cast<Robot*>(robots_[x]); + for (int i = 0; i < filter_per_robot[x].size(); i++){ + robots_[x]->set_tf(filter_per_robot[x][i]); + for ( float p = 0; p < 2*M_PI; p += 0.0872665f){ + for (visualization_msgs::MarkerArray* markers : ceti->rviz_markers()) markers->markers.clear(); + rviz_markers_->markers.clear(); + ceti->rotate(0.0872665f); + ceti->notify(); + if (check_collision(x)) { + ROS_INFO("robot %i found its position", x); + + } else { + continue; + } + for (visualization_msgs::MarkerArray* markers : ceti->rviz_markers())pub_->publish(*markers); + pub_->publish(*rviz_markers_); + loop_rate.sleep(); + } + + } + } + */ + + + ros::Rate loop_rate(10); + Robot* ceti1 = dynamic_cast<Robot*>(robots_[0]); + for(int j = 0; j <= 3; j++){ + for (int i = 0; i < ground_per_robot.size(); i++){ + robots_[0]->set_tf(ground_per_robot[i]); + for ( float p = 0; p < 2*M_PI; p += 0.0872665f){ + for (visualization_msgs::MarkerArray* markers : ceti1->rviz_markers()) markers->markers.clear(); + rviz_markers_->markers.clear(); + ceti1->rotate(0.0872665f); + ceti1->notify(); + if (check_collision(0)){ + approximation(ceti1); + } else { + continue; + } + for (visualization_msgs::MarkerArray* markers : ceti1->rviz_markers())pub_->publish(*markers); + pub_->publish(*rviz_markers_); + } + } + add_wing(ceti1); + } +} + +void Mediator::approximation(Robot* robot){ + ros::Rate loop_rate(1); + std::vector<pcl::PointXYZ> relative_ground; + pcl::octree::OctreePointCloudSearch< pcl::PointXYZ > cloud(0.4f); + cloud.setInputCloud(Abstract_mediator::vector_to_cloud(result_vector_[1])); + cloud.addPointsFromInputCloud(); + + Robot* ceti = dynamic_cast<Robot*>(robots_[1]); + for (int i = 0; i <= 3; i++){ + for (Abstract_robot_element* fields : robot->access_fields()) { + ceti->set_tf(fields->world_tf()); + for ( float p = 0; p < 2*M_PI; p += M_PI/2){ + ceti->rotate(M_PI/2); + ceti->notify(); + if (check_collision(1)) { + ROS_INFO("should work"); + } else { + continue; + } + loop_rate.sleep(); + } + //relative_ground.push_back(pcl::PointXYZ(fields->world_tf().getOrigin().getX(), fields->world_tf().getOrigin().getY(), fields->world_tf().getOrigin().getZ())); + } + add_wing(ceti); + } + ceti->reset(); +} + + diff --git a/src/mtc/src/impl/wing_rviz_decorator.cpp b/src/mtc/src/impl/wing_rviz_decorator.cpp index b19e446ba8d980489362e5322316065b46ce01f0..c21e91c680c03f9f4f85aea42da6b3dc4773a576 100644 --- a/src/mtc/src/impl/wing_rviz_decorator.cpp +++ b/src/mtc/src/impl/wing_rviz_decorator.cpp @@ -15,7 +15,7 @@ void Wing_rviz_decorator::input_filter(tf2::Transform& tf) { visualization_msgs::Marker marker; marker.header.frame_id = "map"; marker.header.stamp = ros::Time(); - marker.ns = "some name"; + marker.ns = "ceti_table "; marker.id = *((int*)(&markers_)); marker.type = visualization_msgs::Marker::CUBE; marker.action = visualization_msgs::Marker::ADD; @@ -44,7 +44,7 @@ void Wing_rviz_decorator::output_filter() { visualization_msgs::Marker marker; marker.header.frame_id = "map"; marker.header.stamp = ros::Time(); - marker.ns = "wings and robo"; + marker.ns = "wings and robo "; marker.id = *((int*)(&next_)); marker.type = visualization_msgs::Marker::CUBE; marker.action = visualization_msgs::Marker::ADD;