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

...

parent 7f1213cb
No related branches found
No related tags found
No related merge requests found
Show whitespace changes
Inline Side-by-side
src/impl/moveit_grasp_mediator.cpp
+ 0
− 73
View file @ 7aa3f9fa
......@@ -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_);
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment