Select Git revision
moveit_robot.h
moveit_grasp_mediator.cpp 25.04 KiB
#include "impl/moveit_grasp_mediator.h"
#include <regex>
#include <gb_grasp/MapConfigLoader.h>
#include <moveit/robot_state/conversions.h>
#include "impl/wing_moveit_decorator.h"
void Moveit_grasp_mediator::connect_robots(Abstract_robot* robot) {
robots_.push_back(robot);
ROS_INFO("%s connected...", robot->name().c_str());
auto mr = dynamic_cast<Moveit_robot*>(robot);
manipulate_grasp_data(mr);
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;
bool top_only;
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);
ROS_INFO("min_quality: %f", min_quality);
ROS_INFO("max_candidate_count: %f", max_candidates);
ROS_INFO("top_grasps_only: %f", top_only);
map_gen->SetQuality(min_quality);
map_gen->SetMaxGrasp(max_candidates);
map_gen->SetZGraspOnly(top_only);
mr->set_grasp_map_generator(map_gen);
}
void Moveit_grasp_mediator::manipulate_mapspace(tf2::Transform& tf, tf2::Vector3& size){
double r,p,y;
std::vector<double> rot_mapspace(3,0);
tf2::Matrix3x3 m(tf.getRotation());
m.getRPY(r,p, y);
nh_->setParam("/mapspace/rot", std::vector<double>({r,p,y}));
nh_->setParam("/mapspace/pos", std::vector<double>({tf.getOrigin().getX(), tf.getOrigin().getY(), 0.91f}));
nh_->setParam("/mapspace/dim", std::vector<double>({size.getX(), size.getY(), size.getZ()}));
//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}));
}
void Moveit_grasp_mediator::manipulate_grasp_data(Moveit_robot* robot) {
XmlRpc::XmlRpcValue data;
nh_->getParam("/", data);
std::regex rx("panda_arm([0-9]+)");
std::smatch match;
std::regex_match(robot->name(), match, rx);
data["base_link"]= "world";
data[robot->map().at("eef_name").c_str()]["end_effector_name"] = robot->map().at("eef_name").c_str();
data[robot->map().at("eef_name").c_str()]["end_effector_type"] = "finger";
data[robot->map().at("eef_name").c_str()]["grasp_resolution"] = 0.001;
data[robot->map().at("eef_name").c_str()]["angle_resolution"] = 45;
data[robot->map().at("eef_name").c_str()]["grasp_max_depth"] = 0.035;
data[robot->map().at("eef_name").c_str()]["grasp_min_depth"] = 0.01;
data[robot->map().at("eef_name").c_str()]["grasp_depth_resolution"] = 0.005;
data[robot->map().at("eef_name").c_str()]["approach_distance_desired"] = 0.05;
data[robot->map().at("eef_name").c_str()]["retreat_distance_desired"] = 0.05;
data[robot->map().at("eef_name").c_str()]["lift_distance_desired"] = 0.02;
data[robot->map().at("eef_name").c_str()]["grasp_padding_on_approach"] = 0.005;
std::stringstream ss;
ss << "panda_" << match[1] << "_tool_center_point";
data[robot->map().at("eef_name").c_str()]["define_tcp_by_name"] = false;
data[robot->map().at("eef_name").c_str()]["tcp_name"] = ss.str().c_str();
data[robot->map().at("eef_name").c_str()]["tcp_to_eef_mount_transform"][0] = 0;
data[robot->map().at("eef_name").c_str()]["tcp_to_eef_mount_transform"][1] = 0;
data[robot->map().at("eef_name").c_str()]["tcp_to_eef_mount_transform"][2] = -0.105;
data[robot->map().at("eef_name").c_str()]["tcp_to_eef_mount_transform"][3] = 0;
data[robot->map().at("eef_name").c_str()]["tcp_to_eef_mount_transform"][4] = 0;
data[robot->map().at("eef_name").c_str()]["tcp_to_eef_mount_transform"][5] = -0.7853;
data[robot->map().at("eef_name").c_str()]["pregrasp_time_from_start"] = 0;
data[robot->map().at("eef_name").c_str()]["grasp_time_from_start"] = 0;
data[robot->map().at("eef_name").c_str()]["pregrasp_posture"][0] = 0.04;
data[robot->map().at("eef_name").c_str()]["grasp_posture"][0] = 0;
data[robot->map().at("eef_name").c_str()]["joints"][0] = robot->mgi_hand()->getJoints()[1].c_str();
data[robot->map().at("eef_name").c_str()]["max_finger_width"] = 0.085;
data[robot->map().at("eef_name").c_str()]["min_finger_width"] = 0.06;
data[robot->map().at("eef_name").c_str()]["max_grasp_width"] = 0.08;
data[robot->map().at("eef_name").c_str()]["gripper_finger_width"] = 0.015;
data[robot->map().at("eef_name").c_str()]["active_suction_range_x"] = 0.022;
data[robot->map().at("eef_name").c_str()]["active_suction_range_y"] = 0.012;
data[robot->map().at("eef_name").c_str()]["suction_rows_count"] = 2;
data[robot->map().at("eef_name").c_str()]["suction_cols_count"] = 2;
nh_->setParam("/", data);
}
bool Moveit_grasp_mediator::generateRandomCuboid(std::string& object_name, geometry_msgs::Pose& object_pose, double& x_depth,
double& y_width, double& z_height)
{
// Generate random cuboid
double xmin = 0.1;
double xmax = 0.7;
double ymin = -0.25;
double ymax = 0.25;
double zmin = 0.89;
double zmax = 1.2;
rviz_visual_tools::RandomPoseBounds pose_bounds(xmin, xmax, ymin, ymax, zmin, zmax);
double cuboid_size_min = 0.01;
double cuboid_size_max = 0.03;
rviz_visual_tools::RandomCuboidBounds cuboid_bounds(cuboid_size_min, cuboid_size_max);
object_name = "pick_target";
visual_tools_->generateRandomCuboid(object_pose, x_depth, y_width, z_height, pose_bounds, cuboid_bounds);
visual_tools_->publishCollisionCuboid(object_pose, x_depth, y_width, z_height, object_name, rviz_visual_tools::RED);
visual_tools_->publishAxis(object_pose, rviz_visual_tools::MEDIUM);
visual_tools_->trigger();
bool success = true;
double timeout = 5; // seconds
ros::Rate rate(100);
while (success && !planning_scene_monitor_->getPlanningScene()->knowsFrameTransform(object_name))
{
rate.sleep();
success = rate.cycleTime().toSec() < timeout;
}
return success;
}
void Moveit_grasp_mediator::mediate(){
visual_tools_->deleteAllMarkers();
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();
map_config.SetObjects(cbd);
float object_offset = 0.02f;
tf2::Vector3 middle_pt(0,0,0);
std::vector<double> x;
std::vector<double> y;
for(auto* ar: robots_){
Moveit_robot* r = dynamic_cast<Moveit_robot*>(ar);
tf2::Vector3 pos = r->tf().getOrigin();
pos.setZ((pos.getZ()*2) +object_offset);
tf2::Transform man_tf(r->tf().getRotation(), pos);
manipulate_mapspace(man_tf, r->size());
map_config.Create(grasp_maps_);
middle_pt+= pos;
ROS_INFO("X: %f, Y: %f, Z: %f", pos.getX(), pos.getY(), pos.getZ());
tf2::Transform A = r->tf() * r->bounds()[0];
tf2::Transform B = r->tf() * r->bounds()[1];
tf2::Transform C = r->tf() * r->bounds()[2];
tf2::Transform D = r->tf() * r->bounds()[3];
x.push_back(A.getOrigin().getX());
x.push_back(B.getOrigin().getX());
x.push_back(C.getOrigin().getX());
x.push_back(D.getOrigin().getX());
y.push_back(A.getOrigin().getY());
y.push_back(B.getOrigin().getY());
y.push_back(C.getOrigin().getY());
y.push_back(D.getOrigin().getY());
r->grasp_map_generator()->SampleMap(grasp_maps_.front());
for (auto* are: r->observers()){
auto wmd = dynamic_cast<Wing_moveit_decorator*>(are);
auto w = dynamic_cast<Wing*>(wmd->wing());
pos.setX(w->world_tf().getOrigin().getX());
pos.setY(w->world_tf().getOrigin().getY());
man_tf.setRotation(w->world_tf().getRotation());
man_tf.setOrigin(pos);
manipulate_mapspace(man_tf, w->size());
map_config.Create(grasp_maps_);
middle_pt+= pos;
ROS_INFO("X: %f, Y: %f, Z: %f", pos.getX(), pos.getY(), pos.getZ());
A = w->world_tf() * w->bounds()[0];
B = w->world_tf() * w->bounds()[1];
C = w->world_tf() * w->bounds()[2];
D = w->world_tf() * w->bounds()[3];
x.push_back(A.getOrigin().getX());
x.push_back(B.getOrigin().getX());
x.push_back(C.getOrigin().getX());
x.push_back(D.getOrigin().getX());
y.push_back(A.getOrigin().getY());
y.push_back(B.getOrigin().getY());
y.push_back(C.getOrigin().getY());
y.push_back(D.getOrigin().getY());
r->grasp_map_generator()->SampleMap(grasp_maps_.front());
}
}
ROS_INFO_STREAM("number of maps: "<< grasp_maps_.size());
middle_pt/= grasp_maps_.size();
ROS_INFO("X: %f, Y: %f, Z: %f", middle_pt.getX(), middle_pt.getY(), middle_pt.getZ());
auto x_min = *std::min_element(x.begin(),x.end());
auto x_max = *std::max_element(x.begin(),x.end());
auto y_min = *std::min_element(y.begin(),y.end());
auto y_max = *std::max_element(y.begin(),y.end());
ROS_INFO("width: %f", x_max-x_min);
ROS_INFO("height: %f", y_max-y_min);
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);
// -----------------------------------
// Generate random object to grasp
geometry_msgs::Pose object_pose;
double object_x_depth;
double object_y_width;
double object_z_height;
std::string object_name;
if (!generateRandomCuboid(object_name, object_pose, object_x_depth, object_y_width, object_z_height))
{
ROS_INFO("Failed to add random cuboid ot planning scene");
}
// -----------------------------------
// Generate grasp candidates
std::vector<moveit_grasps::GraspCandidatePtr> grasp_candidates;
// Configure the desired types of grasps
moveit_grasps::TwoFingerGraspCandidateConfig grasp_generator_config =
moveit_grasps::TwoFingerGraspCandidateConfig();
grasp_generator_config.disableAll();
grasp_generator_config.enable_face_grasps_ = true;
grasp_generator_config.generate_y_axis_grasps_ = true;
grasp_generator_config.generate_x_axis_grasps_ = true;
grasp_generator_config.generate_z_axis_grasps_ = true;
auto grasp_generator_ =std::make_shared<moveit_grasps::TwoFingerGraspGenerator>(visual_tools_);
// Set the ideal grasp orientation for scoring
std::vector<double> ideal_grasp_rpy = { 3.14, 0.0, 0.0 };
grasp_generator_->setIdealTCPGraspPoseRPY(ideal_grasp_rpy);
// Set custom grasp score weights
auto grasp_score_weights = std::make_shared<moveit_grasps::TwoFingerGraspScoreWeights>();
grasp_score_weights->orientation_x_score_weight_ = 2.0;
grasp_score_weights->orientation_y_score_weight_ = 2.0;
grasp_score_weights->orientation_z_score_weight_ = 2.0;
grasp_score_weights->translation_x_score_weight_ = 1.0;
grasp_score_weights->translation_y_score_weight_ = 1.0;
grasp_score_weights->translation_z_score_weight_ = 1.0;
// Finger gripper specific weights.
grasp_score_weights->depth_score_weight_ = 2.0;
grasp_score_weights->width_score_weight_ = 2.0;
// Assign the grasp score weights in the grasp_generator
grasp_generator_->setGraspScoreWeights(grasp_score_weights);
auto grasp_data_ = std::make_shared<moveit_grasps::TwoFingerGraspData>(*nh_, "hand_1", visual_tools_->getRobotModel());
if (!grasp_data_->loadGraspData(*nh_, "hand_1"))
{
ROS_INFO("Failed to load Grasp Data parameters.");
exit(-1);
}
grasp_generator_->setGraspCandidateConfig(grasp_generator_config);
if (!grasp_generator_->generateGrasps(visual_tools_->convertPose(object_pose), object_x_depth, object_y_width,
object_z_height, grasp_data_, grasp_candidates))
{
ROS_INFO("Grasp generator failed to generate any valid grasps");
}
// --------------------------------------------
// Generating a seed state for filtering grasps
auto seed_state = std::make_shared<robot_state::RobotState>(*visual_tools_->getSharedRobotState());
Eigen::Isometry3d eef_mount_grasp_pose =
visual_tools_->convertPose(object_pose) * grasp_data_->tcp_to_eef_mount_.inverse();
if (!getIKSolution(robot_model_->getJointModelGroup("panda_arm1"), eef_mount_grasp_pose, *seed_state, grasp_data_->parent_link_->getName()))
{
ROS_INFO("The ideal seed state is not reachable. Using start state as seed.");
}
// --------------------------------------------
// Filtering grasps
// Note: This step also solves for the grasp and pre-grasp states and stores them in grasp candidates)
bool filter_pregrasps = true;
auto grasp_filter_ = std::make_shared<moveit_grasps::TwoFingerGraspFilter>(visual_tools_->getSharedRobotState(), visual_tools_);
if (!grasp_filter_->filterGrasps(grasp_candidates, planning_scene_monitor_, robot_model_->getJointModelGroup("panda_arm1"), seed_state, filter_pregrasps,
object_name))
{
ROS_INFO("Filter grasps failed");
}
if (!grasp_filter_->removeInvalidAndFilter(grasp_candidates))
{
ROS_INFO("Grasp filtering removed all grasps");
}
// Plan free-space approach, cartesian approach, lift and retreat trajectories
moveit_grasps::GraspCandidatePtr selected_grasp_candidate;
moveit_msgs::MotionPlanResponse pre_approach_plan;
if (!planFullGrasp(grasp_candidates, selected_grasp_candidate, pre_approach_plan, object_name))
{
ROS_INFO("Failed to plan grasp motions");
}
visualizePick(selected_grasp_candidate, pre_approach_plan);
}
void Moveit_grasp_mediator::visualizePick(const moveit_grasps::GraspCandidatePtr& valid_grasp_candidate,
const moveit_msgs::MotionPlanResponse& pre_approach_plan)
{
EigenSTL::vector_Isometry3d waypoints;
moveit_grasps::GraspGenerator::getGraspWaypoints(valid_grasp_candidate, waypoints);
// Visualize waypoints
visual_tools_->publishAxisLabeled(waypoints[0], "pregrasp");
visual_tools_->publishAxisLabeled(waypoints[1], "grasp");
visual_tools_->publishAxisLabeled(waypoints[2], "lifted");
visual_tools_->publishAxisLabeled(waypoints[3], "retreat");
visual_tools_->trigger();
// Get the pre and post grasp states
visual_tools_->prompt("pre_grasp");
robot_state::RobotStatePtr pre_grasp_state =
std::make_shared<robot_state::RobotState>(*visual_tools_->getSharedRobotState());
valid_grasp_candidate->getPreGraspState(pre_grasp_state);
visual_tools_->publishRobotState(pre_grasp_state, rviz_visual_tools::ORANGE);
robot_state::RobotStatePtr grasp_state =
std::make_shared<robot_state::RobotState>(*visual_tools_->getSharedRobotState());
if (valid_grasp_candidate->getGraspStateClosed(grasp_state))
{
visual_tools_->prompt("grasp");
visual_tools_->publishRobotState(grasp_state, rviz_visual_tools::YELLOW);
}
if (valid_grasp_candidate->segmented_cartesian_traj_.size() > 1 &&
valid_grasp_candidate->segmented_cartesian_traj_[1].size())
{
visual_tools_->prompt("lift");
visual_tools_->publishRobotState(valid_grasp_candidate->segmented_cartesian_traj_[1].back(),
rviz_visual_tools::BLUE);
}
if (valid_grasp_candidate->segmented_cartesian_traj_.size() > 2 &&
valid_grasp_candidate->segmented_cartesian_traj_[2].size())
{
visual_tools_->prompt("retreat");
visual_tools_->publishRobotState(valid_grasp_candidate->segmented_cartesian_traj_[2].back(),
rviz_visual_tools::PURPLE);
}
visual_tools_->prompt("show free space approach");
visual_tools_->hideRobot();
visual_tools_->trigger();
bool wait_for_animation = true;
visual_tools_->publishTrajectoryPath(pre_approach_plan.trajectory, pre_grasp_state, wait_for_animation);
ros::Duration(0.25).sleep();
if (valid_grasp_candidate->segmented_cartesian_traj_.size() > moveit_grasps::APPROACH)
visual_tools_->publishTrajectoryPath(valid_grasp_candidate->segmented_cartesian_traj_[moveit_grasps::APPROACH],
valid_grasp_candidate->grasp_data_->arm_jmg_, wait_for_animation);
ros::Duration(0.25).sleep();
if (valid_grasp_candidate->segmented_cartesian_traj_.size() > moveit_grasps::LIFT)
visual_tools_->publishTrajectoryPath(valid_grasp_candidate->segmented_cartesian_traj_[moveit_grasps::LIFT],
valid_grasp_candidate->grasp_data_->arm_jmg_, wait_for_animation);
ros::Duration(0.25).sleep();
if (valid_grasp_candidate->segmented_cartesian_traj_.size() > moveit_grasps::RETREAT)
visual_tools_->publishTrajectoryPath(valid_grasp_candidate->segmented_cartesian_traj_[moveit_grasps::RETREAT],
valid_grasp_candidate->grasp_data_->arm_jmg_, wait_for_animation);
ros::Duration(0.25).sleep();
}
bool Moveit_grasp_mediator::planFullGrasp(std::vector<moveit_grasps::GraspCandidatePtr>& grasp_candidates,
moveit_grasps::GraspCandidatePtr& valid_grasp_candidate,
moveit_msgs::MotionPlanResponse& pre_approach_plan, const std::string& object_name)
{
moveit::core::RobotStatePtr current_state;
{
boost::scoped_ptr<planning_scene_monitor::LockedPlanningSceneRW> ls(
new planning_scene_monitor::LockedPlanningSceneRW(planning_scene_monitor_));
current_state = std::make_shared<robot_state::RobotState>((*ls)->getCurrentState());
}
std::shared_ptr<moveit_grasps::GraspPlanner>grasp_planner_ = std::make_shared<moveit_grasps::GraspPlanner>(visual_tools_);
// MoveIt Grasps allows for a manual breakpoint debugging tool to be optionally passed in
grasp_planner_->setWaitForNextStepCallback(boost::bind(&Moveit_grasp_mediator::waitForNextStep, this, visual_tools_, _1));
bool success = false;
for (; !grasp_candidates.empty(); grasp_candidates.erase(grasp_candidates.begin()))
{
valid_grasp_candidate = grasp_candidates.front();
valid_grasp_candidate->getPreGraspState(current_state);
if (!grasp_planner_->planApproachLiftRetreat(valid_grasp_candidate, current_state, planning_scene_monitor_, false,
object_name))
{
ROS_INFO("failed to plan approach lift retreat");
continue;
}
robot_state::RobotStatePtr pre_grasp_state =
valid_grasp_candidate->segmented_cartesian_traj_[moveit_grasps::APPROACH].front();
if (!planPreApproach(*pre_grasp_state, pre_approach_plan))
{
ROS_INFO("failed to plan to pregrasp_state");
continue;
}
success = true;
break;
}
return success;
}
void Moveit_grasp_mediator::waitForNextStep(const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools, const std::string& prompt)
{
visual_tools->prompt(prompt);
}
bool Moveit_grasp_mediator::planPreApproach(const robot_state::RobotState& goal_state, moveit_msgs::MotionPlanResponse& pre_approach_plan)
{
planning_interface::MotionPlanRequest req;
planning_interface::MotionPlanResponse res;
double tolerance_above = 0.01;
double tolerance_below = 0.01;
// req.planner_id = "RRTConnectkConfigDefault";
req.group_name = "panda_1_arm1";
req.num_planning_attempts = 5;
req.allowed_planning_time = 1.5;
moveit_msgs::Constraints goal =
kinematic_constraints::constructGoalConstraints(goal_state, robot_model_->getJointModelGroup("panda_arm1"), tolerance_below, tolerance_above);
req.goal_constraints.push_back(goal);
boost::scoped_ptr<planning_scene_monitor::LockedPlanningSceneRW> ls(
new planning_scene_monitor::LockedPlanningSceneRW(planning_scene_monitor_));
// ---------------------------------
// Change the robot current state
// NOTE: We have to do this since Panda start configuration is in self collision.
robot_state::RobotState rs = (*ls)->getCurrentState();
std::vector<double> starting_joint_values = { 0.0, -0.785, 0.0, -2.356, 0.0, 1.571, 0.785 };
std::vector<std::string> joint_names = { "panda_1_joint1", "panda_1_joint2", "panda_1_joint3", "panda_1_joint4",
"panda_1_joint5", "panda_1_joint6", "panda_1_joint7" };
// arm_jmg_->getActiveJointModelNames();
for (std::size_t i = 0; i < joint_names.size(); ++i)
{
rs.setJointPositions(joint_names[i], &starting_joint_values[i]);
}
rs.update();
robot_state::robotStateToRobotStateMsg(rs, req.start_state);
// ---------------------------
planning_pipeline_->generatePlan(*ls, req, res);
if (res.error_code_.val != res.error_code_.SUCCESS)
{
ROS_INFO( "Failed to plan approach successfully");
return false;
}
res.getMessage(pre_approach_plan);
return true;
}
bool Moveit_grasp_mediator::isStateValid(const planning_scene::PlanningScene* planning_scene,
const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools, robot_state::RobotState* robot_state,
const robot_model::JointModelGroup* group, const double* ik_solution)
{
robot_state->setJointGroupPositions(group, ik_solution);
robot_state->update();
return !planning_scene->isStateColliding(*robot_state, group->getName());
}
bool Moveit_grasp_mediator::getIKSolution(const moveit::core::JointModelGroup* arm_jmg, const Eigen::Isometry3d& target_pose,
robot_state::RobotState& solution, const std::string& link_name)
{
boost::scoped_ptr<planning_scene_monitor::LockedPlanningSceneRW> ls(
new planning_scene_monitor::LockedPlanningSceneRW(planning_scene_monitor_));
moveit::core::GroupStateValidityCallbackFn constraint_fn = boost::bind(
&Moveit_grasp_mediator::isStateValid,this, static_cast<const planning_scene::PlanningSceneConstPtr&>(*ls).get(), visual_tools_, _1, _2, _3);
// seed IK call with current state
solution = (*ls)->getCurrentState();
// Solve IK problem for arm
// disable explicit restarts to guarantee close solution if one exists
const double timeout = 0.1;
return solution.setFromIK(arm_jmg, target_pose, link_name, timeout, constraint_fn);
}
void Moveit_grasp_mediator::scene_setup(){
for(Abstract_robot* ar: robots_) ar->notify();
visual_tools_->setAlpha(1.0);
auto cbd = static_cast<Cuboid_reader*>(cuboid_reader_.get())->cuboid_bin();
auto cod = static_cast<Cuboid_reader*>(cuboid_reader_.get())->cuboid_obstacle();
ROS_INFO("%i die cuboids", cbd.size());
ROS_INFO("%i cuboids", cod.size());
for(auto&o:cod){
o.publishCOwait(visual_tools_,planning_scene_monitor_,rviz_visual_tools::GREY);
psi_->addCollisionObjects({o.getCoMsg()});
}
for(auto&o:cbd){
//o.publish(visual_tools_,rviz_visual_tools::GREEN);
psi_->addCollisionObjects({o.getCoMsg()});
}
visual_tools_->publishRobotState(mgi_->getCurrentState());
visual_tools_->trigger();
}
Moveit_grasp_mediator::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)) {
// , job_reader_(std::make_unique<Job_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_);
grasp_ns_ = "grasps_ns";
ROS_INFO("ns is %s", grasp_ns_.c_str());
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 = robot_reader_->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);
}
for (auto* r : robots_)r->notify();
publish_tables();
scene_setup();
};