diff --git a/src/impl/moveit_grasp_mediator.cpp b/src/impl/moveit_grasp_mediator.cpp
index cc7129286701ceb2a19b66d2bbfb423a26aa7aae..f02e0ef1f0215681c23d7b3cbf07e5ee4e49acb8 100644
--- a/src/impl/moveit_grasp_mediator.cpp
+++ b/src/impl/moveit_grasp_mediator.cpp
@@ -2,10 +2,7 @@
#include <regex>
#include <gb_grasp/MapConfigLoader.h>
#include <moveit/robot_state/conversions.h>
-<<<<<<< HEAD
-=======
#include "impl/wing_moveit_decorator.h"
->>>>>>> 4694ff9 (acm merge done, ferdinand done, working on tasks)
void Moveit_grasp_mediator::connect_robots(Abstract_robot* robot) {
@@ -14,10 +11,6 @@ void Moveit_grasp_mediator::connect_robots(Abstract_robot* robot) {
auto mr = dynamic_cast<Moveit_robot*>(robot);
manipulate_grasp_data(mr);
-<<<<<<< HEAD
- manipulate_mapspace();
-=======
->>>>>>> 4694ff9 (acm merge done, ferdinand done, working on tasks)
auto map_gen = std::make_shared<MapGenerator>(*nh_, visual_tools_, planning_scene_monitor_, mr->map().at("eef_name"), mr->name().c_str(), robot_model_->getJointModelGroup(mr->name().c_str()), mgi().get()->getCurrentState(), voxel_manager_);
double min_quality, max_candidates;
@@ -26,13 +19,10 @@ void Moveit_grasp_mediator::connect_robots(Abstract_robot* robot) {
rosparam_shortcuts::get(grasp_ns_.c_str(),*nh_,"min_quality",min_quality);
rosparam_shortcuts::get(grasp_ns_.c_str(),*nh_,"max_candidate_count",max_candidates);
rosparam_shortcuts::get(grasp_ns_.c_str(),*nh_,"top_grasps_only",top_only);
-<<<<<<< HEAD
-=======
ROS_INFO("min_quality: %f", min_quality);
ROS_INFO("max_candidate_count: %f", max_candidates);
ROS_INFO("top_grasps_only: %f", top_only);
->>>>>>> 4694ff9 (acm merge done, ferdinand done, working on tasks)
map_gen->SetQuality(min_quality);
@@ -42,17 +32,6 @@ void Moveit_grasp_mediator::connect_robots(Abstract_robot* robot) {
}
-<<<<<<< HEAD
-void Moveit_grasp_mediator::manipulate_mapspace(){
- std::vector<float> pos_voxel_space;
- std::vector<float> pos_mapspace;
- nh_->getParam("/voxel_space/pos", pos_voxel_space);
- nh_->getParam("/mapspace/pos", pos_mapspace);
- pos_mapspace[2]= 0.91f;
- pos_voxel_space[2]= 0.91f;
- nh_->setParam("/voxel_space/pos", pos_voxel_space);
- nh_->setParam("/mapspace/pos", pos_mapspace);
-=======
void Moveit_grasp_mediator::manipulate_mapspace(tf2::Transform& tf, tf2::Vector3& size){
double r,p,y;
std::vector<double> rot_mapspace(3,0);
@@ -65,7 +44,6 @@ void Moveit_grasp_mediator::manipulate_mapspace(tf2::Transform& tf, tf2::Vector3
//nh_->setParam("/voxel_space/dim", std::vector<float>({0.3f,0.3f,0.2f}));
//nh_->setParam("/voxel_space/pos", std::vector<double>({tf.getOrigin().getX(), tf.getOrigin().getY(), tf.getOrigin().getZ()}));
//nh_->setParam("/voxel_space/rot", std::vector<double>({r,p,y}));
->>>>>>> 4694ff9 (acm merge done, ferdinand done, working on tasks)
}
void Moveit_grasp_mediator::manipulate_grasp_data(Moveit_robot* robot) {
@@ -131,11 +109,7 @@ bool Moveit_grasp_mediator::generateRandomCuboid(std::string& object_name, geome
double& y_width, double& z_height)
{
// Generate random cuboid
-<<<<<<< HEAD
- double xmin = 0.5;
-=======
double xmin = 0.1;
->>>>>>> 4694ff9 (acm merge done, ferdinand done, working on tasks)
double xmax = 0.7;
double ymin = -0.25;
double ymax = 0.25;
@@ -166,36 +140,7 @@ bool Moveit_grasp_mediator::generateRandomCuboid(std::string& object_name, geome
void Moveit_grasp_mediator::mediate(){
visual_tools_->deleteAllMarkers();
-<<<<<<< HEAD
- /*
- MapConfigLoader map_config(*nh_, grasp_ns_);
- auto cbd = static_cast<Cuboid_reader*>(cuboid_reader_.get())->cuboid_bin();
- map_config.SetObjects(cbd);
- map_config.Create(grasp_maps_);
- ROS_INFO_STREAM("number of maps: "<< grasp_maps_.size());
- {
- ROS_INFO("creating map ...");
- auto start_time = ros::Time::now();
- for (auto* r : robots_){
- auto mr = dynamic_cast<Moveit_robot*>(r);
- for (GraspMap &m: grasp_maps_) { mr->grasp_map_generator()->SampleMap(m); }
- double durration = (start_time - ros::Time::now()).toSec();
- ROS_INFO_STREAM(grasp_maps_.size() << " GraspMaps created in " << durration << " seconds");
- }
- }
-
-
- visual_tools_->setAlpha(0.1);
- voxel_manager_->Voxelize();
-
- auto cod = static_cast<Cuboid_reader*>(cuboid_reader_.get())->cuboid_obstacle();
- for(auto&o:cod){
- voxel_manager_->GetCuboidCollisions(o,voxel_environment_);
- }
- voxel_manager_->Voxelize(true);
- */
-=======
ROS_INFO("ns is %s", grasp_ns_.c_str());
MapConfigLoader map_config(*nh_, grasp_ns_.c_str());
auto cbd = static_cast<Cuboid_reader*>(cuboid_reader_.get())->cuboid_bin();
@@ -275,7 +220,6 @@ void Moveit_grasp_mediator::mediate(){
}
voxel_manager_->Voxelize(true);
->>>>>>> 4694ff9 (acm merge done, ferdinand done, working on tasks)
// -----------------------------------
// Generate random object to grasp
@@ -490,11 +434,7 @@ bool Moveit_grasp_mediator::planPreApproach(const robot_state::RobotState& goal_
double tolerance_above = 0.01;
double tolerance_below = 0.01;
// req.planner_id = "RRTConnectkConfigDefault";
-<<<<<<< HEAD
- req.group_name = "panda_arm1";
-=======
req.group_name = "panda_1_arm1";
->>>>>>> 4694ff9 (acm merge done, ferdinand done, working on tasks)
req.num_planning_attempts = 5;
req.allowed_planning_time = 1.5;
moveit_msgs::Constraints goal =
@@ -564,13 +504,8 @@ void Moveit_grasp_mediator::scene_setup(){
auto cbd = static_cast<Cuboid_reader*>(cuboid_reader_.get())->cuboid_bin();
auto cod = static_cast<Cuboid_reader*>(cuboid_reader_.get())->cuboid_obstacle();
-<<<<<<< HEAD
- ROS_INFO("%i die cuboids", cbd.size());
- ROS_INFO("%i cuboids", cod.size());
-=======
ROS_INFO("%i die cuboids", cbd.size());
ROS_INFO("%i cuboids", cod.size());
->>>>>>> 4694ff9 (acm merge done, ferdinand done, working on tasks)
for(auto&o:cod){
o.publishCOwait(visual_tools_,planning_scene_monitor_,rviz_visual_tools::GREY);
@@ -579,13 +514,8 @@ void Moveit_grasp_mediator::scene_setup(){
for(auto&o:cbd){
//o.publish(visual_tools_,rviz_visual_tools::GREEN);
-<<<<<<< HEAD
- psi_->addCollisionObjects({o.getCoMsg()});
- }
-=======
psi_->addCollisionObjects({o.getCoMsg()});
}
->>>>>>> 4694ff9 (acm merge done, ferdinand done, working on tasks)
visual_tools_->publishRobotState(mgi_->getCurrentState());
visual_tools_->trigger();
@@ -615,11 +545,8 @@ Moveit_grasp_mediator::Moveit_grasp_mediator(std::vector<std::vector<tf2::Transf
visual_tools_->trigger();
nh_->getParam("/grasps_ns", grasp_ns_);
-<<<<<<< HEAD
-=======
grasp_ns_ = "grasps_ns";
ROS_INFO("ns is %s", grasp_ns_.c_str());
->>>>>>> 4694ff9 (acm merge done, ferdinand done, working on tasks)
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_);