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

Initial commit

parents
No related branches found
No related tags found
No related merge requests found
Showing
with 1097 additions and 0 deletions
cmake_minimum_required(VERSION 3.0.2)
project(multi_cell_builder)
# C++ 11
add_compile_options(-std=c++17)
# Load catkin and all dependencies required for this package
find_package(catkin REQUIRED COMPONENTS
moveit_task_constructor_core
moveit_task_constructor_visualization
moveit_task_constructor_msgs
eigen_conversions
geometry_msgs
message_generation
moveit_core
moveit_msgs
moveit_ros_planning
moveit_ros_planning_interface
moveit_visual_tools
roscpp
roslint
rosparam_shortcuts
std_msgs
tf
tf_conversions
trajectory_msgs
pcl_ros
gb_grasp
moveit_grasps
)
find_package(yaml-cpp REQUIRED)
find_package(Protobuf 3 REQUIRED)
include_directories(${CMAKE_CURRENT_BINARY_DIR})
# Catkin
catkin_package(
LIBRARIES
${Protobuf_LIBRARIES}
CATKIN_DEPENDS
moveit_ros_planning_interface
moveit_grasps
INCLUDE_DIRS
${CMAKE_CURRENT_BINARY_DIR}
${CMAKE_CURRENT_SOURCE_DIR}
${Protobuf_INCLUDE_DIRS}
include
DEPENDS
)
###########
## Build ##
###########
include_directories(
include
${catkin_INCLUDE_DIRS}
${PCL_INCLUDE_DIR}
${CMAKE_CURRENT_BINARY_DIR}
${CMAKE_CURRENT_SOURCE_DIR}
)
protobuf_generate_cpp(PROTO_SRCS PROTO_HDRS proto/connector.proto)
add_executable(base_routine src/base_routine.cpp
src/impl/abstract_mediator.cpp
src/impl/base_by_rotation.cpp
src/impl/field_rviz_decorator.cpp
src/impl/map_loader.cpp
src/impl/mediator.cpp
src/impl/moveit_mediator.cpp
src/impl/robot.cpp
src/impl/wing_moveit_decorator.cpp
src/impl/wing_rviz_decorator.cpp
src/impl/collision_helper.cpp
src/reader/abstract_param_reader.cpp
src/reader/robot_reader.cpp
src/reader/wing_reader.cpp
src/reader/cuboid_reader.cpp
)
add_executable(cell_routine src/cell_routine.cpp
src/impl/map_loader.cpp
src/impl/abstract_mediator.cpp
src/impl/moveit_mediator.cpp
src/impl/wing_moveit_decorator.cpp
src/impl/robot.cpp
src/impl/mediator.cpp
src/impl/wing_moveit_decorator.cpp
src/impl/wing_rviz_decorator.cpp
src/impl/collision_helper.cpp
src/reader/abstract_param_reader.cpp
src/reader/robot_reader.cpp
src/reader/wing_reader.cpp
src/reader/cuboid_reader.cpp
)
add_executable(grasp_cell_routine src/grasp_cell_routine.cpp
src/impl/map_loader.cpp
src/impl/abstract_mediator.cpp
src/impl/moveit_mediator.cpp
src/impl/wing_moveit_decorator.cpp
src/impl/robot.cpp
src/impl/mediator.cpp
src/impl/wing_moveit_decorator.cpp
src/impl/wing_rviz_decorator.cpp
src/impl/collision_helper.cpp
src/impl/moveit_grasp_mediator.cpp
src/reader/abstract_param_reader.cpp
src/reader/robot_reader.cpp
src/reader/wing_reader.cpp
src/reader/cuboid_reader.cpp
)
add_executable(mtc2taskspace src/mtc2taskspace.cpp
src/reader/abstract_param_reader.cpp
src/reader/task_space_reader.cpp
src/reader/robot_reader.cpp
)
add_dependencies(base_routine ${base_routine_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_dependencies(cell_routine ${cell_routine_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_dependencies(mtc2taskspace ${mtc2taskspace_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_dependencies(grasp_cell_routine ${grasp_cell_routine_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(base_routine
${catkin_LIBRARIES}
${OCTOMAP_LIBRARIES}
${PCL_LIBRARY_DIRS}
${moveit_grasps_LIBRARIES}
yaml-cpp
)
target_link_libraries(cell_routine
${catkin_LIBRARIES}
${OCTOMAP_LIBRARIES}
${PCL_LIBRARY_DIRS}
${moveit_grasps_LIBRARIES}
yaml-cpp
)
target_link_libraries(grasp_cell_routine
${catkin_LIBRARIES}
${OCTOMAP_LIBRARIES}
${PCL_LIBRARY_DIRS}
${moveit_grasps_LIBRARIES}
yaml-cpp
)
target_link_libraries(mtc2taskspace
${catkin_LIBRARIES}
${OCTOMAP_LIBRARIES}
yaml-cpp
)
# multi_cell_builder
multi_cell_builder is the core package developed with different design patterns to ensure both extensibility and variability.
## executables
multi_cell_builder consists of 3 executables, with each computing .yaml files for the next subsequent module, according the following strcture.
> mtc2taskspace >> base_routine >> cell_routine
### mtc2taskspace
From a bare Ceti-resource file representing an arbitrary scene with specific locations, located in the /resources directory. Mtc2taskspace computes a new file and associates each object with a drop-of-location to a robot. The shared space between robots is expressed by identical location entries for each robot involved. <br/>
multi_cell_builder/mtc/mtc2taskspace.cpp (/resources/{resource}) -> (/descriptions/{resource}) <br/>
roslaunch multi_cell_builder mtc2taskspace.launch resource='dummy.yaml'
### base_routine
Calculates the workspace from a descriptions file (/descriptions/{description}) and a precomputed reachability map (/map/{map}), combined with information of a resource file (/resources/{resource}). Creates the directory (results/{resource}/ros::time.yaml}) with the resulting files as contents. <br/>
multi_cell_builder/src/base_routine.cpp (/map/{map}, /resources/{resource}, /descriptions/{descriptions}) -> (/results/{resource}/ros::time.yaml) <br/>
roslaunch multi_cell_builder base_routine.launch resource='dummy.yaml' map='dummy.yaml' description='dummy.yaml'
> Replace already existing result folder if exists.
>> Resulting files can break the yaml schema, because each robot has an extra rpy element which is important for setting robot pose in franca_description. Also each 8th entry must be a table[0-9]+_table_top id. Cosmetic data (table_wheel...) included as placeholder
### cell_routine
Designed to load a resource file (+ with additional rpy component in the right order) and execute a task with moveit task constructor. A task description can also be the /descriptions/{description} in the example. <br/>
multi_cell_builder/src/cell_routine.cpp (/results/{result}, /descriptions/{task}) -> () <br/>
roslaunch multi_cell_builder base_routine.launch result='/dummy/dummy.yaml' task='dummy.yaml' (is coop) <br/>
roslaunch multi_cell_builder base_routine.launch result='/dummy/dummy.yaml' task='dummy2.yaml' (is parallel)
> Christoph's parser could not be integrated because there is no way to have multiple robots compute a task.
## Current state
State: in progress. Every executable is working with some explained downfalls that should be fixed in the next days. Implementing or omitting ideas because some modules do not work as expected took a lot of time. The actual results (62 files for dummy.yaml) can be loaded and run. I tested 10/62 myself, with surprisingly good results. The following list shows open and closed ToDos.<br/>
:ballot_box_with_check: present a faster work-space execution <br/>
:ballot_box_with_check: generate more solutions for 1 dump file <br/>
:ballot_box_with_check: present solution for general robots <br/>
:ballot_box_with_check: elaborate the moveit task constructor <br/>
:ballot_box_with_check: present more general task files <br/>
:black_square_button: include cosmetic values <br/>
:ballot_box_with_check: passing yaml in xacro (franca_description) <br/>
:ballot_box_with_check: parallel tasks (Docker already downloaded, just have to try) (main focus) <br/>
:black_square_button: fix acm <br/>
:black_square_button: include ferdinands work (main focus)<br/>
Im working non stop, so don't forget to pull. <br/>
[Git Docker](https://github.com/cambel/moveit_simultaneous_motions_demo.git "github link for parallel docker")
[Ferdinand](https://git-st.inf.tu-dresden.de/ceti/ros-internal/technical-tests/gb-grasp-auerswald/-/blob/main/GB_Auerswald.pdf "Ferdi bre")
moveit_grasps:
####################################
# These settings are used by the grasp generator which generates GraspCandidates
####################################
generator:
# Other verbose stuff
verbose: true
# Show hand of generated grasps
show_prefiltered_grasps: true
show_prefiltered_grasps_speed: 0.001 # (sec / grasp)
# --------------------------------
# Finger gripper specific settings
# --------------------------------
# None yet
# --------------------------------
# Suction gripper specific settings
# --------------------------------
# Visual debug options
debug_top_grasps: false
# Show suction gripper overhang
show_grasp_overhang: false
####################################
# The grasp filter filters out unreachable GraspCandidates and populates valid GraspCandidates' pregrasp_ik_solution_ and grasp_ik_solution_'s
####################################
filter:
# Show summary statistics
statistics_verbose: true
# Visualize the planes used by the filter
show_cutting_planes: true
# Collision checking in verbose. Note: this will cause filtering to happen on a single thread rather than in parallel
collision_verbose: true
collision_verbose_speed: 0.01 # (sec/ solution)
# Collision checking in verbose on second pass (after all failed)
show_grasp_filter_collision_if_failed: true
# Show post-filter arrows
show_filtered_grasps: true
# Show pose-filter arm ik solutions
show_filtered_arm_solutions: true
show_filtered_arm_solutions_pregrasp_speed: 0.01 # (sec / solution)
show_filtered_arm_solutions_speed: 0.01 # (sec / solution)
####################################
# The GraspPlanner generates approach, lift and retreat paths for a GraspCandidate.
# If the GraspPlanner is unable to plan 100% of the approach path and at least ~90% of the lift and retreat paths, then it considers the GraspCandidate to be infeasible
####################################
planner:
# Cartesian planning
statistics_verbose: true
verbose_cartesian_filtering: false
show_cartesian_waypoints: false
collision_checking_verbose: false
base_link: 'world'
# =====================================================
# Note: these parameters are copied from Robotiq gripper without tweaking
hand:
end_effector_name: 'hand' # ee group name
end_effector_type: 'finger' # 'finger' or 'suction'
# x/y translation and angle orientation grasp resolution parameters used for sampling grasp candidates
grasp_resolution : 1
angle_resolution : 90 # degrees
# min/max values of the grasp depth range in meter.
# This should fit to the distance from finger tip to inner palm or the suction cup stroke
grasp_max_depth : 0.035
grasp_min_depth : 0.0
# grasp_depth_resolution is the discretization distance in meter used for sampling the grasp depth value.
# Idealy this should be a small fraction of the depth range (i.e. 0.005).
# If the value exceeds the depth distance range grasp_max_depth is used by default.
grasp_depth_resolution : 0.035
# A grasp motion is composed of three linear paths: approach, retreat, lift
# The values below define the desired distances of each path.
# Length of approach path (this is in addition to the grasp_max_depth)
approach_distance_desired: 0.05 # approach path (in addition to grasp_max_depth)
retreat_distance_desired: 0.05 # retreat path (in addition to grasp_max_depth)
lift_distance_desired: 0.01 # lift path (used as MINIMUM safety distance from surface)
# minimum padding on each side of the object on approach
grasp_padding_on_approach: 0.005 # meter
# We need to know the transform from the arm IK link to the grasp point. By default we define the transform manually with
# `tcp_to_eef_mount_transform`. Alternatively, we can set this to a frame in the robot model and compute the transform
# automatically. this is done by setting `define_tcp_by_name: true` and providing a value for `tcp_name`.
#
# Distance from the eef mount to the palm of end effector [x, y, z, r, p, y]
# z-axis pointing toward object to grasp
# x-axis perp. to movement of grippers
# y-axis parallel to movement of grippers
# If we have a tool center point frame, we would set:
define_tcp_by_name: true
tcp_name: "tool_center_point"
# Alternatively, if we cannot add a frame for the tool center point, we can define tcp by transform
# tcp_to_eef_mount_transform : [0, 0, -0.105, 0, 0, -0.7853] # NOTE: Imaginary point in the middle
# Target durations in seconds for reaching pregrasp and grasp positions in end effector motions
pregrasp_time_from_start : 0.0
grasp_time_from_start : 0.0
# Joint names and state values for the end effector.
# The names should match the values of pregrasp_posture and grasp_posture.
# These values can be left empty if end_effector_type is 'suction'
pregrasp_posture : [0.04] # open position
grasp_posture : [0.0] # close position
joints : ['panda_finger_joint1']
##############################
## Finger gripper parameters
##############################
# Distances between fingers matching to pregrasp_posture and grasp_posture, in meters
max_finger_width: 0.085
min_finger_width: 0.06
# Maximum allowed finger width for a grasp.
# This value should be considerably smaller than max_finger_width
# to allow padded collision checks
max_grasp_width: 0.08
# width of gripper fingers to ensure the finger tips fully overlap the object
gripper_finger_width : 0.015
##############################
## Suction gripper parameters
##############################
# The width of the suction gripper along the specified axes. (Assumes that Z points in the direction of the suction cups)
active_suction_range_x : 0.22
active_suction_range_y : 0.12
# Some suction grippers can selectively enable and disable regions of suction. If you have more than one
# region that can be selectively activated or de-activated, set those values here and moveit_grasps
# will prioritize grasp candidates which use full suction regions rather than partial regions.
# it will also set the suction_regions_in_contact_ variable in the grasp candidates so that you are able
# to only enable suction on regions that are in contact with the grasp target.
# Note that this metric assumes that suction regions are equally sized
# ie. width = active_suction_range_x / suction_cols_count
#
suction_rows_count: 2
suction_cols_count: 2
task:
groups:
panda_arm1:
- -0.300000 -0.700000 0.890000
- -0.300000 -0.600000 0.890000
- -0.200000 -0.700000 0.890000
- -0.200000 -0.600000 0.890000
- 0.100000 -0.700000 0.890000
- 0.100000 -0.600000 0.890000
- 0.100000 -0.300000 0.890000
- 0.100000 -0.200000 0.890000
- 0.100000 -0.100000 0.890000
- 0.200000 -0.300000 0.890000
- 0.200000 -0.200000 0.890000
- 0.200000 -0.100000 0.890000
- 0.300000 -0.300000 0.890000
- 0.300000 -0.200000 0.890000
- 0.300000 -0.100000 0.890000
- 0.100000 0.100000 0.890000
- 0.100000 0.200000 0.890000
- 0.100000 0.300000 0.890000
- 0.200000 0.100000 0.890000
- 0.200000 0.200000 0.890000
- 0.200000 0.300000 0.890000
- 0.300000 0.100000 0.890000
- 0.300000 0.200000 0.890000
- 0.300000 0.300000 0.890000
- -0.300000 0.600000 0.890000
- -0.300000 0.700000 0.890000
- -0.200000 0.600000 0.890000
- -0.200000 0.700000 0.890000
- 0.100000 0.600000 0.890000
- 0.100000 0.700000 0.890000
panda_arm2:
- 0.100000 1.010000 0.890000
- 0.100000 1.110000 0.890000
- 0.100000 1.210000 0.890000
- 0.200000 1.010000 0.890000
- 0.200000 1.110000 0.890000
- 0.200000 1.210000 0.890000
- 0.300000 1.010000 0.890000
- 0.300000 1.110000 0.890000
- 0.300000 1.210000 0.890000
- 0.100000 1.410000 0.890000
- 0.100000 1.510000 0.890000
- 0.100000 1.610000 0.890000
- 0.200000 1.410000 0.890000
- 0.200000 1.510000 0.890000
- 0.200000 1.610000 0.890000
- 0.300000 1.410000 0.890000
- 0.300000 1.510000 0.890000
- 0.300000 1.610000 0.890000
- -0.300000 1.910000 0.890000
- -0.300000 2.010000 0.890000
- -0.200000 1.910000 0.890000
- -0.200000 2.010000 0.890000
- 0.100000 1.910000 0.890000
- 0.100000 2.010000 0.890000
- -0.300000 0.600000 0.890000
- -0.300000 0.700000 0.890000
- -0.200000 0.600000 0.890000
- -0.200000 0.700000 0.890000
- 0.100000 0.600000 0.890000
- 0.100000 0.700000 0.890000
\ No newline at end of file
task:
groups:
panda_arm1:
- -0.300000 -0.700000 0.890000
- -0.300000 -0.600000 0.890000
- -0.200000 -0.700000 0.890000
- -0.200000 -0.600000 0.890000
- 0.100000 -0.700000 0.890000
- 0.100000 -0.600000 0.890000
- 0.100000 -0.300000 0.890000
- 0.100000 -0.200000 0.890000
- 0.100000 -0.100000 0.890000
- 0.200000 -0.300000 0.890000
- 0.200000 -0.200000 0.890000
- 0.200000 -0.100000 0.890000
- 0.300000 -0.300000 0.890000
- 0.300000 -0.200000 0.890000
- 0.300000 -0.100000 0.890000
- 0.100000 0.100000 0.890000
- 0.100000 0.200000 0.890000
- 0.100000 0.300000 0.890000
- 0.200000 0.100000 0.890000
- 0.200000 0.200000 0.890000
- 0.200000 0.300000 0.890000
- 0.300000 0.100000 0.890000
- 0.300000 0.200000 0.890000
- 0.300000 0.300000 0.890000
panda_arm2:
- 0.100000 1.110000 0.890000
- 0.100000 1.210000 0.890000
- 0.200000 1.010000 0.890000
- 0.200000 1.110000 0.890000
- 0.200000 1.210000 0.890000
- 0.300000 1.010000 0.890000
- 0.300000 1.110000 0.890000
- 0.300000 1.210000 0.890000
- 0.100000 1.410000 0.890000
- 0.100000 1.510000 0.890000
- 0.100000 1.610000 0.890000
- 0.200000 1.410000 0.890000
- 0.200000 1.510000 0.890000
- 0.200000 1.610000 0.890000
- 0.300000 1.410000 0.890000
- 0.300000 1.510000 0.890000
- 0.300000 1.610000 0.890000
- -0.300000 1.910000 0.890000
- -0.300000 2.010000 0.890000
- -0.200000 1.910000 0.890000
- -0.200000 2.010000 0.890000
- 0.100000 1.910000 0.890000
- 0.100000 2.010000 0.890000
\ No newline at end of file
#ifndef ABSTRACT_MAP_LOADER_
#define ABSTRACT_MAP_LOADER_
#include "ros/ros.h"
#include <xmlrpcpp/XmlRpc.h>
#include <tf2/LinearMath/Transform.h>
#include "octomap/octomap.h"
#include <pcl/point_cloud.h>
#include <pcl/octree/octree.h>
#include <ros/package.h>
#include <yaml-cpp/yaml.h>
#include <fstream>
#include "impl/abstract_robot.h"
#include "impl/abstract_strategy.h"
class Abstract_strategy;
class Abstract_robot;
class Abstract_map_loader{
protected:
std::vector<tf2::Transform> inv_map_;
std::vector<tf2::Transform> map_;
std::vector<std::vector<tf2::Transform>> task_grasps_;
std::vector<std::vector<std::vector<tf2::Quaternion>>> target_rot_;
std::vector<std::vector<pcl::PointCloud< pcl::PointXYZ >::Ptr>> target_cloud_;
Abstract_strategy *strategy_;
public:
Abstract_map_loader() = default;
~Abstract_map_loader() = default;
inline void set_strategy(Abstract_strategy* some_stratergy) {strategy_ = some_stratergy;}
inline Abstract_strategy* strategy() {return strategy_;}
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 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_;}
static std::vector<pcl::PointXYZ> create_pcl_box();
virtual void write_task(Abstract_robot* robot)=0;
virtual std::vector<std::vector<pcl::PointXYZ>> base_calculation()=0;
};
#endif
#ifndef ABSTRACT_MEDIATOR_
#define ABSTRACT_MEDIATOR_
#include <ros/ros.h>
#include "impl/abstract_robot.h"
#include "impl/abstract_robot_element.h"
#include "impl/robot.h"
#include "reader/wing_reader.h"
#include <ros/package.h>
#include <yaml-cpp/yaml.h>
#include <fstream>
#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)
#define R_POS tf2::Vector3(0,0.6525f,0.4425f -0.005f)
#define L_POS tf2::Vector3(0,-0.6525f,0.4425f -0.005f)
#define M_POS tf2::Vector3(0.6525f,0,0.4425f-0.005f)
struct wing_BP {
std::string name_;
tf2::Transform pos_;
tf2::Vector3 size_;
};
class Abstract_mediator {
protected:
std::vector<Abstract_robot*> robots_;
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<std::vector<Abstract_robot_element*>> wings_;
std::string dirname_;
public:
Abstract_mediator(std::vector<std::vector<tf2::Transform>> objects, ros::Publisher* pub) : objects_(objects), pub_(pub){
}
inline void set_result_vector(std::vector<std::vector<pcl::PointXYZ>>& res) {result_vector_ = res;}
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 connect_robots(Abstract_robot* robot)=0;
virtual void set_wings(std::vector<std::pair<std::vector<object_data>, int>>& 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;
};
#endif
\ No newline at end of file
#ifndef ABSTRACT_ROBOT_
#define ABSTRACT_ROBOT_
#include "ros/ros.h"
#include <tf2/LinearMath/Transform.h>
#include "impl/abstract_strategy.h"
#include "impl/abstract_robot_element.h"
#define right_size tf2::Vector3(0.7f, 0.5f, 0.01f)
#define left_size tf2::Vector3(0.7f, 0.5f, 0.01f)
#define mid_size tf2::Vector3(0.5f, 0.7f, 0.01f)
enum wing_config{
RML = 7,
RM_ = 6,
R_L = 5,
R__ = 4,
_ML = 3,
_M_ = 2,
__L = 1,
___ = 0
};
class Abstract_robot {
protected:
std::string name_;
tf2::Vector3 size_;
tf2::Transform tf_;
tf2::Transform root_tf_;
std::vector<tf2::Transform> bounds_;
std::vector<tf2::Transform> robot_root_bounds_;
std::bitset<3> observer_mask_;
public:
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()))); // ++
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)));
observer_mask_ = std::bitset<3>(0);
};
inline std::string& name() { return name_;}
inline tf2::Transform& tf() { return tf_;}
inline tf2::Vector3& size() { return size_;}
inline tf2::Transform& root_tf() { return root_tf_;}
inline std::vector<tf2::Transform>& bounds() {return bounds_;}
inline void size(tf2::Vector3& s) { size_ = s;}
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;}
inline std::bitset<3> observer_mask() {return observer_mask_;}
inline void set_observer_mask(int i) {observer_mask_ = std::bitset<3>(i);}
virtual void notify()= 0;
virtual bool check_single_object_collision(tf2::Transform& obj, std::string& b)= 0;
virtual void workload_checker(std::vector<int>& count_vector, tf2::Transform& obj) =0;
};
#endif
\ No newline at end of file
#ifndef ABSTRACT_ROBOT_ELEMENT_
#define ABSTRACT_ROBOT_ELEMENT_
#include "ros/ros.h"
#include <tf2/LinearMath/Transform.h>
#include "visualization_msgs/MarkerArray.h"
class Abstract_robot_element {
protected:
tf2::Transform relative_tf_;
tf2::Transform world_tf_;
public:
Abstract_robot_element() = default;
inline void set_relative_tf(tf2::Transform tf) { relative_tf_= tf;}
inline tf2::Transform& relative_tf(){ return relative_tf_;}
inline void calc_world_tf(tf2::Transform& tf) {world_tf_= tf * relative_tf_;}
inline void set_world_tf(tf2::Transform& tf) { world_tf_ = tf;}
inline tf2::Transform& world_tf() { return world_tf_;}
virtual void update(tf2::Transform& tf)= 0;
};
#endif
\ No newline at end of file
#ifndef ABSTRACT_ROBOT_ELEMENT_DECORATOR_
#define ABSTRACT_ROBOT_ELEMENT_DECORATOR_
#include "ros/ros.h"
#include "impl/abstract_robot_element.h"
class Abstract_robot_element_decorator : public Abstract_robot_element{
protected:
Abstract_robot_element* next_;
public:
Abstract_robot_element_decorator(Abstract_robot_element* next) : next_(next){};
inline Abstract_robot_element* wing() { return next_;}
void update(tf2::Transform& tf) override {next_->update(tf);}
virtual void input_filter(tf2::Transform& tf)=0;
virtual void output_filter()=0;
};
#endif
\ No newline at end of file
#ifndef ABSTRACT_STRATEGY_
#define ABSTRACT_STRATEGY_
#include <ros/ros.h>
#include "impl/abstract_map_loader.h"
class Abstract_map_loader;
class Abstract_strategy {
public:
Abstract_strategy() = default;
~Abstract_strategy() = default;
virtual void inv_map_creation(Abstract_map_loader* var)=0;
virtual void cloud_calculation(Abstract_map_loader* var)=0;
};
#endif
\ No newline at end of file
#ifndef BASE_BY_ROTATION_
#define BASE_BY_ROTATION_
#include <ros/ros.h>
#include "impl/abstract_map_loader.h"
#include "impl/abstract_strategy.h"
class Base_by_rotation : public Abstract_strategy {
public:
Base_by_rotation()= default;
~Base_by_rotation()= default;
void inv_map_creation(Abstract_map_loader* var) override;
void cloud_calculation(Abstract_map_loader* var) override;
};
#endif
\ No newline at end of file
#ifndef COLLISION_HELPER_
#define COLLISION_HELPER_
#include <ros/ros.h>
#include <moveit/collision_detection/collision_env.h>
#include <moveit/collision_detection_fcl/collision_env_fcl.h>
class Collision_helper : public collision_detection::CollisionEnvFCL{
public:
Collision_helper()= default;
~Collision_helper()= default;
bool collision_detector(const std::string& object1_name, const std::string& object2_name);
};
#endif
\ No newline at end of file
#ifndef Field_
#define Field_
#include "ros/ros.h"
#include "impl/abstract_robot_element.h"
class Field : public Abstract_robot_element{
public:
Field(tf2::Transform tf){ relative_tf_ = tf;}
void update(tf2::Transform& tf) override {this->calc_world_tf(tf);}
};
#endif
\ No newline at end of file
#ifndef FIELD_RVIZ_DECORATOR_
#define FIELD_RVIZ_DECORATOR_
#include "ros/ros.h"
#include "visualization_msgs/Marker.h"
#include "impl/field.h"
#include "impl/abstract_robot_element_decorator.h"
class Field_rviz_decorator : public Abstract_robot_element_decorator{
private:
visualization_msgs::MarkerArray* markers_;
public:
Field_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_;}
void update(tf2::Transform& tf) override;
void input_filter(tf2::Transform& tf) override;
void output_filter() override;
};
#endif
\ No newline at end of file
#ifndef MAP_LOADER_
#define MAP_LOADER_
#include "ros/ros.h"
#include "impl/abstract_map_loader.h"
#include <regex>
class Map_loader : public Abstract_map_loader {
public:
Map_loader(XmlRpc::XmlRpcValue& map_data, XmlRpc::XmlRpcValue& target_data);
std::vector<std::vector<pcl::PointXYZ>> base_calculation() override;
void write_task(Abstract_robot* robot) override;
};
#endif
\ No newline at end of file
#ifndef MEDIATOR_
#define MEDIATOR_
#include <ros/ros.h>
#include <ros/package.h>
#include <yaml-cpp/yaml.h>
#include <fstream>
#include "impl/abstract_mediator.h"
#include "impl/abstract_robot.h"
#include "impl/robot.h"
class Mediator : public Abstract_mediator{
public:
Mediator(std::vector<std::vector<tf2::Transform>> objects, ros::Publisher* pub) : Abstract_mediator(objects, pub){};
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* 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::pair<std::vector<object_data>, int>>& wbp) override;
bool check_collision(const int& robot) override;
void mediate() override;
void build_wings(std::bitset<3>& wings, int& robot) override;
void connect_robots(Abstract_robot* robot) override;
};
#endif
\ No newline at end of file
#ifndef MOVEIT_GRASP_MEDIATOR_
#define MOVEIT_GRASP_MEDIATOR_
#include <ros/ros.h>
#include <ros/package.h>
#include <yaml-cpp/yaml.h>
#include <fstream>
#include "impl/abstract_mediator.h"
#include "impl/moveit_mediator.h"
#include "impl/abstract_robot.h"
#include "impl/robot.h"
#include <gb_grasp/VoxelManager.h>
#include "reader/abstract_param_reader.h"
#include "reader/robot_reader.h"
#include "reader/cuboid_reader.h"
class Moveit_grasp_mediator : public Moveit_mediator{
protected:
std::shared_ptr<planning_pipeline::PlanningPipeline> planning_pipeline_;
std::shared_ptr<VoxelManager> voxel_manager_;
std::string grasp_ns_;
// Readers
std::unique_ptr<Abstract_param_reader> robot_reader_;
std::unique_ptr<Abstract_param_reader> wing_reader_;
std::unique_ptr<Abstract_param_reader> cuboid_reader_;
public:
Moveit_grasp_mediator(std::vector<std::vector<tf2::Transform>> objects, ros::Publisher* pub, std::shared_ptr<ros::NodeHandle> const& nh)
: Moveit_mediator(objects, pub, nh)
, robot_reader_(std::make_unique<Robot_reader>(nh))
, wing_reader_(std::make_unique<Wing_reader>(nh))
, cuboid_reader_(std::make_unique<Cuboid_reader>(nh)) {
visual_tools_ = std::make_shared<moveit_visual_tools::MoveItVisualTools>(
robot_model_->getModelFrame(), "/rviz_visual_tools", planning_scene_monitor_);
visual_tools_->loadMarkerPub();
visual_tools_->loadRemoteControl();
visual_tools_->setPlanningSceneMonitor(planning_scene_monitor_);
visual_tools_->loadRobotStatePub("/display_robot_state");
visual_tools_->setRobotStateTopic("/robot_state_publisher");
visual_tools_->loadTrajectoryPub("/display_planned_path");
visual_tools_->loadSharedRobotState();
visual_tools_->enableBatchPublishing();
visual_tools_->deleteAllMarkers();
visual_tools_->removeAllCollisionObjects();
visual_tools_->trigger();
nh_->getParam("/grasps_ns", grasp_ns_);
planning_pipeline_ = std::make_shared<planning_pipeline::PlanningPipeline>(robot_model_, *nh_, "planning_plugin", "request_adapter");
voxel_manager_ = std::make_shared<VoxelManager>(*nh_, grasp_ns_.c_str() ,visual_tools_,planning_scene_monitor_);
auto rd = static_cast<Robot_reader*>(robot_reader_.get())->robot_data();
for (int i = 0; i < rd.size() ;i++) connect_robots(new Moveit_robot(rd[i].name_, rd[i].pose_, rd[i].size_));
auto wd = static_cast<Wing_reader*>(wing_reader_.get())->wing_data();
for (int i = 0; i < robots_.size(); i++){
for(object_data& w : wd[i].first){
w.pose_ = robots_[i]->tf().inverse() * w.pose_;
}
robots_[i]->set_observer_mask(static_cast<wing_config>(wd[i].second));
}
set_wings(wd);
for (int i = 0; i < robots_.size(); i++){
std::bitset<3> bs = wd[i].second;
build_wings(bs, i);
}
scene_setup();
};
void mediate() override;
void connect_robots(Abstract_robot* robot) override;
void manipulate_grasp_data(Moveit_robot* robot);
void scene_setup();
};
#endif
\ No newline at end of file
#ifndef MOVEIT_MEDIATOR_
#define MOVEIT_MEDIATOR_
#include <ros/ros.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit_msgs/ApplyPlanningScene.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/kinematic_constraints/utils.h>
#include <moveit_visual_tools/moveit_visual_tools.h>
#include "impl/abstract_mediator.h"
#include "impl/abstract_robot.h"
#include "impl/robot.h"
#include "impl/moveit_robot.h"
#include "impl/wing_moveit_decorator.h"
#include "impl/collision_helper.h"
#include <moveit/task_constructor/task.h>
#include <moveit/task_constructor/stages/compute_ik.h>
#include <moveit/task_constructor/stages/connect.h>
#include <moveit/task_constructor/stages/current_state.h>
#include <moveit/task_constructor/stages/generate_grasp_pose.h>
#include <moveit/task_constructor/stages/generate_place_pose.h>
#include <moveit/task_constructor/stages/generate_pose.h>
#include <moveit/task_constructor/stages/modify_planning_scene.h>
#include <moveit/task_constructor/stages/move_relative.h>
#include <moveit/task_constructor/stages/move_to.h>
#include <moveit/task_constructor/stages/predicate_filter.h>
#include <moveit/task_constructor/solvers/cartesian_path.h>
#include <moveit/task_constructor/solvers/pipeline_planner.h>
#include <moveit_task_constructor_msgs/ExecuteTaskSolutionAction.h>
#include <moveit/task_constructor/properties.h>
#include <moveit/task_constructor/solvers/joint_interpolation.h>
#include <moveit/task_constructor/solvers/planner_interface.h>
#include <moveit/task_constructor/stage.h>
#include <moveit/task_constructor/stages/fixed_state.h>
#include <eigen_conversions/eigen_msg.h>
#include <moveit/trajectory_execution_manager/trajectory_execution_manager.h>
#include <moveit/planning_scene_monitor/current_state_monitor.h>
#include <moveit/planning_scene_monitor/planning_scene_monitor.h>
#include <moveit/moveit_cpp/moveit_cpp.h>
class Moveit_mediator : public Abstract_mediator{
protected:
std::shared_ptr<ros::NodeHandle> nh_;
std::shared_ptr<moveit::core::RobotModel> robot_model_;
std::shared_ptr<moveit_visual_tools::MoveItVisualTools> visual_tools_;
// planning Interfaces and planing scene
std::shared_ptr<moveit::planning_interface::MoveGroupInterface> mgi_;
std::unique_ptr<moveit::planning_interface::PlanningSceneInterface> psi_;
std::shared_ptr<planning_scene::PlanningScene> ps_;
std::shared_ptr<ros::Publisher> planning_scene_diff_publisher_;
std::shared_ptr<planning_scene_monitor::PlanningSceneMonitor> planning_scene_monitor_;
// task constructor
std::shared_ptr<moveit::task_constructor::solvers::PipelinePlanner> sampling_planner_;
std::shared_ptr<moveit::task_constructor::solvers::CartesianPath> cartesian_planner_;
std::map<std::string, std::vector<moveit::task_constructor::Task>> task_map_;
public:
Moveit_mediator(std::vector<std::vector<tf2::Transform>> objects, ros::Publisher* pub, std::shared_ptr<ros::NodeHandle> const& nh)
: Abstract_mediator(objects, pub), nh_(nh)
, sampling_planner_(std::make_unique<moveit::task_constructor::solvers::PipelinePlanner>())
, cartesian_planner_(std::make_unique<moveit::task_constructor::solvers::CartesianPath>())
, psi_(std::make_unique<moveit::planning_interface::PlanningSceneInterface>())
, mgi_(std::make_shared<moveit::planning_interface::MoveGroupInterface>("panda_arms"))
, planning_scene_diff_publisher_(std::make_shared<ros::Publisher>(nh_->advertise<moveit_msgs::PlanningScene>("planning_scene", 1)))
, planning_scene_monitor_(std::make_shared<planning_scene_monitor::PlanningSceneMonitor>("robot_description")){
robot_model_loader::RobotModelLoaderPtr robot_model_loader;
robot_model_loader = std::make_shared<robot_model_loader::RobotModelLoader>("robot_description");
robot_model_ = robot_model_loader->getModel();
ps_ = std::make_shared<planning_scene::PlanningScene>(robot_model_);
// planner
sampling_planner_->setProperty("goal_joint_tolerance", 1e-5);
// cartesian
cartesian_planner_->setMaxVelocityScaling(1.0);
cartesian_planner_->setMaxAccelerationScaling(1.0);
cartesian_planner_->setStepSize(.01);
};
void setup_task();
inline std::shared_ptr<moveit::planning_interface::MoveGroupInterface> mgi() {return mgi_;};
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::pair<std::vector<object_data>, int>>& wbp) override;
void connect_robots(Abstract_robot* robot) override;
void merge_ps(moveit_msgs::PlanningScene& out, moveit_msgs::PlanningScene* in, Moveit_robot* mr);
void publish_tables();
void rewrite_task_template(Abstract_robot* r, moveit_msgs::CollisionObject& source, tf2::Transform& target);
void parallel_exec(Moveit_robot& r, moveit_msgs::RobotTrajectory rt, moveit_msgs::PlanningScene ps);
moveit::task_constructor::Task create_Task(Moveit_robot* r, moveit_msgs::CollisionObject& source, tf2::Transform& target);
inline std::map<std::string, std::vector<moveit::task_constructor::Task>>& task_map(){return task_map_;};
};
#endif
\ No newline at end of file
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment