diff --git a/include/mediator/grasp_mediator.h b/include/mediator/grasp_mediator.h
index 18b910705f289dfc7ed4e61668a377b214012201..2428b23e1aafb5ed27d9881fe4462b67e2c08245 100644
--- a/include/mediator/grasp_mediator.h
+++ b/include/mediator/grasp_mediator.h
@@ -8,18 +8,25 @@
#include "gb_grasp/Cuboid.h"
#include "gb_grasp/MapGenerator.h"
#include "gb_grasp/VoxelManager.h"
+#include "gb_grasp/GraspPipelineDemo.h"
-class GraspMediator : public MoveitMediator{
+
+class GraspMediator : public AbstractMediator{
protected:
std::shared_ptr<moveit_visual_tools::MoveItVisualTools> visual_tools_; //!< MoveItVisualTools
std::shared_ptr<planning_scene_monitor::PlanningSceneMonitor> planning_scene_monitor_; //!< Planningscene monitor
+ std::unique_ptr<moveit_grasps_demo::GraspPipelineDemo> grasp_pipeline_demo_; //!< Auerswald grasp pipeline
+ std::string referenceRobot_; //!< Reference Robot
public:
GraspMediator(std::shared_ptr<ros::NodeHandle> const& nh);
void mediate() override;
- void generateGraspMap();
+ inline void setReferenceRobot(std::string& str) {referenceRobot_ = str;}
+ void connectRobots(std::unique_ptr<AbstractRobotDecorator> robot) override;
+ void setPanel() override;
+
};
#endif
\ No newline at end of file
diff --git a/include/reader/cuboid_reader.h b/include/reader/cuboid_reader.h
index b57ce93a8309a409115fab3c22a0cc6cdd8a4662..47d0465bf0652726347bccbad0dbc35a8f970633 100644
--- a/include/reader/cuboid_reader.h
+++ b/include/reader/cuboid_reader.h
@@ -31,25 +31,25 @@ class CuboidReader : public AbstractParamReader{
/*!
\param cuboid_data
*/
- inline void setCuboidBinData(std::vector<Cuboid>& cuboid_data) {cuboid_box_ = cuboid_data;}
+ inline void setCuboidBox(std::vector<Cuboid>& cuboid_data) {cuboid_box_ = cuboid_data;}
//! Set Cuboid obstacle
/*!
\param cuboid_data
*/
- inline void setCuboidObstacleData(std::vector<Cuboid>& cuboid_data) {cuboid_obstacle_ = cuboid_data;}
+ inline void setCuboidObstacle(std::vector<Cuboid>& cuboid_data) {cuboid_obstacle_ = cuboid_data;}
//! Get Cuboid box
/*!
\return Cuboid box vector
*/
- inline std::vector<Cuboid> cuboidBin() {return cuboid_box_;}
+ inline std::vector<Cuboid>& cuboidBox() {return cuboid_box_;}
//! Get Cuboid obstacle
/*!
\return Cuboid obstacle vector
*/
- inline std::vector<Cuboid> cuboidObstacle() {return cuboid_obstacle_;}
+ inline std::vector<Cuboid>& cuboidObstacle() {return cuboid_obstacle_;}
//! read implementatin
void read() override;
diff --git a/launch/config_routine.launch b/launch/config_routine.launch
index dc76e32ffb0b1b7c7e9152548c1d982c20871b34..c397a43f143824887b81231b095bed41f1b9f4fd 100644
--- a/launch/config_routine.launch
+++ b/launch/config_routine.launch
@@ -1,6 +1,16 @@
<launch>
<!-- specify the planning pipeline -->
+ <arg name="reference_robot" default="panda_arm1" />
+ <arg name="referenceXYZ" default="-0.320002 -0.000003 0.89" />
+ <arg name="referenceRPY" default="0 0 0" />
+
+ <arg name="result" default="dummy/-243824774.yaml" />
+
+ <rosparam command="load" file="$(find multi_cell_builder)/results/$(arg result)"/>
+
+ <rosparam param="reference_robot" subst_value="True"> $(arg reference_robot)</rosparam>
+
<arg name="pipeline" default="ompl" />
<!-- By default, we do not start a database (it can be large) -->
@@ -51,6 +61,9 @@
<arg name="debug" value="$(arg debug)"/>
<arg name="pipeline" value="$(arg pipeline)"/>
<arg name="load_robot_description" value="$(arg load_robot_description)"/>
+ <arg name="referenceXYZ" value="$(arg referenceXYZ)"/>
+ <arg name="referenceRPY" value="$(arg referenceRPY)"/>
+
</include>
<!-- If database loading was enabled, start mongodb as well -->
diff --git a/results/dummy/-243824774.yaml b/results/dummy/-243824774.yaml
index da5172d49ff5e79c970a8cea9111382f53d7ef84..841590a692ac7212b62ff5c7139fd0a7cb357be8 100644
--- a/results/dummy/-243824774.yaml
+++ b/results/dummy/-243824774.yaml
@@ -21,8 +21,15 @@
{ 'id': 'table1_front_panel' , 'pos': { 'x': 0.552498 , 'y': -0.000000 , 'z': 0.885000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } },
{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.652502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } },
{ 'id': 'table2_right_panel' , 'pos': { 'x': -0.100007 , 'y': 1.957498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } },
-{ 'id': 'blue1', 'type': 'BOX', 'pos': { 'x': -0.300000, 'y': -0.7, 'z': 0.9355 },'size': { 'length': 0.0318, 'width': 0.0636, 'height': 0.091 },'orientation': { 'x':0, 'y':0, 'z':0, 'w':1},'color': { 'b': 1 } },
-{ 'id': 'blue2', 'type': 'BOX', 'pos': { 'x': 0.1, 'y': 1.11, 'z': 0.9355 },'size': { 'length': 0.0318, 'width': 0.0636, 'height': 0.091 },'orientation': { 'x':0, 'y':0, 'z':0, 'w':1},'color': { 'b': 1 } },
+{ 'id': 'binBlue','type': 'BIN','pos': { 'x': -0.34,'y': 0.49,'z': 0.9725 }, 'size': { 'length': 0.21,'width': 0.3,'height': 0.165 },'orientation': { 'w': 1 },'color': { 'b': 1 } },
+{ 'id': 'binRed','type': 'BIN','pos': { 'x': 0.06,'y': 0.49,'z': 0.9725 }, 'size': { 'length': 0.21,'width': 0.3,'height': 0.165 },'orientation': { 'w': 1 },'color': { 'r': 1 } },
+{ 'id': 'binGreen','type': 'BIN','pos': { 'x': 0.46,'y': 0.49,'z': 0.9725 }, 'size': { 'length': 0.21,'width': 0.3,'height': 0.165 },'orientation': { 'w': 1 },'color': { 'g': 1 } },
+{ 'id': 'object1','type': 'BOX','pos': { 'x': -0.6,'y': -0.6,'z': 0.996 }, 'size': { 'length': 0.031,'width': 0.062,'height': 0.121 },'orientation': { 'z': 0.382683,'w': 0.92388 },'color': { 'r': 1 } },
+{ 'id': 'object2','type': 'BOX','pos': { 'x': -0.6,'y': -0.5,'z': 0.996 }, 'size': { 'length': 0.031,'width': 0.062,'height': 0.121 },'orientation': { 'w': 1 },'color': { 'r': 1 } },
+{ 'id': 'object3','type': 'BOX','pos': { 'x': -0.4,'y': -0.6,'z': 0.959 }, 'size': { 'length': 0.031,'width': 0.031,'height': 0.138 },'orientation': { 'z': 0.382683,'w': 0.92388 },'color': { 'r': 1 } },
+{ 'id': 'object4','type': 'BOX','pos': { 'x': -0.6,'y': -0.4,'z': 0.996 }, 'size': { 'length': 0.031,'width': 0.062,'height': 0.121},'orientation': { 'z': 0.382683,'w': 0.92388 },'color': { 'g': 1 } },
+{ 'id': 'object5','type': 'BOX','pos': { 'x': -0.4,'y': -0.5,'z': 0.959 }, 'size': { 'length': 0.031,'width': 0.031,'height': 0.138},'orientation': { 'z': 1,'w': 0.92388 },'color': { 'g': 1 } },
+{ 'id': 'obstacle1','pos': { 'x': 0.34,'y': -0.11,'z': 0.94 }, 'size': { 'length': 0.3,'width': 0.3,'height': 0.1 },'orientation': { 'z': 0.452123,'w': 1 },'color': { 'b': 1 } },
{ 'id': 'arm1','type': 'ARM','pos': { 'x': -0.320002, 'y': -0.000003, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } },
{ 'id': 'arm2','type': 'ARM','pos': { 'x': -0.320005, 'y': 1.304997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } },
]}
\ No newline at end of file
diff --git a/src/cell_routine.cpp b/src/cell_routine.cpp
index ef2a9957bfbd22229b7d21a93e797c635efbfb34..ade662b5982ae7e081940ea36f4daa975dac3466 100644
--- a/src/cell_routine.cpp
+++ b/src/cell_routine.cpp
@@ -32,10 +32,7 @@ int main(int argc, char **argv){
mediator->connectRobots(std::move(ceti_panda));
}
- //mediator->set_dirname(filename);
-
- mediator->setPanel();
-
+ //mediator->set_dirname(filename);
mediator->mediate();
while (ros::ok()){
diff --git a/src/config_routine.cpp b/src/config_routine.cpp
index 8c228c457b7f86c598d6dde949db95e2a1376709..cf9976eb56ed4f03a2fa162c666a9e8068084ebe 100644
--- a/src/config_routine.cpp
+++ b/src/config_routine.cpp
@@ -8,24 +8,28 @@
int main(int argc, char *argv[]) {
ros::init(argc, argv, "config_routine");
+ std::shared_ptr<ros::NodeHandle> n(new ros::NodeHandle);
// Allow the action server to recieve and send ros messages
ros::AsyncSpinner spinner(2);
spinner.start();
+ std::shared_ptr<GraspMediator> mediator = std::make_shared<GraspMediator>(n);
- // Seed random
- srand(ros::Time::now().toSec());
-
- // Run Tests
- moveit_grasps_demo::GraspPipelineDemo tester;
- ros::Duration(2).sleep();
- tester.publishConfigScene();
- tester.generateGraspMap();
- tester.getVoxelizedEnv();
- tester.computeConfig();
- tester.build();
+ auto rd = mediator->robotReader()->robotData();
+ for (int i = 0; i < rd.size() ;i++){
+ std::unique_ptr<AbstractRobot> ceti = std::make_unique<CetiRobot>(rd[i].name_, rd[i].pose_, rd[i].size_);
+ std::unique_ptr<PandaDecorator> ceti_panda = std::make_unique<PandaDecorator>(std::move(ceti));
+ mediator->connectRobots(std::move(ceti_panda));
+ }
+ std::string demo_robot;
+ n->getParam("/reference_robot", demo_robot);
+ mediator->setReferenceRobot(demo_robot);
+ mediator->mediate();
+ while (ros::ok()){
+ ros::spinOnce();
+ }
return 0;
}
\ No newline at end of file
diff --git a/src/mediator/base_calculation_mediator.cpp b/src/mediator/base_calculation_mediator.cpp
index 9050579ad467a91268cc5ed204f0e29ec353a6c9..e13a242b30eacabbd21b9925a19747ed275caa1e 100644
--- a/src/mediator/base_calculation_mediator.cpp
+++ b/src/mediator/base_calculation_mediator.cpp
@@ -559,7 +559,7 @@ void BaseCalculationMediator::writeFile(std::vector<protobuf_entry>& wc_solution
std::stringstream box_ss;
- for (auto& box : cuboid_reader_->cuboidBin()){
+ for (auto& box : cuboid_reader_->cuboidBox()){
box_ss << "{ 'id': '" << box.Name << "', 'type': 'BOX', 'pos': { 'x': "<<box.Pose.position.x<<", 'y': "<<box.Pose.position.y<<", 'z': "<< box.Pose.position.z<<" },'size': { 'length': "<< box.x_depth<<", 'width': "<<box.y_width<<", 'height': "<< box.z_heigth<<" },'orientation': { 'x':"<< box.Pose.orientation.x <<", 'y':" << box.Pose.orientation.y<< ", 'z':" << box.Pose.orientation.z << ", 'w':" << box.Pose.orientation.w<< "},'color': { 'b': 1 } }, \n";
}
diff --git a/src/mediator/grasp_mediator.cpp b/src/mediator/grasp_mediator.cpp
index 9bcd9e480fa6a093c90607bee17cf2c80e18e44d..dfa9bd8e4a7a48f2adfed56b36298077f21de2bb 100644
--- a/src/mediator/grasp_mediator.cpp
+++ b/src/mediator/grasp_mediator.cpp
@@ -1,201 +1,118 @@
#include "mediator/grasp_mediator.h"
GraspMediator::GraspMediator(std::shared_ptr<ros::NodeHandle> const& nh)
- : MoveitMediator(nh)
+ : AbstractMediator(nh)
+ , grasp_pipeline_demo_(std::make_unique<moveit_grasps_demo::GraspPipelineDemo>())
, planning_scene_monitor_(std::make_shared<planning_scene_monitor::PlanningSceneMonitor>("robot_description"))
{
- if (planning_scene_monitor_->getPlanningScene()){
- planning_scene_monitor_->startPublishingPlanningScene(planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE,
- "planning_scene");
- planning_scene_monitor_->getPlanningScene()->setName("planning_scene");
- } else {
- ROS_ERROR_STREAM_NAMED("test", "Planning scene not configured");
- return;
- }
-
- 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_->hideRobot();
-
- visual_tools_->trigger();
-
- // Publish the global frame
- visual_tools_->publishAxis(Eigen::Isometry3d::Identity());
- visual_tools_->trigger();
}
-void GraspMediator::mediate() {
- setPanel();
- publishTables();
- generateGraspMap();
- ros::waitForShutdown();
+void GraspMediator::connectRobots(std::unique_ptr<AbstractRobotDecorator> robot) {
+ ROS_INFO("connecting %s...", robot->name().c_str());
+ robots_.insert_or_assign(robot->name(), std::move(robot));
}
-void GraspMediator::generateGraspMap() {
- auto cd = cuboid_reader_->cuboidBin();
-
- //std::vector<std::string> objs = {"bottle1", "bottle2"};
+void GraspMediator::setPanel(){
+ auto wd = wing_reader_->wingData();
- for (int i = 0; i < cd.size(); i ++){
- std::stringstream ss;
- ss << "box_" << i;
-
- moveit_msgs::CollisionObject item;
- item.id = ss.str();
- item.header.frame_id = "world";
- item.header.stamp = ros::Time::now();
- item.primitives.resize(1);
- item.primitives[0].type = item.primitives[0].BOX;
- item.primitives[0].dimensions.resize(3);
- item.primitives[0].dimensions[0] = cd[i].x_depth;
- item.primitives[0].dimensions[1] = cd[i].y_width;
- item.primitives[0].dimensions[2] = cd[i].z_heigth;
+ for (const auto& s_r : robots_){
+ try{
+ auto ceti_bot = dynamic_cast<CetiRobot*>(s_r.second->next());
+ ceti_bot->setObserverMask(static_cast<wing_config>(wd.at(s_r.first).second));
+ } catch(const std::out_of_range& oor) {
+ ROS_INFO("No mask data for %s", s_r.first.c_str());
+ }
+ }
- item.primitive_poses.resize(1);
- item.primitive_poses[0].position.x = cd[i].Pose.position.x;
- item.primitive_poses[0].position.y = cd[i].Pose.position.y;
- item.primitive_poses[0].position.z = cd[i].Pose.position.z;
- item.primitive_poses[0].orientation.x = cd[i].Pose.orientation.x;
- item.primitive_poses[0].orientation.y = cd[i].Pose.orientation.y;
- item.primitive_poses[0].orientation.z = cd[i].Pose.orientation.z;
- item.primitive_poses[0].orientation.w = cd[i].Pose.orientation.w;
- item.operation = item.ADD;
+ for (const auto& s_r : robots_){
+ auto ceti_bot = dynamic_cast<CetiRobot*>(s_r.second->next());
+ std::vector<std::unique_ptr<AbstractRobotElementDecorator>> panels(3);
+
+ for (std::size_t j = 0; j < ceti_bot->observerMask().size(); j++){
+ if (ceti_bot->observerMask()[j]){
+ try{
+ tf2::Transform relative_tf = ceti_bot->tf().inverse() * wd.at(ceti_bot->name()).first[j].pose_;
+
+ std::unique_ptr<AbstractRobotElement> panel = std::make_unique<MoveitPanel>(wd.at(s_r.first).first[j].name_, relative_tf, wd.at(s_r.first).first[j].size_);
+ std::unique_ptr<AbstractRobotElementDecorator> log = std::make_unique<LogDecorator>(std::move(panel));
+ panels[j] = std::move(log);
+ } catch (std::out_of_range &oor){
+ ROS_INFO("OOR in set_panel");
+ }
+ }
+ }
+ ceti_bot->setObservers(panels);
+ }
- psi_->applyCollisionObject(item);
- // Could also safe id's somewhere
+ for (const auto& s_r : robots_){
+ s_r.second->notify();
}
+}
- ROS_INFO("Debug1!");
- auto arm_jmg_ = robot_model_->getJointModelGroup(robots_.at("panda_arm1")->name());
- ROS_INFO("Debug1.5!");
- auto voxel_manager_ = std::make_shared<VoxelManager>(*nh_,"",visual_tools_,planning_scene_monitor_);
-
- ROS_INFO("%f", robots_.at("panda_arm1")->tf().getOrigin().getZ());
- ROS_INFO("Debug2!");
- Cuboid voxelSpace("VS");
- voxelSpace.Pose.position.x= robots_.at("panda_arm1")->tf().getOrigin().getX();
- voxelSpace.Pose.position.y= robots_.at("panda_arm1")->tf().getOrigin().getY();
- voxelSpace.Pose.position.z= robots_.at("panda_arm1")->tf().getOrigin().getZ()*2 + 0.1f;
- voxelSpace.Pose.orientation.x = robots_.at("panda_arm1")->tf().getRotation().getX();
- voxelSpace.Pose.orientation.y = robots_.at("panda_arm1")->tf().getRotation().getY();
- voxelSpace.Pose.orientation.z = robots_.at("panda_arm1")->tf().getRotation().getZ();
- voxelSpace.Pose.orientation.w = robots_.at("panda_arm1")->tf().getRotation().getW();
- voxelSpace.x_depth = 0.3f;
- voxelSpace.y_width = 0.3f;
- voxelSpace.z_heigth = 0.2f;
-
- //voxel_manager_->setVerboseLevel(0);
- //voxel_manager_->setVoxelSize(0.02f);
- //voxel_manager_->setVoxelSpace(voxelSpace);
- ROS_INFO("Debug2.5!");
-
- XmlRpc::XmlRpcValue data;
- nh_->getParam("/", data);
-
- std::regex rx("panda_arm([0-9]+)");
- std::smatch match;
- std::regex_match(robots_.at("panda_arm1")->name(), match, rx);
- data["base_link"]= "world";
-
- data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["end_effector_name"] = robots_.at("panda_arm1")->map().at("eef_name").c_str();
- data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["end_effector_type"] = "finger";
- data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["grasp_resolution"] = 0.001f;
- data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["angle_resolution"] = 45;
- data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["grasp_max_depth"] = 0.035f;
- data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["grasp_min_depth"] = 0.01f;
- data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["grasp_depth_resolution"] = 0.005f;
- data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["approach_distance_desired"] = 0.05f;
- data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["retreat_distance_desired"] = 0.05f;
- data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["lift_distance_desired"] = 0.02f;
- data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["grasp_padding_on_approach"] = 0.005f;
-
- std::stringstream ss;
- ss << "panda_" << match[1] << "_tool_center_point";
- data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["define_tcp_by_name"] = false;
- data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["tcp_name"] = ss.str().c_str();
- data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["tcp_to_eef_mount_transform"][0] = 0;
- data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["tcp_to_eef_mount_transform"][1] = 0;
- data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["tcp_to_eef_mount_transform"][2] = -0.105f;
- data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["tcp_to_eef_mount_transform"][3] = 0;
- data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["tcp_to_eef_mount_transform"][4] = 0;
- data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["tcp_to_eef_mount_transform"][5] = -0.7853f;
-
- data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["pregrasp_time_from_start"] = 0;
- data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["grasp_time_from_start"] = 0;
- data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["pregrasp_posture"][0] = 0.04f;
- data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["grasp_posture"][0] = 0;
- data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["joints"][0] = robots_.at("panda_arm1")->mgiHand()->getJoints()[1].c_str();
- data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["max_finger_width"] = 0.085f;
- data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["min_finger_width"] = 0.06f;
- data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["max_grasp_width"] = 0.08f;
- data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["gripper_finger_width"] = 0.015f;
- data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["active_suction_range_x"] = 0.022f;
- data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["active_suction_range_y"] = 0.012f;
- data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["suction_rows_count"] = 2;
- data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["suction_cols_count"] = 2;
- nh_->setParam("/", data);
-
- MapGenerator::MapGeneratorPtr grasp_map_generator_ = std::make_shared<MapGenerator>(*nh_,
- visual_tools_,
- planning_scene_monitor_,
- robots_.at("panda_arm1")->map().at("eef_name"),
- robots_.at("panda_arm1")->name(),
- arm_jmg_,
- robots_.at("panda_arm1")->mgi()->getCurrentState(),
- voxel_manager_);
- auto cb = cuboid_reader_->cuboidBin();
-
- ROS_INFO("Debug3!");
-
- std::vector<GraspMap> maps;
-
- Cuboid workspace("WS");
- workspace.Pose.position.x= robots_.at("panda_arm1")->tf().getOrigin().getX();
- workspace.Pose.position.y= robots_.at("panda_arm1")->tf().getOrigin().getY();
- workspace.Pose.position.z= robots_.at("panda_arm1")->tf().getOrigin().getZ()*2 + 0.02f;
- workspace.Pose.orientation.x = robots_.at("panda_arm1")->tf().getRotation().getX();
- workspace.Pose.orientation.y = robots_.at("panda_arm1")->tf().getRotation().getY();
- workspace.Pose.orientation.z = robots_.at("panda_arm1")->tf().getRotation().getZ();
- workspace.Pose.orientation.w = robots_.at("panda_arm1")->tf().getRotation().getW();
- workspace.x_depth = 0.2f;
- workspace.y_width = 0.2f;
- workspace.z_heigth = 0.04f;
-
- ROS_INFO("Debug4!");
+void GraspMediator::mediate() {
+ setPanel();
+ ros::Duration(2).sleep();
+ std::vector<Cuboid> relevant_box;
+ std::vector<Cuboid> relevant_obstacle;
+
+ Cuboid rob(referenceRobot_);
+ rob.Pose.position.x = robots_.at(referenceRobot_)->tf().getOrigin().getX();
+ rob.Pose.position.y = robots_.at(referenceRobot_)->tf().getOrigin().getY();
+ rob.Pose.position.z = robots_.at(referenceRobot_)->tf().getOrigin().getZ();
+ rob.Pose.orientation.x = robots_.at(referenceRobot_)->tf().getRotation().getX();
+ rob.Pose.orientation.y = robots_.at(referenceRobot_)->tf().getRotation().getY();
+ rob.Pose.orientation.z = robots_.at(referenceRobot_)->tf().getRotation().getZ();
+ rob.Pose.orientation.w = robots_.at(referenceRobot_)->tf().getRotation().getW();
+ rob.x_depth = robots_.at(referenceRobot_)->size().getX();
+ rob.y_width = robots_.at(referenceRobot_)->size().getY();
+ rob.z_heigth = robots_.at(referenceRobot_)->tf().getOrigin().getZ()*2;
+ relevant_obstacle.push_back(rob);
+
+ auto ceti_bot = dynamic_cast<CetiRobot*>(robots_.at(referenceRobot_)->next());
+
+ for (std::size_t k = 0; k < ceti_bot->observerMask().size(); k++){
+ if(ceti_bot->observerMask()[k]){
+ Cuboid wing(ceti_bot->observers()[k]->name());
+ wing.Pose.position.x = ceti_bot->observers()[k]->worldTf().getOrigin().getX();
+ wing.Pose.position.y = ceti_bot->observers()[k]->worldTf().getOrigin().getY();
+ wing.Pose.position.z = ceti_bot->observers()[k]->worldTf().getOrigin().getZ() - ceti_bot->observers()[k]->size().getZ()/2;
+ wing.Pose.orientation.x = ceti_bot->observers()[k]->worldTf().getRotation().getX();
+ wing.Pose.orientation.y = ceti_bot->observers()[k]->worldTf().getRotation().getY();
+ wing.Pose.orientation.z = ceti_bot->observers()[k]->worldTf().getRotation().getZ();
+ wing.Pose.orientation.w = ceti_bot->observers()[k]->worldTf().getRotation().getW();
+ wing.x_depth = ceti_bot->observers()[k]->size().getX();
+ wing.y_width = ceti_bot->observers()[k]->size().getY();
+ wing.z_heigth = ceti_bot->observers()[k]->size().getZ();
+ relevant_obstacle.push_back(wing);
+ }
+ }
- double cube_clearence, translation_rate, rotation_rate, max_rotation;
- cube_clearence = 0.01f;
- translation_rate = 0.03f;
- rotation_rate = 45.f * (M_PI/180.f);
- max_rotation = 90.f * (M_PI/180.f);
+ for (auto& ro :relevant_obstacle){
+ grasp_pipeline_demo_->sceneObstacles().push_back(ro);
+ }
- for (auto& object: cb) {
- GraspMap map(workspace,object,cube_clearence,translation_rate,rotation_rate,max_rotation);
- maps.push_back(map);
+ for (const auto& b : cuboid_reader_->cuboidBox()){
+ std::string temp;
+ std::bitset<3> temp2 = ceti_bot->observerMask();
+ tf2::Transform pos(tf2::Quaternion(b.Pose.orientation.x, b.Pose.orientation.y, b.Pose.orientation.z, b.Pose.orientation.w), tf2::Vector3(b.Pose.position.x, b.Pose.position.y, b.Pose.position.z));
+ if (robots_.at(referenceRobot_)->checkSingleObjectCollision(pos, temp, temp2)){
+ grasp_pipeline_demo_->sceneBoxes().push_back(b);
+ }
}
- ROS_INFO("Debug5!");
-
- // Create Maps
- {
- ROS_INFO("creating map ...");
- auto start_time = ros::Time::now();
- for (GraspMap &m: maps) { grasp_map_generator_->SampleMap(m); }
- double durration = (start_time - ros::Time::now()).toSec();
- ROS_INFO_STREAM(maps.size() << " GraspMaps created in " << durration << " seconds");
+ for (const auto& b : cuboid_reader_->cuboidObstacle()){
+ std::string temp;
+ std::bitset<3> temp2 = ceti_bot->observerMask();
+ tf2::Transform pos(tf2::Quaternion(b.Pose.orientation.x, b.Pose.orientation.y, b.Pose.orientation.z, b.Pose.orientation.w), tf2::Vector3(b.Pose.position.x, b.Pose.position.y, b.Pose.position.z));
+ if (robots_.at(referenceRobot_)->checkSingleObjectCollision(pos, temp, temp2)){
+ grasp_pipeline_demo_->sceneObstacles().push_back(b);
+ }
}
- ROS_INFO("Debug6!");
+ srand(ros::Time::now().toSec());
+ grasp_pipeline_demo_->publishConfigScene();
+ grasp_pipeline_demo_->generateGraspMap();
+ grasp_pipeline_demo_->getVoxelizedEnv();
+ grasp_pipeline_demo_->computeConfig();
+ grasp_pipeline_demo_->build();
}
diff --git a/src/mediator/moveit_mediator.cpp b/src/mediator/moveit_mediator.cpp
index ee307a0e9f404ed5a1b746fcf4a1e0f2b7d93c1f..c76ec90c17fbf5c2f8a2b444f8a1f305a15077f5 100644
--- a/src/mediator/moveit_mediator.cpp
+++ b/src/mediator/moveit_mediator.cpp
@@ -239,7 +239,7 @@ void MoveitMediator::taskPlanner(){
2. A box position in acm is not the first entry in task, in that case we can might try for each position
*/
auto jq = job_reader_->robotData();
- auto cd = cuboid_reader_->cuboidBin();
+ auto cd = cuboid_reader_->cuboidBox();
//std::vector<std::string> objs = {"bottle1", "bottle2"};
diff --git a/src/reader/cuboid_reader.cpp b/src/reader/cuboid_reader.cpp
index 085795bcdc90e52878b8b9312c3ed2451d935a29..001da07faa18108a6661baee2fb0a76665d9e5ea 100644
--- a/src/reader/cuboid_reader.cpp
+++ b/src/reader/cuboid_reader.cpp
@@ -4,6 +4,7 @@ void CuboidReader::read(){
ROS_INFO("--- CUBOID_READER ---");
XmlRpc::XmlRpcValue resources;
nh_->getParam("/objects", resources);
+ std::smatch match;
for (int i = 0; i < resources.size(); i++){
std::string id = resources[i]["id"];
@@ -45,7 +46,7 @@ void CuboidReader::read(){
ROS_INFO("=> Object: size('%f', '%f', '%f')", size.getX(), size.getY(), size.getZ());
}
- if(resources[i]["type"] == "BIN"){
+ if(resources[i]["type"] == "BIN" || std::regex_match(id, match, std::regex("obstacle([0-9]+)"))){
tf2::Vector3 pos;
tf2::Vector3 size;
tf2::Quaternion rot;