/**
* Sent STATUSTEXT message to rosout
*
* @param[in] severity Levels defined in common.xml
*/
void process_statustext_normal(uint8_t severity, std::string &text) {
switch (severity) {
case MAV_SEVERITY_EMERGENCY:
case MAV_SEVERITY_ALERT:
case MAV_SEVERITY_CRITICAL:
case MAV_SEVERITY_ERROR:
ROS_ERROR_STREAM_NAMED("fcu", "FCU: " << text);
break;
case MAV_SEVERITY_WARNING:
case MAV_SEVERITY_NOTICE:
ROS_WARN_STREAM_NAMED("fcu", "FCU: " << text);
break;
case MAV_SEVERITY_INFO:
ROS_INFO_STREAM_NAMED("fcu", "FCU: " << text);
break;
case MAV_SEVERITY_DEBUG:
ROS_DEBUG_STREAM_NAMED("fcu", "FCU: " << text);
break;
default:
ROS_WARN_STREAM_NAMED("fcu", "FCU: UNK(" <<
int(severity) << "): " << text);
break;
};
}
void JointData::update(const sensor_msgs::JointState::ConstPtr& msg)
{
if(msg->name.empty())
{
ROS_WARN_STREAM_NAMED("KHANHWInterface", _name << " had an invalid JointState message, ignoring.");
return;
}
boost::mutex::scoped_lock lock(_mutex);
size_t counter = 0;
for(; counter < msg->name.size(); ++counter)
{
if(msg->name[counter] == _name)
{
break;
}
}
if(counter >= msg->name.size())
{
ROS_WARN_STREAM_NAMED("KHANHWInterface", _name << " received a JointState message which did not contain the required joint name.");
return;
}
if(counter < msg->position.size())
{
_pos = msg->position[counter];
}
if(counter < msg->velocity.size())
{
_vel = msg->velocity[counter];
}
}
void testIKServiceCall()
{
// define goal pose for right hand end effector
Eigen::Affine3d goal_pose = Eigen::Affine3d::Identity();
goal_pose *= Eigen::AngleAxisd(3.141593, Eigen::Vector3d::UnitY());
goal_pose.translation()[0] = 0.6;
goal_pose.translation()[1] = -0.5;
goal_pose.translation()[2] = 0.1;
visual_tools_->publishAxisLabeled(goal_pose, "goal_pose");
// call ik service
geometry_msgs::PoseStamped tf2_msg;
tf2_msg.header.stamp = ros::Time::now();
tf::poseEigenToMsg(goal_pose, tf2_msg.pose);
ik_srv_.request.pose_stamp.push_back(tf2_msg);
// TODO: not the right way to call this? compile error on this section...
if (ik_client_right_.call(ik_srv_))
{
ROS_DEBUG_STREAM_NAMED("testIKServiceCall","ik service called successfully.");
}
else
{
ROS_WARN_STREAM_NAMED("testIKServiceCall","Failed to call ik service...");
}
}
bool TransmissionInterfaceLoader::load(const TransmissionInfo& transmission_info)
{
// Create transmission instance
TransmissionPtr transmission;
try
{
TransmissionLoaderPtr transmission_loader = transmission_class_loader_->createInstance(transmission_info.type_);
transmission = transmission_loader->load(transmission_info);
if (!transmission) {return false;}
}
catch(pluginlib::LibraryLoadException &ex)
{
ROS_ERROR_STREAM_NAMED("parser", "Failed to load transmission '" << transmission_info.name_ <<
"'. Unsupported type '" << transmission_info.type_ << "'.\n" << ex.what());
return false;
}
// We currently only deal with transmissions specifying a single hardware interface in the joints
assert(!transmission_info.joints_.empty() && !transmission_info.joints_.front().hardware_interfaces_.empty());
const std::vector<std::string>& hw_ifaces_ref = transmission_info.joints_.front().hardware_interfaces_; // First joint
BOOST_FOREACH(const JointInfo& jnt_info, transmission_info.joints_)
{
// Error out if at least one joint has a different set of hardware interfaces
if (hw_ifaces_ref.size() != jnt_info.hardware_interfaces_.size() ||
!internal::is_permutation(hw_ifaces_ref.begin(), hw_ifaces_ref.end(),
jnt_info.hardware_interfaces_.begin()))
{
ROS_ERROR_STREAM_NAMED("parser",
"Failed to load transmission '" << transmission_info.name_ <<
"'. It has joints with different hardware interfaces. This is currently unsupported.");
return false;
}
}
// Load transmission for all specified hardware interfaces
bool has_at_least_one_hw_iface = false;
BOOST_FOREACH(const std::string& hw_iface, hw_ifaces_ref)
{
RequisiteProviderPtr req_provider;
try
{
req_provider = req_provider_loader_->createInstance(hw_iface);
if (!req_provider) {continue;}
}
catch(pluginlib::LibraryLoadException &ex)
{
ROS_WARN_STREAM_NAMED("parser", "Failed to process the '" << hw_iface <<
"' hardware interface for transmission '" << transmission_info.name_ <<
"'.\n" << ex.what());
continue;
}
const bool load_ok = req_provider->loadTransmissionMaps(transmission_info, loader_data_, transmission);
if (load_ok) {has_at_least_one_hw_iface = true;}
else {continue;}
}
// move platform server receives a new goal
void moveGoalCB()
{
set_terminal_state_ = true;
move_goal_.nav_goal = as_.acceptNewGoal()->nav_goal;
ROS_INFO_STREAM_NAMED(logger_name_, "Received Goal #" <<move_goal_.nav_goal.header.seq);
if (as_.isPreemptRequested() ||!ros::ok())
{
ROS_WARN_STREAM_NAMED(logger_name_, "Preempt Requested on goal #" << move_goal_.nav_goal.header.seq);
if (planning_)
ac_planner_.cancelGoalsAtAndBeforeTime(ros::Time::now());
if (controlling_)
ac_control_.cancelGoalsAtAndBeforeTime(ros::Time::now());
move_result_.result_state = 0;
move_result_.error_string = "Preempt Requested!!!";
as_.setPreempted(move_result_);
return;
}
// Convert move goal to plan goal
pose_goal_.pose_goal = move_goal_.nav_goal;
if (planner_state_sub.getNumPublishers()==0)
{
ROS_WARN_STREAM_NAMED(logger_name_, "Goal #" << move_goal_.nav_goal.header.seq << " not sent - planner is down");
planning_ = false;
move_result_.result_state = 0;
move_result_.error_string = "Planner is down";
as_.setAborted(move_result_);
}
else
{
ac_planner_.sendGoal(pose_goal_, boost::bind(&MovePlatformAction::planningDoneCB, this, _1, _2),
actionlib::SimpleActionClient<oea_planner::planAction>::SimpleActiveCallback(),
actionlib::SimpleActionClient<oea_planner::planAction>::SimpleFeedbackCallback());
planning_ = true;
ROS_DEBUG_STREAM_NAMED(logger_name_, "Goal #" << move_goal_.nav_goal.header.seq << " sent to planner");
}
return;
}
void planningDoneCB(const actionlib::SimpleClientGoalState& state, const oea_planner::planResultConstPtr &result)
{
planning_ = false;
ROS_DEBUG_STREAM_NAMED(logger_name_, "Plan Action finished: " << state.toString());
move_result_.result_state = result->result_state;
if (move_result_.result_state) //if plan OK
{
planned_path_goal_.plan_goal = result->planned_path; // goal for the controller is result of the planner
if (ctrl_state_sub.getNumPublishers()==0)
{
ROS_WARN_STREAM_NAMED(logger_name_, "Goal #" << move_goal_.nav_goal.header.seq << " not sent - Controller is down");
controlling_ = false;
move_result_.result_state = 0;
move_result_.error_string = "Controller is down!!!";
as_.setAborted(move_result_);
}
else
{
ac_control_.sendGoal(planned_path_goal_, boost::bind(&MovePlatformAction::ControlDoneCB, this, _1, _2),
actionlib::SimpleActionClient<oea_controller::controlPlatformAction>::SimpleActiveCallback(),
actionlib::SimpleActionClient<oea_controller::controlPlatformAction>::SimpleFeedbackCallback()); //boost::bind(&MovePlatformAction::ControlFeedbackCB, this,_1));
controlling_ = true;
ROS_DEBUG_STREAM_NAMED(logger_name_,"Goal #" << move_goal_.nav_goal.header.seq << " sent to Controller");
}
}
else //if plan NOT OK
{
ac_control_.cancelGoalsAtAndBeforeTime(ros::Time::now());
move_result_.error_string = "Planning Failed: " + result->error_string;
ROS_WARN_STREAM_NAMED(logger_name_, "Aborting because " << move_result_.error_string);
as_.setAborted(move_result_);
}
return;
}
bool IMarkerRobotState::setToRandomState(double clearance)
{
static const std::size_t MAX_ATTEMPTS = 1000;
for (std::size_t attempt = 0; attempt < MAX_ATTEMPTS; ++attempt)
{
// Set each planning group to random
for (std::size_t i = 0; i < arm_datas_.size(); ++i)
{
imarker_state_->setToRandomPositions(arm_datas_[i].jmg_);
}
// Update transforms
imarker_state_->update();
planning_scene_monitor::LockedPlanningSceneRO planning_scene(psm_); // Read only lock
// Collision check
// which planning group to collision check, "" is everything
static const bool verbose = false;
if (planning_scene->isStateValid(*imarker_state_, "", verbose))
{
// Check clearance
if (clearance > 0)
{
// which planning group to collision check, "" is everything
if (planning_scene->distanceToCollision(*imarker_state_) < clearance)
{
continue; // clearance is not enough
}
}
ROS_INFO_STREAM_NAMED(name_, "Found valid random robot state after " << attempt << " attempts");
// Set updated pose from robot state
for (std::size_t i = 0; i < arm_datas_.size(); ++i)
end_effectors_[i]->setPoseFromRobotState();
// Send to imarker
for (std::size_t i = 0; i < arm_datas_.size(); ++i)
end_effectors_[i]->sendUpdatedIMarkerPose();
return true;
}
if (attempt == 100)
ROS_WARN_STREAM_NAMED(name_, "Taking long time to find valid random state");
}
ROS_ERROR_STREAM_NAMED(name_, "Unable to find valid random robot state for imarker after " << MAX_ATTEMPTS << " attem"
"pts");
return false;
}
/**
* Send STATUSTEXT messate to rosout with APM severity levels
*
* @param[in] severity APM levels.
*/
void process_statustext_apm_quirk(uint8_t severity, std::string &text) {
switch (severity) {
case 1: // SEVERITY_LOW
ROS_INFO_STREAM_NAMED("fcu", "FCU: " << text);
break;
case 2: // SEVERITY_MEDIUM
ROS_WARN_STREAM_NAMED("fcu", "FCU: " << text);
break;
case 3: // SEVERITY_HIGH
case 4: // SEVERITY_CRITICAL
case 5: // SEVERITY_USER_RESPONSE
ROS_ERROR_STREAM_NAMED("fcu", "FCU: " << text);
break;
default:
ROS_WARN_STREAM_NAMED("fcu", "FCU: UNK(" <<
int(severity) << "): " << text);
break;
};
}
void GraspData::print()
{
ROS_WARN_STREAM_NAMED("grasp_data","Debug Grasp Data variable values:");
std::cout << "grasp_pose_to_eef_pose_: \n" <<grasp_pose_to_eef_pose_<<std::endl;
std::cout << "pre_grasp_posture_: \n" <<pre_grasp_posture_<<std::endl;
std::cout << "grasp_posture_: \n" <<grasp_posture_<<std::endl;
std::cout << "base_link_: " <<base_link_<<std::endl;
std::cout << "ee_parent_link_: " <<ee_parent_link_<<std::endl;
std::cout << "ee_group_: " <<ee_group_<<std::endl;
std::cout << "grasp_depth_: " <<grasp_depth_<<std::endl;
std::cout << "angle_resolution_: " <<angle_resolution_<<std::endl;
std::cout << "approach_retreat_desired_dist_: " <<approach_retreat_desired_dist_<<std::endl;
std::cout << "approach_retreat_min_dist_: " <<approach_retreat_min_dist_<<std::endl;
std::cout << "object_size_: " <<object_size_<<std::endl;
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "clam_gripper_controller");
// Simulation mode
bool simulation_mode = false;
ros::NodeHandle nh("~");
nh.getParam("simulate", simulation_mode);
if(simulation_mode)
ROS_WARN_STREAM_NAMED("clam_gripper_controller","In simulation mode");
// Start controller
clam_controller::ClamGripperController server("gripper_action", simulation_mode);
ros::spin();
return 0;
}
void rosNamed(const std::vector<std::string> &msgs) {
if (msgs.size()==0) return;
if (msgs.size()==1) { ROS_INFO_STREAM("Kobuki : " << msgs[0]); }
if (msgs.size()==2) {
if (msgs[0] == "debug") { ROS_DEBUG_STREAM("Kobuki : " << msgs[1]); }
else if (msgs[0] == "info" ) { ROS_INFO_STREAM ("Kobuki : " << msgs[1]); }
else if (msgs[0] == "warn" ) { ROS_WARN_STREAM ("Kobuki : " << msgs[1]); }
else if (msgs[0] == "error") { ROS_ERROR_STREAM("Kobuki : " << msgs[1]); }
else if (msgs[0] == "fatal") { ROS_FATAL_STREAM("Kobuki : " << msgs[1]); }
}
if (msgs.size()==3) {
if (msgs[0] == "debug") { ROS_DEBUG_STREAM_NAMED(msgs[1], "Kobuki : " << msgs[2]); }
else if (msgs[0] == "info" ) { ROS_INFO_STREAM_NAMED (msgs[1], "Kobuki : " << msgs[2]); }
else if (msgs[0] == "warn" ) { ROS_WARN_STREAM_NAMED (msgs[1], "Kobuki : " << msgs[2]); }
else if (msgs[0] == "error") { ROS_ERROR_STREAM_NAMED(msgs[1], "Kobuki : " << msgs[2]); }
else if (msgs[0] == "fatal") { ROS_FATAL_STREAM_NAMED(msgs[1], "Kobuki : " << msgs[2]); }
}
}
// ******************************************************************************************
// Delete a controller
// ******************************************************************************************
void ROSControllersWidget::deleteController()
{
std::string controller_name = current_edit_controller_;
if (controller_name.empty())
{
QTreeWidgetItem* item = controllers_tree_->currentItem();
// Check that something was actually selected
if (item == nullptr)
return;
// Get the user custom properties of the currently selected row
int type = item->data(0, Qt::UserRole).value<int>();
if (type == 0)
controller_name = item->text(0).toUtf8().constData();
}
// Confirm user wants to delete controller
if (QMessageBox::question(
this, "Confirm Controller Deletion",
QString("Are you sure you want to delete the controller '").append(controller_name.c_str()).append(" ?"),
QMessageBox::Ok | QMessageBox::Cancel) == QMessageBox::Cancel)
{
return;
}
// Delete actual controller
if (config_data_->deleteROSController(controller_name))
{
ROS_INFO_STREAM_NAMED("Setup Assistant", "Controller " << controller_name << " deleted succefully");
}
else
{
ROS_WARN_STREAM_NAMED("Setup Assistant", "Couldn't delete Controller " << controller_name);
}
current_edit_controller_.clear();
// Switch to main screen
showMainScreen();
// Reload main screen table
loadControllersTree();
}
// Get Transmissions from the URDF
bool GazeboRosSoftHandPlugin::parseTransmissionsFromURDF(const std::string& urdf_string)
{
transmission_interface::TransmissionParser::parse(urdf_string, transmissions_);
std::vector<transmission_interface::TransmissionInfo>::iterator it = transmissions_.begin();
for(; it != transmissions_.end(); )
{
if (robot_namespace_.compare(it->robot_namespace_) != 0)
{
ROS_WARN_STREAM_NAMED("gazebo_ros_soft_hand", "gazebo_ros_soft_hand plugin deleted transmission "
<< it->name_
<< " because it is not in the same robotNamespace as this plugin. This might be normal in a multi-robot configuration though.");
it = transmissions_.erase(it);
}
else
{
++it;
}
}
return true;
}
bool IMarkerRobotState::loadFromFile(const std::string& file_name)
{
if (!boost::filesystem::exists(file_name))
{
ROS_WARN_STREAM_NAMED(name_, "File not found: " << file_name);
return false;
}
std::ifstream input_file(file_name);
std::string line;
if (!std::getline(input_file, line))
{
ROS_ERROR_STREAM_NAMED(name_, "Unable to read line");
return false;
}
// Get robot state from file
moveit::core::streamToRobotState(*imarker_state_, line);
return true;
}
void movePreemptCB()
{
ROS_WARN_STREAM_NAMED(logger_name_, "Preempt Requested");
if (planning_)
{
ROS_DEBUG_NAMED(logger_name_, "Planning - cancelling all plan goals");
ac_planner_.cancelGoalsAtAndBeforeTime(ros::Time::now());
}
if (controlling_)
{
ROS_DEBUG_NAMED(logger_name_, "Controlling - cancelling all ctrl goals");
ac_control_.cancelGoalsAtAndBeforeTime(ros::Time::now());
}
move_result_.result_state = 0;
move_result_.error_string = "Move Platform Preempt Request!";
as_.setPreempted(move_result_);
set_terminal_state_ = false;
return;
}
KHANHWInterface::KHANHWInterface(const std::string& robot_ns) :
hardware_interface::RobotHW(),
_js_interface(),
_vj_interface(),
_transmissions(),
_joints()
{
ros::NodeHandle nh(robot_ns);
std::string rd_param;
if(!nh.searchParam("robot_description", rd_param))
{
ROS_WARN_STREAM_NAMED("KHANHWInterface", " Cannot find URDF from parameter server. Bailing.");
return;
}
string urdf_string;
nh.getParam(rd_param, urdf_string);
// urdf::Model model;
// if(!model.initString(urdf_string))
// {
// ROS_WARN_STREAM_NAMED("KHANHWInterface", " Cannot parse URDF string");
// return;
// }
transmission_interface::TransmissionParser::parse(urdf_string, _transmissions);
_joints.reserve(_transmissions.size());
for(vector<transmission_interface::TransmissionInfo>::iterator ii = _transmissions.begin();
ii != _transmissions.end(); ++ii)
{
if(ii->joints_.empty())
{
ROS_WARN_STREAM_NAMED("KHANHWInterface", ii->name_ << " does not have any joints assigned.");
continue;
}
//grab first VelocityJointInterface in the transmission
vector<transmission_interface::JointInfo>::iterator joint = std::find_if(ii->joints_.begin(), ii->joints_.end(),
IsVelocityJointInterface);
if(joint == ii->joints_.end())
{
ROS_WARN_STREAM_NAMED("KHANHWInterface", ii->name_ <<
" has no valid joints with a velocity interface to control");
continue;
}
if(joint->name_.empty())
{
ROS_WARN_STREAM_NAMED("KHANHWInterface", "Encountered a joint with an empty name, refusing to use it.");
continue;
}
_joints.push_back(boost::make_shared<JointData>(joint->name_));
JointData& j_info = *_joints.back();
hardware_interface::JointStateHandle base_handle =
hardware_interface::JointStateHandle(j_info._name, &j_info._pos,
&j_info._vel, &j_info._effort);
_js_interface.registerHandle(base_handle);
hardware_interface::JointHandle vel_handle = hardware_interface::JointHandle(base_handle,
&j_info._vel_cmd);
_vj_interface.registerHandle(vel_handle);
//Note all pub/sub is assumed to be in $robot_ns/py_controller/joint_name
//Inside, a JointState topic named encoder should exist, which the Python node publishes
//joint updates
//Inside, a JointState topic named cmd should exist, which the Python node reads for
//velocity commands
const string control_base = robot_ns + "/py_controller/" + j_info._name;
j_info._input = nh.subscribe(control_base + "/encoder", 10,
&JointData::update, &j_info);
j_info._output = nh.advertise<sensor_msgs::JointState>(control_base + "/cmd", 10);
//init PID controller for this joint
j_info._controller.init(control_base, true);
}
registerInterface(&_js_interface);
registerInterface(&_vj_interface);
}
// Proccess the point clouds
void processPointCloud( const sensor_msgs::PointCloud2ConstPtr& pointcloud_msg )
{
ROS_INFO_NAMED("perception","\n\n\n");
ROS_INFO_STREAM_NAMED("perception","Processing new point cloud");
// ---------------------------------------------------------------------------------------------
// Start making result
result_.blocks.poses.clear(); // Clear last block perception result
result_.blocks.header.stamp = pointcloud_msg->header.stamp;
result_.blocks.header.frame_id = base_link;
// Basic point cloud conversions ---------------------------------------------------------------
// Convert from ROS to PCL
pcl::PointCloud<pcl::PointXYZRGB> cloud;
pcl::fromROSMsg(*pointcloud_msg, cloud);
// Make new point cloud that is in our working frame
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_transformed(new pcl::PointCloud<pcl::PointXYZRGB>);
// Transform to whatever frame we're working in, probably the arm's base frame, ie "base_link"
ROS_INFO_STREAM_NAMED("perception","Waiting for transform...");
ros::spinOnce();
tf_listener_.waitForTransform(base_link, cloud.header.frame_id, cloud.header.stamp, ros::Duration(2.0));
if(!pcl_ros::transformPointCloud(base_link, cloud, *cloud_transformed, tf_listener_))
{
if( process_count_ > 1 ) // the first time we can ignore it
ROS_ERROR_STREAM_NAMED("perception","Error converting to desired frame");
// Do this to speed up the next process attempt:
process_count_ = PROCESS_EVERY_NTH;
return;
}
// Limit to things we think are roughly at the table height ------------------------------------
// pcl::PointIndices::Ptr filtered_indices(new pcl::PointIndices); // hold things at table height
// std::vector<int>
boost::shared_ptr<std::vector<int> > filtered_indices(new std::vector<int>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PassThrough<pcl::PointXYZRGB> pass;
pass.setInputCloud(cloud_transformed);
pass.setFilterFieldName("z");
pass.setFilterLimits(table_height - 0.05, table_height + block_size + 0.05);
//pass.setFilterLimits(table_height - 0.01, table_height + block_size + 0.02); // DTC
pass.filter(*filtered_indices);
/*
// Limit to things in front of the robot ---------------------------------------------------
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZRGB>);
//pass.setInputCloud(cloud_filteredZ);
pass.setIndices(filtered_indices);
pass.setFilterFieldName("x");
pass.setFilterLimits(.1,.5);
pass.filter(*cloud_filtered);
*/
/*
// Check if any points remain
if( cloud_filtered->points.size() == 0 )
{
ROS_ERROR_STREAM_NAMED("perception","0 points left");
return;
}
else
{
ROS_INFO_STREAM_NAMED("perception","Filtered, %d points left", (int) cloud_filtered->points.size());
}
*/
// Segment components --------------------------------------------------------------------------
// Create the segmentation object for the planar model and set all the parameters
pcl::SACSegmentation<pcl::PointXYZRGB> seg;
seg.setOptimizeCoefficients(true);
seg.setModelType(pcl::SACMODEL_PLANE);
seg.setMethodType(pcl::SAC_RANSAC); // robustness estimator - RANSAC is simple
seg.setMaxIterations(200); // the maximum number of iterations the sample consensus method will run
seg.setDistanceThreshold(0.005); // determines how close a point must be to the model in order to be considered an inlier
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
pcl::ModelCoefficients::Ptr model_coefficients(new pcl::ModelCoefficients);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_plane(new pcl::PointCloud<pcl::PointXYZRGB>());
/*
int nr_points = cloud_filtered->points.size();
// Segment cloud until there are less than 30% of points left? not sure why this is necessary
while(cloud_filtered->points.size() > 0.3 * nr_points)
{
// Segment the largest planar component from the remaining cloud (find the table)
seg.setInputCloud(cloud_filtered);
// seg.setIndices();
seg.segment(*inliers, *model_coefficients);
if(inliers->indices.size() == 0)
{
ROS_ERROR_STREAM_NAMED("perception","Could not estimate a planar model for the given dataset.");
return;
}
//std::cout << "Inliers: " << (inliers->indices.size()) << std::endl;
// Extract the planar inliers from the input cloud
pcl::ExtractIndices<pcl::PointXYZRGB> extract;
extract.setInputCloud(cloud_filtered);
extract.setIndices(inliers);
extract.setNegative(false);
// Copy the extracted component (the table) to a seperate point cloud
extract.filter(*cloud_plane);
//std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size() << " data points." << std::endl;
// Remove the planar inliers, extract the rest
extract.setNegative(true);
extract.filter(*cloud_filtered); // remove table from cloud_filtered
// Debug output - DTC
// Show the contents of the inlier set, together with the estimated plane parameters, in ax+by+cz+d=0 form (general equation of a plane)
ROS_INFO_STREAM_NAMED("perception", "Model coefficients: " << model_coefficients->values[0] << " "
<< model_coefficients->values[1] << " "
<< model_coefficients->values[2] << " "
<< model_coefficients->values[3] ); // TODO: turn this into an rviz marker somehow?
*/
// Show groups of recognized objects (the inliers)
/*std::cerr << "Model inliers: " << inliers->indices.size () << std::endl;
for (size_t i = 0; i < inliers->indices.size (); ++i) {
std::cerr << inliers->indices[i] << " " << cloud.points[inliers->indices[i]].x << " "
<< cloud.points[inliers->indices[i]].y << " "
<< cloud.points[inliers->indices[i]].z << std::endl;
}*/
// }
// DTC: Removed to make compatible with PCL 1.5
// Creating the KdTree object for the search method of the extraction
// pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree(new pcl::search::KdTreeFLANN<pcl::PointXYZRGB>);
pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZRGB>);
// tree->setInputCloud(cloud_filtered);
tree->setInputCloud(cloud_transformed, filtered_indices);
// Find the clusters (objects) on the table
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> cluster_extract;
//cluster_extract.setClusterTolerance(0.005); // 5mm - If you take a very small value, it can happen that an actual object can be seen as multiple clusters. On the other hand, if you set the value too high, it could happen, that multiple objects are seen as one cluster. So our recommendation is to just test and try out which value suits your dataset.
cluster_extract.setClusterTolerance(0.02); // 2cm
cluster_extract.setMinClusterSize(100);
cluster_extract.setMaxClusterSize(25000);
// cluster_extract.setSearchMethod(tree);
// cluster_extract.setInputCloud(cloud_filtered);
cluster_extract.setInputCloud(cloud_transformed);
cluster_extract.setIndices(filtered_indices);
ROS_INFO_STREAM_NAMED("perception","Extracting...");
cluster_extract.extract(cluster_indices);
ROS_INFO_STREAM_NAMED("perception","after cluster extract");
// Publish point cloud data
// filtered_pub_.publish(cloud_filtered);
// plane_pub_.publish(cloud_plane);
ROS_WARN_STREAM_NAMED("perception","Number indicies/clusters: " << cluster_indices.size() );
// processClusters( cluster_indices, pointcloud_msg, cloud_filtered );
processClusters( cluster_indices, cloud_transformed, cloud_filtered, cloud );
// ---------------------------------------------------------------------------------------------
// Final results
if(result_.blocks.poses.size() > 0)
{
// Change action state, if we the action is currently active
if(action_server_.isActive())
{
action_server_.setSucceeded(result_);
}
// Publish block poses
block_pose_pub_.publish(result_.blocks);
// Publish rviz markers of the blocks
publishBlockLocation();
ROS_INFO_STREAM_NAMED("perception","Finished ---------------------------------------------- ");
}
else
{
ROS_INFO_STREAM_NAMED("perception","Couldn't find any blocks this iteration!");
}
}
// Processes the point cloud with OpenCV using the PCL cluster indices
void processClusters( const std::vector<pcl::PointIndices> cluster_indices,
// const sensor_msgs::PointCloud2ConstPtr& pointcloud_msg,
const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr cloud_transformed,
const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr& cloud_filtered,
const pcl::PointCloud<pcl::PointXYZRGB>& cloud )
{
// -------------------------------------------------------------------------------------------------------
// Convert image
ROS_INFO_STREAM_NAMED("perception","Converting image to OpenCV format");
try
{
sensor_msgs::ImagePtr image_msg(new sensor_msgs::Image);
// pcl::toROSMsg (*pointcloud_msg, *image_msg);
pcl::toROSMsg (*cloud_transformed, *image_msg);
cv_bridge::CvImagePtr input_bridge = cv_bridge::toCvCopy(image_msg, "rgb8");
full_input_image = input_bridge->image;
}
catch (cv_bridge::Exception& ex)
{
ROS_ERROR_STREAM_NAMED("perception","[calibrate] Failed to convert image");
return;
}
// -------------------------------------------------------------------------------------------------------
// Process Image
// Convert image to gray
cv::cvtColor( full_input_image, full_input_image_gray, CV_BGR2GRAY );
//cv::adaptiveThreshold( full_input_image, full_input_image_gray, 255, CV_ADAPTIVE_THRESH_MEAN_C, CV_THRESH_BINARY,5,10);
// Blur image - reduce noise with a 3x3 kernel
cv::blur( full_input_image_gray, full_input_image_gray, cv::Size(3,3) );
ROS_INFO_STREAM_NAMED("perception","Finished coverting");
// -------------------------------------------------------------------------------------------------------
// Check OpenCV and PCL image height for errors
int image_width = cloud.width;
int image_height = cloud.height;
ROS_DEBUG_STREAM( "PCL Image height " << image_height << " -- width " << image_width << "\n");
int image_width_cv = full_input_image.size.p[1];
int image_height_cv = full_input_image.size.p[0];
ROS_DEBUG_STREAM( "OpenCV Image height " << image_height_cv << " -- width " << image_width_cv << "\n");
if( image_width != image_width_cv || image_height != image_height_cv )
{
ROS_ERROR_STREAM_NAMED("perception","PCL and OpenCV image heights/widths do not match!");
return;
}
// -------------------------------------------------------------------------------------------------------
// GUI Stuff
// First window
const char* opencv_window = "Source";
/*
cv::namedWindow( opencv_window, CV_WINDOW_AUTOSIZE );
cv::imshow( opencv_window, full_input_image_gray );
*/
// while(true) // use this when we want to tweak the image
{
output_image = full_input_image.clone();
// -------------------------------------------------------------------------------------------------------
// Start processing clusters
ROS_INFO_STREAM_NAMED("perception","Finding min/max in x/y axis");
int top_image_overlay_x = 0; // tracks were to copyTo the mini images
// for each cluster, see if it is a block
for(size_t c = 0; c < cluster_indices.size(); ++c)
{
ROS_INFO_STREAM_NAMED("perception","\n\n");
ROS_INFO_STREAM_NAMED("perception","On cluster " << c);
// find the outer dimensions of the cluster
double xmin = 0; double xmax = 0;
double ymin = 0; double ymax = 0;
// also remember each min & max's correponding other coordinate (not needed for z)
double xminy = 0; double xmaxy = 0;
double yminx = 0; double ymaxx = 0;
// also remember their corresponding indice
int xmini = 0; int xmaxi = 0;
int ymini = 0; int ymaxi = 0;
// loop through and find all min/max of x/y
for(size_t i = 0; i < cluster_indices[c].indices.size(); i++)
{
int j = cluster_indices[c].indices[i];
// Get RGB from point cloud
pcl::PointXYZRGB p = cloud_transformed->points[j];
double x = p.x;
double y = p.y;
if(i == 0) // initial values
{
xmin = xmax = x;
ymin = ymax = y;
xminy = xmaxy = y;
yminx = ymaxx = x;
xmini = xmaxi = ymini = ymaxi = j; // record the indice corresponding to the min/max
}
else
{
if( x < xmin )
{
xmin = x;
xminy = y;
xmini = j;
}
if( x > xmax )
{
xmax = x;
xmaxy = y;
xmaxi = j;
}
if( y < ymin )
{
ymin = y;
yminx = x;
ymini = j;
}
if( y > ymax )
{
ymax = y;
ymaxx = x;
ymaxi = j;
}
}
}
ROS_DEBUG_STREAM_NAMED("perception","Cluster size - xmin: " << xmin << " xmax: " << xmax << " ymin: " << ymin << " ymax: " << ymax);
ROS_DEBUG_STREAM_NAMED("perception","Cluster size - xmini: " << xmini << " xmaxi: " << xmaxi << " ymini: " << ymini << " ymaxi: " << ymaxi);
// ---------------------------------------------------------------------------------------------
// Check if these dimensions make sense for the block size specified
double xside = xmax-xmin;
double yside = ymax-ymin;
const double tol = 0.01; // 1 cm error tolerance
// In order to be part of the block, xside and yside must be between
// blocksize and blocksize*sqrt(2)
if(xside > block_size-tol &&
xside < block_size*sqrt(2)+tol &&
yside > block_size-tol &&
yside < block_size*sqrt(2)+tol )
{
// -------------------------------------------------------------------------------------------------------
// Get the four farthest corners of the block - use OpenCV only on the region identified by PCL
// Get the pixel coordinates of the xmax and ymax indicies
int px_xmax = 0; int py_xmax = 0;
int px_ymax = 0; int py_ymax = 0;
getXYCoordinates( xmaxi, image_height, image_width, px_xmax, py_xmax);
getXYCoordinates( ymaxi, image_height, image_width, px_ymax, py_ymax);
// Get the pixel coordinates of the xmin and ymin indicies
int px_xmin = 0; int py_xmin = 0;
int px_ymin = 0; int py_ymin = 0;
getXYCoordinates( xmini, image_height, image_width, px_xmin, py_xmin);
getXYCoordinates( ymini, image_height, image_width, px_ymin, py_ymin);
ROS_DEBUG_STREAM_NAMED("perception","px_xmin " << px_xmin << " px_xmax: " << px_xmax << " py_ymin: " << py_ymin << " py_ymax: " << py_ymax );
// -------------------------------------------------------------------------------------------------------
// Change the frame of reference from the robot to the camera
// Create an array of all the x value options
const int x_values_a[] = {px_xmax, px_ymax, px_xmin, px_ymin};
const int y_values_a[] = {py_xmax, py_ymax, py_xmin, py_ymin};
// Turn it into a vector
std::vector<int> x_values (x_values_a, x_values_a + sizeof(x_values_a) / sizeof(x_values_a[0]));
std::vector<int> y_values (y_values_a, y_values_a + sizeof(y_values_a) / sizeof(y_values_a[0]));
// Find the min
int x1 = *std::min_element(x_values.begin(), x_values.end());
int y1 = *std::min_element(y_values.begin(), y_values.end());
// Find the max
int x2 = *std::max_element(x_values.begin(), x_values.end());
int y2 = *std::max_element(y_values.begin(), y_values.end());
ROS_DEBUG_STREAM_NAMED("perception","x1: " << x1 << " y1: " << y1 << " x2: " << x2 << " y2: " << y2);
// -------------------------------------------------------------------------------------------------------
// Expand the ROI by a fudge factor, if possible
const int FUDGE_FACTOR = 5; // pixels
if( x1 > FUDGE_FACTOR)
x1 -= FUDGE_FACTOR;
if( y1 > FUDGE_FACTOR )
y1 -= FUDGE_FACTOR;
if( x2 < image_width - FUDGE_FACTOR )
x2 += FUDGE_FACTOR;
if( y2 < image_height - FUDGE_FACTOR )
y2 += FUDGE_FACTOR;
ROS_DEBUG_STREAM_NAMED("perception","After Fudge Factor - x1: " << x1 << " y1: " << y1 << " x2: " << x2 << " y2: " << y2);
// -------------------------------------------------------------------------------------------------------
// Create ROI parameters
// (x1,y1)----------------------
// | |
// | ROI |
// | |
// |_____________________(x2,y2)|
// Create Region of Interest
int roi_width = x2 - x1;
int roi_height = y2 - y1;
cv::Rect region_of_interest = cv::Rect( x1, y1, roi_width, roi_height );
ROS_DEBUG_STREAM_NAMED("perception","ROI: x " << x1 << " -- y " << y1 << " -- height " << roi_height << " -- width " << roi_width );
// -------------------------------------------------------------------------------------------------------
// Find paramters of the block in pixel coordiantes
int block_center_x = x1 + 0.5*roi_width;
int block_center_y = y1 + 0.5*roi_height;
int block_center_z = block_size / 2; // TODO: make this better
const cv::Point block_center = cv::Point( block_center_x, block_center_y );
// -------------------------------------------------------------------------------------------------------
// Create a sub image of just the block
cv::Point a1 = cv::Point(x1, y1);
cv::Point a2 = cv::Point(x2, y2);
cv::rectangle( output_image, a1, a2, cv::Scalar(0, 255, 255), 1, 8);
// Crop image (doesn't actually copy the data)
cropped_image = full_input_image_gray(region_of_interest);
// -------------------------------------------------------------------------------------------------------
// Detect edges using canny
ROS_INFO_STREAM_NAMED("perception","Detecting edges using canny");
// Find edges
cv::Mat canny_output;
cv::Canny( cropped_image, canny_output, canny_threshold, canny_threshold*2, 3 );
// Get mini window stats
const int mini_width = canny_output.size.p[1];
const int mini_height = canny_output.size.p[0];
const cv::Size mini_size = canny_output.size();
const cv::Point mini_center = cv::Point( mini_width/2, mini_height/2 );
// Find contours
vector<vector<cv::Point> > contours;
vector<cv::Vec4i> hierarchy;
cv::findContours( canny_output, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE, cv::Point(0, 0) );
ROS_INFO_STREAM_NAMED("perception","Contours");
// Draw contours
cv::Mat drawing = cv::Mat::zeros( mini_size, CV_8UC3 );
ROS_INFO_STREAM_NAMED("perception","Drawing contours");
// Find the largest contour for getting the angle
double max_contour_length = 0;
int max_contour_length_i;
for( size_t i = 0; i< contours.size(); i++ )
{
double contour_length = cv::arcLength( contours[i], false );
if( contour_length > max_contour_length )
{
max_contour_length = contour_length;
max_contour_length_i = i;
}
//ROS_DEBUG_STREAM_NAMED("perception","Contour length = " << contour_length << " of index " << max_contour_length_i);
cv::Scalar color = cv::Scalar( (30 + i*10) % 255, (30 + i*10) % 255, (30 + i*10) % 255);
cv::drawContours( drawing, contours, (int)i, color, 1, 8, hierarchy, 0, cv::Point() );
//drawContours( image, contours, contourIdx, color, thickness, lineType, hierarchy, maxLevel, offset )
}
// -------------------------------------------------------------------------------------------------------
// Copy largest contour to main image
cv::Scalar color = cv::Scalar( 0, 255, 0 );
cv::drawContours( output_image, contours, (int)max_contour_length_i, color, 1, 8, hierarchy, 0, a1 );
//drawContours( image, contours, contourIdx, color, thickness, lineType, hierarchy, maxLevel, offset )
// -------------------------------------------------------------------------------------------------------
// Copy largest contour to seperate image
cv::Mat hough_input = cv::Mat::zeros( mini_size, CV_8UC1 );
cv::Mat hough_input_color;
cv::Scalar hough_color = cv::Scalar( 200 );
cv::drawContours( hough_input, contours, (int)max_contour_length_i, hough_color, 1, 8, hierarchy, 0 );
cv::cvtColor(hough_input, hough_input_color, CV_GRAY2BGR);
// -------------------------------------------------------------------------------------------------------
// Hough Transform
cv::Mat hough_drawing = cv::Mat::zeros( mini_size, CV_8UC3 );
std::vector<cv::Vec4i> lines;
ROS_DEBUG_STREAM_NAMED("perception","hough_rho " << hough_rho << " hough_theta " << hough_theta <<
" theta_converted " << (1/hough_theta)*CV_PI/180 << " hough_threshold " <<
hough_threshold << " hough_minLineLength " << hough_minLineLength <<
" hough_maxLineGap " << hough_maxLineGap );
cv::HoughLinesP(hough_input, lines, hough_rho, (1/hough_theta)*CV_PI/180, hough_threshold, hough_minLineLength, hough_maxLineGap);
ROS_WARN_STREAM_NAMED("perception","Found " << lines.size() << " lines");
std::vector<double> line_angles;
// Copy detected lines to the drawing image
for( size_t i = 0; i < lines.size(); i++ )
{
cv::Vec4i line = lines[i];
cv::line( hough_drawing, cv::Point(line[0], line[1]), cv::Point(line[2], line[3]),
cv::Scalar(255,255,255), 1, CV_AA);
// Error check
if(line[3] - line[1] == 0 && line[2] - line[0] == 0)
{
ROS_ERROR_STREAM_NAMED("perception","Line is actually two points at the origin, unable to calculate. TODO: handle better?");
continue;
}
// Find angle
double line_angle = atan2(line[3] - line[1], line[2] - line[0]); //in radian, degrees: * 180.0 / CV_PI;
// Reverse angle direction if negative
if( line_angle < 0 )
{
line_angle += CV_PI;
}
line_angles.push_back(line_angle);
ROS_DEBUG_STREAM_NAMED("perception","Hough Line angle: " << line_angle * 180.0 / CV_PI;);
}
double block_angle = 0; // the overall result of the block's angle
// Everything is based on the first angle
if( line_angles.size() == 0 ) // make sure we have at least 1 angle
{
ROS_ERROR_STREAM_NAMED("perception","No lines were found for this cluster, unable to calculate block angle");
}
else
{
calculateBlockAngle( line_angles, block_angle );
}
// -------------------------------------------------------------------------------------------------------
// Draw chosen angle
ROS_INFO_STREAM_NAMED("perception","Using block angle " << block_angle*180.0/CV_PI);
// Draw chosen angle on mini image
cv::Mat angle_drawing = cv::Mat::zeros( mini_size, CV_8UC3 );
int line_length = 0.5*double(mini_width); // have the line go 1/4 across the screen
int new_x = mini_center.x + line_length*cos( block_angle );
int new_y = mini_center.y + line_length*sin( block_angle );
ROS_INFO_STREAM("Origin (" << mini_center.x << "," << mini_center.y << ") New (" << new_x << "," << new_y <<
") length " << line_length << " angle " << block_angle <<
" mini width " << mini_width << " mini height " << mini_height);
cv::Point angle_point = cv::Point(new_x, new_y);
cv::line( angle_drawing, mini_center, angle_point, cv::Scalar(255,255,255), 1, CV_AA);
// Draw chosen angle on contours image
cv::line( hough_drawing, mini_center, angle_point, cv::Scalar(255,0, 255), 1, CV_AA);
// Draw chosen angle on main image
line_length = 0.75 * double(mini_width); // have the line go 1/2 across the box
new_x = block_center_x + line_length*cos( block_angle );
new_y = block_center_y + line_length*sin( block_angle );
ROS_INFO_STREAM_NAMED("perception",block_center_x << ", " << block_center_y << ", " << new_x << ", " << new_y);
angle_point = cv::Point(new_x, new_y);
cv::line( output_image, block_center, angle_point, cv::Scalar(255,0,255), 2, CV_AA);
// -------------------------------------------------------------------------------------------------------
// Get world coordinates
// Find the block's center point
double world_x1 = xmin+(xside)/2.0;
double world_y1 = ymin+(yside)/2.0;
double world_z1 = table_height + block_size / 2;
// Convert pixel coordiantes back to world coordinates
double world_x2 = cloud_transformed->at(new_x, new_y).x;
double world_y2 = cloud_transformed->at(new_x, new_y).y;
double world_z2 = world_z1; // is this even necessary?
// Get angle from two world coordinates...
double world_theta = abs( atan2(world_y2 - world_y1, world_x2 - world_x1) );
// Attempt to make all angles point in same direction
makeAnglesUniform( world_theta );
// -------------------------------------------------------------------------------------------------------
// GUI Stuff
// Copy the cluster image to the main image in the top left corner
if( top_image_overlay_x + mini_width < image_width )
{
const int common_height = 42;
cv::Rect small_roi_row0 = cv::Rect(top_image_overlay_x, common_height*0, mini_width, mini_height);
cv::Rect small_roi_row1 = cv::Rect(top_image_overlay_x, common_height*1, mini_width, mini_height);
cv::Rect small_roi_row2 = cv::Rect(top_image_overlay_x, common_height*2, mini_width, mini_height);
cv::Rect small_roi_row3 = cv::Rect(top_image_overlay_x, common_height*3, mini_width, mini_height);
drawing.copyTo( output_image(small_roi_row0) );
hough_input_color.copyTo( output_image(small_roi_row1) );
hough_drawing.copyTo( output_image(small_roi_row2) );
angle_drawing.copyTo( output_image(small_roi_row3) );
top_image_overlay_x += mini_width;
}
// figure out the position and the orientation of the block
//double angle = atan(block_size/((xside+yside)/2));
//double angle = atan( (xmaxy - xminy) / (xmax - xmin ) );
// Then add it to our set
//addBlock( xmin+(xside)/2.0, ymin+(yside)/2.0, zmax - block_size/2.0, angle);
//ROS_INFO_STREAM_NAMED("perception","FOUND -> xside: " << xside << " yside: " << yside << " angle: " << block_angle);
addBlock( world_x1, world_y1, world_z1, world_theta );
//addBlock( block_center_x, block_center_y, block_center_z, block_angle);
}
else
{
void TOea_Planner::goalCB(const geometry_msgs::PoseStamped::ConstPtr& goal_msg)
{
if (!map_received_)
{
ROS_ERROR_NAMED(logger_name_, "No map received yet. Unable to compute path.");
return;
}
planner_state.data = hardware::BUSY; //PLANNING;
state_pub_.publish(planner_state);
ROS_DEBUG_NAMED(logger_name_, "New Goal received on topic");
// get world pose from msg
Astar_.goal_world_pose_.x = goal_msg->pose.position.x;
Astar_.goal_world_pose_.y = goal_msg->pose.position.y;
Astar_.goal_world_pose_.yaw = tf::getYaw(goal_msg->pose.orientation);
oea_msgs::Oea_path oea_path; // plan variable
// oea_path.path.poses.clear();
//check validity:
if (Astar_.goal_world_pose_.yaw!=Astar_.goal_world_pose_.yaw) //if nan
{
ROS_ERROR_STREAM_NAMED(logger_name_, "Invalig Goal: yaw is " << to_degrees(Astar_.goal_world_pose_.yaw));
//else just publish the blank path
oea_path_pub_.publish(oea_path); //publishing 0 poses will cause the controller to stop following the previous path
visual_path_pub_.publish(oea_path.path); // publish nav_msgs/Path to view path on rviz
planner_state.data = hardware::IDLE;
state_pub_.publish(planner_state);
return;
}
//convert it to grid coord
Astar_.ConvertWorlCoordToMatrix(Astar_.goal_world_pose_.x, Astar_.goal_world_pose_.y, Astar_.goal_world_pose_.yaw , Astar_.goal_grid_pose_.x, Astar_.goal_grid_pose_.y, Astar_.goal_grid_pose_.z);
std::string error_str;
// check if goal is valid
//error_str = "GoalCb: check grid pose val";
if (Astar_.is_valid_point(Astar_.goal_grid_pose_,error_str))
{
//get path from the Astar...
executeCycle(error_str, oea_path);
}
else
{
ROS_WARN_STREAM_NAMED(logger_name_, error_str);
// no need to clear Grid, because Astar was not called
// publish empty plan to stop the robot in case it's moving and received a new point...
oea_path_pub_.publish(oea_path);
visual_path_pub_.publish(oea_path.path);
}
planner_state.data = hardware::IDLE;
state_pub_.publish(planner_state);
}
void GenericHWInterface::registerJointLimits(const hardware_interface::JointHandle &joint_handle_position,
const hardware_interface::JointHandle &joint_handle_velocity,
const hardware_interface::JointHandle &joint_handle_effort,
std::size_t joint_id)
{
// Default values
joint_position_lower_limits_[joint_id] = -std::numeric_limits<double>::max();
joint_position_upper_limits_[joint_id] = std::numeric_limits<double>::max();
joint_velocity_limits_[joint_id] = std::numeric_limits<double>::max();
joint_effort_limits_[joint_id] = std::numeric_limits<double>::max();
// Limits datastructures
joint_limits_interface::JointLimits joint_limits; // Position
joint_limits_interface::SoftJointLimits soft_limits; // Soft Position
bool has_joint_limits = false;
bool has_soft_limits = false;
// Get limits from URDF
if (urdf_model_ == NULL)
{
ROS_WARN_STREAM_NAMED("generic_hw_interface", "No URDF model loaded, unable to get joint limits");
return;
}
// Get limits from URDF
const boost::shared_ptr<const urdf::Joint> urdf_joint = urdf_model_->getJoint(joint_names_[joint_id]);
// Get main joint limits
if (urdf_joint == NULL)
{
ROS_ERROR_STREAM_NAMED("generic_hw_interface", "URDF joint not found " << joint_names_[joint_id]);
return;
}
// Get limits from URDF
if (joint_limits_interface::getJointLimits(urdf_joint, joint_limits))
{
has_joint_limits = true;
ROS_DEBUG_STREAM_NAMED("generic_hw_interface", "Joint " << joint_names_[joint_id] << " has URDF position limits ["
<< joint_limits.min_position << ", "
<< joint_limits.max_position << "]");
if (joint_limits.has_velocity_limits)
ROS_DEBUG_STREAM_NAMED("generic_hw_interface", "Joint " << joint_names_[joint_id] << " has URDF velocity limit ["
<< joint_limits.max_velocity << "]");
}
else
{
if (urdf_joint->type != urdf::Joint::CONTINUOUS)
ROS_WARN_STREAM_NAMED("generic_hw_interface", "Joint " << joint_names_[joint_id] << " does not have a URDF "
"position limit");
}
// Get limits from ROS param
if (use_rosparam_joint_limits_)
{
if (joint_limits_interface::getJointLimits(joint_names_[joint_id], nh_, joint_limits))
{
has_joint_limits = true;
ROS_DEBUG_STREAM_NAMED("generic_hw_interface",
"Joint " << joint_names_[joint_id] << " has rosparam position limits ["
<< joint_limits.min_position << ", " << joint_limits.max_position << "]");
if (joint_limits.has_velocity_limits)
ROS_DEBUG_STREAM_NAMED("generic_hw_interface", "Joint " << joint_names_[joint_id]
<< " has rosparam velocity limit ["
<< joint_limits.max_velocity << "]");
} // the else debug message provided internally by joint_limits_interface
}
// Get soft limits from URDF
if (joint_limits_interface::getSoftJointLimits(urdf_joint, soft_limits))
{
has_soft_limits = true;
ROS_DEBUG_STREAM_NAMED("generic_hw_interface", "Joint " << joint_names_[joint_id] << " has soft joint limits.");
}
else
{
ROS_DEBUG_STREAM_NAMED("generic_hw_interface", "Joint " << joint_names_[joint_id] << " does not have soft joint "
"limits");
}
// Quit we we haven't found any limits in URDF or rosparam server
if (!has_joint_limits)
{
return;
}
// Copy position limits if available
if (joint_limits.has_position_limits)
{
// Slighly reduce the joint limits to prevent floating point errors
joint_limits.min_position += std::numeric_limits<double>::epsilon();
joint_limits.max_position -= std::numeric_limits<double>::epsilon();
joint_position_lower_limits_[joint_id] = joint_limits.min_position;
joint_position_upper_limits_[joint_id] = joint_limits.max_position;
}
// Copy velocity limits if available
if (joint_limits.has_velocity_limits)
{
joint_velocity_limits_[joint_id] = joint_limits.max_velocity;
}
// Copy effort limits if available
if (joint_limits.has_effort_limits)
{
joint_effort_limits_[joint_id] = joint_limits.max_effort;
}
if (has_soft_limits) // Use soft limits
{
ROS_DEBUG_STREAM_NAMED("generic_hw_interface", "Using soft saturation limits");
const joint_limits_interface::PositionJointSoftLimitsHandle soft_handle_position(joint_handle_position,
joint_limits, soft_limits);
pos_jnt_soft_limits_.registerHandle(soft_handle_position);
const joint_limits_interface::VelocityJointSoftLimitsHandle soft_handle_velocity(joint_handle_velocity,
joint_limits, soft_limits);
vel_jnt_soft_limits_.registerHandle(soft_handle_velocity);
const joint_limits_interface::EffortJointSoftLimitsHandle soft_handle_effort(joint_handle_effort, joint_limits,
soft_limits);
eff_jnt_soft_limits_.registerHandle(soft_handle_effort);
}
else // Use saturation limits
{
ROS_DEBUG_STREAM_NAMED("generic_hw_interface", "Using saturation limits (not soft limits)");
const joint_limits_interface::PositionJointSaturationHandle sat_handle_position(joint_handle_position, joint_limits);
pos_jnt_sat_interface_.registerHandle(sat_handle_position);
const joint_limits_interface::VelocityJointSaturationHandle sat_handle_velocity(joint_handle_velocity, joint_limits);
vel_jnt_sat_interface_.registerHandle(sat_handle_velocity);
const joint_limits_interface::EffortJointSaturationHandle sat_handle_effort(joint_handle_effort, joint_limits);
eff_jnt_sat_interface_.registerHandle(sat_handle_effort);
}
}
void BenchmarkNode::run()
{
// setup visualizer
dvo::visualization::Switch pause_switch(true), dummy_switch(true);
dvo::visualization::CameraTrajectoryVisualizerInterface* visualizer;
if(cfg_.VisualizationRequired())
{
visualizer = new dvo::visualization::PclCameraTrajectoryVisualizer(!cfg_.RenderVideo);
((dvo::visualization::PclCameraTrajectoryVisualizer*) visualizer)->bindSwitchToKey(pause_switch, "p");
((dvo::visualization::PclCameraTrajectoryVisualizer*) visualizer)->bindSwitchToKey(dump_camera_pose_, "s");
if(cfg_.RenderVideo)
{
((dvo::visualization::PclCameraTrajectoryVisualizer*) visualizer)->bindSwitchToKey(load_camera_pose_, "l");
((dvo::visualization::PclCameraTrajectoryVisualizer*) visualizer)->visualizer().getRenderWindow()->SetSize(1280, 960);
}
}
else
{
visualizer = new dvo::visualization::NoopCameraTrajectoryVisualizer();
}
dvo::util::IdGenerator frame_ids(cfg_.VideoFolder + std::string("/frame_"));
// configure debugging visualizer
dvo::visualization::Visualizer::instance()
.useExternalWaitKey(false)
.enabled(false)
.save(false)
;
// setup camera parameters
// TODO: load from file
//dvo::core::IntrinsicMatrix intrinsics = dvo::core::IntrinsicMatrix::create(525.0f, 525.0f, 320.0f, 240.0f);
dvo::core::IntrinsicMatrix intrinsics = dvo::core::IntrinsicMatrix::create(517.3, 516.5, 318.6, 255.3);
// setup tracker configuration
dvo_ros::CameraDenseTrackerConfig dynreconfg_cfg = dvo_ros::CameraDenseTrackerConfig::__getDefault__();
dynreconfg_cfg.__fromServer__(nh_private_);
dvo::DenseTracker::Config cfg = dvo::DenseTracker::getDefaultConfig();
dvo_ros::util::updateConfigFromDynamicReconfigure(dynreconfg_cfg, cfg);
ROS_WARN_STREAM_NAMED("config", "config: \"" << cfg << "\"");
// setup tracker
dvo::DenseTracker dense_tracker(intrinsics, cfg);
// initialize first pose
Eigen::Affine3d trajectory, relative;
if(groundtruth_reader_ != 0)
{
dvo_benchmark::findClosestEntry(*groundtruth_reader_, rgbdpair_reader_->entry().RgbTimestamp());
dvo_benchmark::toPoseEigen(groundtruth_reader_->entry(), trajectory);
}
else
{
trajectory.setIdentity();
}
std::string folder = cfg_.RgbdPairFile.substr(0, cfg_.RgbdPairFile.find_last_of("/") + 1);
std::vector<dvo_benchmark::RgbdPair> pairs;
rgbdpair_reader_->readAllEntries(pairs);
dvo::core::RgbdImagePyramid::Ptr reference, current;
for(std::vector<dvo_benchmark::RgbdPair>::iterator it = pairs.begin(); ros::ok() && it != pairs.end(); ++it)
{
reference = current;
current = load(folder + it->RgbFile(), folder + it->DepthFile());
if(!reference)
{
createReferenceCamera(visualizer, current->level(0), intrinsics, trajectory);
}
// pause in the beginning
renderWhileSwitchAndNotTerminated(visualizer, pause_switch);
processInput(visualizer);
if(!reference)
{
continue;
}
if((it->RgbTimestamp() - pairs.front().RgbTimestamp()).toSec() < 5 || (pairs.back().RgbTimestamp() - it->RgbTimestamp()).toSec() < 5)
{
visualizer->camera("reference")->show();
}
else
{
visualizer->camera("reference")->hide();
}
if(cfg_.ShowGroundtruth)
{
Eigen::Affine3d groundtruth_pose;
dvo_benchmark::findClosestEntry(*groundtruth_reader_, it->RgbTimestamp());
dvo_benchmark::toPoseEigen(groundtruth_reader_->entry(), groundtruth_pose);
visualizer->trajectory("groundtruth")->
color(dvo::visualization::Color::green()).
add(groundtruth_pose);
visualizer->camera("groundtruth")->
color(dvo::visualization::Color::green()).
update(current->level(0), intrinsics, groundtruth_pose).
show(cfg_.ShowEstimate ? dvo::visualization::CameraVisualizer::ShowCamera : dvo::visualization::CameraVisualizer::ShowCameraAndCloud);
}
if(cfg_.EstimateRequired())
{
static dvo::util::stopwatch sw_match("match", 100);
sw_match.start();
{
dense_tracker.match(*reference, *current, relative);
}
sw_match.stopAndPrint();
trajectory = trajectory * relative;
if(cfg_.EstimateTrajectory)
{
Eigen::Quaterniond q(trajectory.rotation());
(*trajectory_out_)
<< it->RgbTimestamp() << " "
<< trajectory.translation()(0) << " "
<< trajectory.translation()(1) << " "
<< trajectory.translation()(2) << " "
<< q.x() << " "
<< q.y() << " "
<< q.z() << " "
<< q.w() << " "
<< std::endl;
}
if(cfg_.ShowEstimate)
{
visualizer->trajectory("estimate")->
color(dvo::visualization::Color::red()).
add(trajectory);
visualizer->camera("estimate")->
color(dvo::visualization::Color::red()).
update(current->level(0), intrinsics, trajectory).
show(dvo::visualization::CameraVisualizer::ShowCameraAndCloud);
}
}
if(cfg_.RenderVideo)
{
((dvo::visualization::PclCameraTrajectoryVisualizer*) visualizer)->render(5);
((dvo::visualization::PclCameraTrajectoryVisualizer*) visualizer)->visualizer().saveScreenshot(frame_ids.next() + std::string(".png"));
}
}
// keep visualization alive
renderWhileSwitchAndNotTerminated(visualizer, dummy_switch);
}
// Return grasps that are kinematically feasible
bool GraspFilter::filterGrasps(std::vector<moveit_msgs::Grasp>& possible_grasps,
std::vector<trajectory_msgs::JointTrajectoryPoint>& ik_solutions, bool filter_pregrasp,
const std::string &ee_parent_link, const std::string& planning_group)
{
// -----------------------------------------------------------------------------------------------
// Error check
if( possible_grasps.empty() )
{
ROS_ERROR_NAMED("filter","Unable to filter grasps because vector is empty");
return false;
}
// -----------------------------------------------------------------------------------------------
// how many cores does this computer have and how many do we need?
int num_threads = boost::thread::hardware_concurrency();
if( num_threads > possible_grasps.size() )
num_threads = possible_grasps.size();
if(false)
{
num_threads = 1;
ROS_WARN_STREAM_NAMED("grasp_filter","Using only " << num_threads << " threads");
}
// -----------------------------------------------------------------------------------------------
// Get the solver timeout from kinematics.yaml
double timeout = robot_state_.getRobotModel()->getJointModelGroup( planning_group )->getDefaultIKTimeout();
timeout = 0.05;
ROS_DEBUG_STREAM_NAMED("grasp_filter","Grasp filter IK timeout " << timeout);
// -----------------------------------------------------------------------------------------------
// Load kinematic solvers if not already loaded
if( kin_solvers_[planning_group].size() != num_threads )
{
kin_solvers_[planning_group].clear();
const robot_model::JointModelGroup* jmg = robot_state_.getRobotModel()->getJointModelGroup(planning_group);
// Create an ik solver for every thread
for (int i = 0; i < num_threads; ++i)
{
//ROS_INFO_STREAM_NAMED("filter","Creating ik solver " << i);
kin_solvers_[planning_group].push_back(jmg->getSolverInstance());
// Test to make sure we have a valid kinematics solver
if( !kin_solvers_[planning_group][i] )
{
ROS_ERROR_STREAM_NAMED("grasp_filter","No kinematic solver found");
return false;
}
}
}
// Transform poses -------------------------------------------------------------------------------
// bring the pose to the frame of the IK solver
const std::string &ik_frame = kin_solvers_[planning_group][0]->getBaseFrame();
Eigen::Affine3d link_transform;
if (!moveit::core::Transforms::sameFrame(ik_frame, robot_state_.getRobotModel()->getModelFrame()))
{
const robot_model::LinkModel *lm = robot_state_.getLinkModel((!ik_frame.empty() && ik_frame[0] == '/') ? ik_frame.substr(1) : ik_frame);
if (!lm)
return false;
//pose = getGlobalLinkTransform(lm).inverse() * pose;
link_transform = robot_state_.getGlobalLinkTransform(lm).inverse();
}
// Benchmark time
ros::Time start_time;
start_time = ros::Time::now();
// -----------------------------------------------------------------------------------------------
// Loop through poses and find those that are kinematically feasible
std::vector<moveit_msgs::Grasp> filtered_grasps;
boost::thread_group bgroup; // create a group of threads
boost::mutex lock; // used for sharing the same data structures
ROS_INFO_STREAM_NAMED("filter", "Filtering possible grasps with " << num_threads << " threads");
// split up the work between threads
double num_grasps_per_thread = double(possible_grasps.size()) / num_threads;
//ROS_INFO_STREAM("total grasps " << possible_grasps.size() << " per thead: " << num_grasps_per_thread);
int grasps_id_start;
int grasps_id_end = 0;
for(int i = 0; i < num_threads; ++i)
{
grasps_id_start = grasps_id_end;
grasps_id_end = ceil(num_grasps_per_thread*(i+1));
if( grasps_id_end >= possible_grasps.size() )
grasps_id_end = possible_grasps.size();
//ROS_INFO_STREAM_NAMED("filter","low " << grasps_id_start << " high " << grasps_id_end);
IkThreadStruct tc(possible_grasps, filtered_grasps, ik_solutions, link_transform, grasps_id_start,
grasps_id_end, kin_solvers_[planning_group][i], filter_pregrasp, ee_parent_link, timeout, &lock, i);
bgroup.create_thread( boost::bind( &GraspFilter::filterGraspThread, this, tc ) );
}
ROS_DEBUG_STREAM_NAMED("filter","Waiting to join " << num_threads << " ik threads...");
bgroup.join_all(); // wait for all threads to finish
ROS_INFO_STREAM_NAMED("filter", "Grasp filter complete, found " << filtered_grasps.size() << " IK solutions out of " <<
possible_grasps.size() );
possible_grasps = filtered_grasps;
if (verbose_)
{
// End Benchmark time
double duration = (ros::Time::now() - start_time).toNSec() * 1e-6;
ROS_INFO_STREAM_NAMED("filter","Grasp generator IK grasp filtering benchmark time:");
std::cout << duration << "\t" << possible_grasps.size() << "\n";
ROS_INFO_STREAM_NAMED("filter","Possible grasps filtered to " << possible_grasps.size() << " options.");
}
return true;
}
bool DefaultRobotHWSim::initSim(
const std::string& robot_namespace,
ros::NodeHandle model_nh,
gazebo::physics::ModelPtr parent_model,
const urdf::Model *const urdf_model,
std::vector<transmission_interface::TransmissionInfo> transmissions)
{
// getJointLimits() searches joint_limit_nh for joint limit parameters. The format of each
// parameter's name is "joint_limits/<joint name>". An example is "joint_limits/axle_joint".
const ros::NodeHandle joint_limit_nh(model_nh, robot_namespace);
// Resize vectors to our DOF
n_dof_ = transmissions.size();
joint_names_.resize(n_dof_);
joint_types_.resize(n_dof_);
joint_lower_limits_.resize(n_dof_);
joint_upper_limits_.resize(n_dof_);
joint_effort_limits_.resize(n_dof_);
joint_control_methods_.resize(n_dof_);
pid_controllers_.resize(n_dof_);
joint_position_.resize(n_dof_);
joint_velocity_.resize(n_dof_);
joint_effort_.resize(n_dof_);
joint_effort_command_.resize(n_dof_);
joint_position_command_.resize(n_dof_);
joint_velocity_command_.resize(n_dof_);
// Initialize values
for(unsigned int j=0; j < n_dof_; j++)
{
// Check that this transmission has one joint
if(transmissions[j].joints_.size() == 0)
{
ROS_WARN_STREAM_NAMED("default_robot_hw_sim","Transmission " << transmissions[j].name_
<< " has no associated joints.");
continue;
}
else if(transmissions[j].joints_.size() > 1)
{
ROS_WARN_STREAM_NAMED("default_robot_hw_sim","Transmission " << transmissions[j].name_
<< " has more than one joint. Currently the default robot hardware simulation "
<< " interface only supports one.");
continue;
}
std::vector<std::string> joint_interfaces = transmissions[j].joints_[0].hardware_interfaces_;
if (joint_interfaces.empty() &&
!(transmissions[j].actuators_.empty()) &&
!(transmissions[j].actuators_[0].hardware_interfaces_.empty()))
{
// TODO: Deprecate HW interface specification in actuators in ROS J
joint_interfaces = transmissions[j].actuators_[0].hardware_interfaces_;
ROS_WARN_STREAM_NAMED("default_robot_hw_sim", "The <hardware_interface> element of tranmission " <<
transmissions[j].name_ << " should be nested inside the <joint> element, not <actuator>. " <<
"The transmission will be properly loaded, but please update " <<
"your robot model to remain compatible with future versions of the plugin.");
}
if (joint_interfaces.empty())
{
ROS_WARN_STREAM_NAMED("default_robot_hw_sim", "Joint " << transmissions[j].joints_[0].name_ <<
" of transmission " << transmissions[j].name_ << " does not specify any hardware interface. " <<
"Not adding it to the robot hardware simulation.");
continue;
}
else if (joint_interfaces.size() > 1)
{
ROS_WARN_STREAM_NAMED("default_robot_hw_sim", "Joint " << transmissions[j].joints_[0].name_ <<
" of transmission " << transmissions[j].name_ << " specifies multiple hardware interfaces. " <<
"Currently the default robot hardware simulation interface only supports one. Using the first entry!");
//continue;
}
// Add data from transmission
joint_names_[j] = transmissions[j].joints_[0].name_;
joint_position_[j] = 1.0;
joint_velocity_[j] = 0.0;
joint_effort_[j] = 1.0; // N/m for continuous joints
joint_effort_command_[j] = 0.0;
joint_position_command_[j] = 0.0;
joint_velocity_command_[j] = 0.0;
const std::string& hardware_interface = joint_interfaces.front();
// Debug
ROS_DEBUG_STREAM_NAMED("default_robot_hw_sim","Loading joint '" << joint_names_[j]
<< "' of type '" << hardware_interface << "'");
// Create joint state interface for all joints
js_interface_.registerHandle(hardware_interface::JointStateHandle(
joint_names_[j], &joint_position_[j], &joint_velocity_[j], &joint_effort_[j]));
// Decide what kind of command interface this actuator/joint has
hardware_interface::JointHandle joint_handle;
if(hardware_interface == "EffortJointInterface")
{
// Create effort joint interface
joint_control_methods_[j] = EFFORT;
joint_handle = hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[j]),
&joint_effort_command_[j]);
ej_interface_.registerHandle(joint_handle);
}
else if(hardware_interface == "PositionJointInterface")
{
// Create position joint interface
joint_control_methods_[j] = POSITION;
joint_handle = hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[j]),
&joint_position_command_[j]);
pj_interface_.registerHandle(joint_handle);
}
else if(hardware_interface == "VelocityJointInterface")
{
// Create velocity joint interface
joint_control_methods_[j] = VELOCITY;
joint_handle = hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[j]),
&joint_velocity_command_[j]);
vj_interface_.registerHandle(joint_handle);
}
else
{
ROS_FATAL_STREAM_NAMED("default_robot_hw_sim","No matching hardware interface found for '"
<< hardware_interface );
return false;
}
// Get the gazebo joint that corresponds to the robot joint.
//ROS_DEBUG_STREAM_NAMED("default_robot_hw_sim", "Getting pointer to gazebo joint: "
// << joint_names_[j]);
gazebo::physics::JointPtr joint = parent_model->GetJoint(joint_names_[j]);
if (!joint)
{
ROS_ERROR_STREAM("This robot has a joint named \"" << joint_names_[j]
<< "\" which is not in the gazebo model.");
return false;
}
sim_joints_.push_back(joint);
registerJointLimits(joint_names_[j], joint_handle, joint_control_methods_[j],
joint_limit_nh, urdf_model,
&joint_types_[j], &joint_lower_limits_[j], &joint_upper_limits_[j],
&joint_effort_limits_[j]);
if (joint_control_methods_[j] != EFFORT)
{
// Initialize the PID controller. If no PID gain values are found, use joint->SetAngle() or
// joint->SetVelocity() to control the joint.
const ros::NodeHandle nh(model_nh, robot_namespace + "/gazebo_ros_control/pid_gains/" +
joint_names_[j]);
if (pid_controllers_[j].init(nh, true))
{
switch (joint_control_methods_[j])
{
case POSITION:
joint_control_methods_[j] = POSITION_PID;
break;
case VELOCITY:
joint_control_methods_[j] = VELOCITY_PID;
break;
}
}
else
{
// joint->SetMaxForce() must be called if joint->SetAngle() or joint->SetVelocity() are
// going to be called. joint->SetMaxForce() must *not* be called if joint->SetForce() is
// going to be called.
//joint->SetMaxForce(0, joint_effort_limits_[j]);
joint->SetParam("max_force", 0, joint_effort_limits_[j]);
}
}
}
// Register interfaces
registerInterface(&js_interface_);
registerInterface(&ej_interface_);
registerInterface(&pj_interface_);
registerInterface(&vj_interface_);
// Initialize the emergency stop code.
e_stop_active_ = false;
last_e_stop_active_ = false;
return true;
}
// Overloaded Gazebo entry point
void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::ElementPtr sdf)
{
ROS_INFO_STREAM_NAMED("gazebo_ros_control","Loading gazebo_ros_control plugin");
// Save pointers to the model
parent_model_ = parent;
sdf_ = sdf;
// Error message if the model couldn't be found
if (!parent_model_)
{
ROS_ERROR_STREAM_NAMED("loadThread","parent model is NULL");
return;
}
// Check that ROS has been initialized
if(!ros::isInitialized())
{
ROS_FATAL_STREAM_NAMED("gazebo_ros_control","A ROS node for Gazebo has not been initialized, unable to load plugin. "
<< "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
return;
}
// Get namespace for nodehandle
if(sdf_->HasElement("robotNamespace"))
{
robot_namespace_ = sdf_->GetElement("robotNamespace")->Get<std::string>();
}
else
{
robot_namespace_ = parent_model_->GetName(); // default
}
// Get robot_description ROS param name
if (sdf_->HasElement("robotParam"))
{
robot_description_ = sdf_->GetElement("robotParam")->Get<std::string>();
}
else
{
robot_description_ = "robot_description"; // default
}
// Get the robot simulation interface type
if(sdf_->HasElement("robotSimType"))
{
robot_hw_sim_type_str_ = sdf_->Get<std::string>("robotSimType");
}
else
{
robot_hw_sim_type_str_ = "gazebo_ros_control/DefaultRobotHWSim";
ROS_DEBUG_STREAM_NAMED("loadThread","Using default plugin for RobotHWSim (none specified in URDF/SDF)\""<<robot_hw_sim_type_str_<<"\"");
}
// Get the Gazebo simulation period
ros::Duration gazebo_period(parent_model_->GetWorld()->GetPhysicsEngine()->GetMaxStepSize());
// Decide the plugin control period
if(sdf_->HasElement("controlPeriod"))
{
control_period_ = ros::Duration(sdf_->Get<double>("controlPeriod"));
// Check the period against the simulation period
if( control_period_ < gazebo_period )
{
ROS_ERROR_STREAM_NAMED("gazebo_ros_control","Desired controller update period ("<<control_period_
<<" s) is faster than the gazebo simulation period ("<<gazebo_period<<" s).");
}
else if( control_period_ > gazebo_period )
{
ROS_WARN_STREAM_NAMED("gazebo_ros_control","Desired controller update period ("<<control_period_
<<" s) is slower than the gazebo simulation period ("<<gazebo_period<<" s).");
}
}
else
{
control_period_ = gazebo_period;
ROS_DEBUG_STREAM_NAMED("gazebo_ros_control","Control period not found in URDF/SDF, defaulting to Gazebo period of "
<< control_period_);
}
// Get parameters/settings for controllers from ROS param server
model_nh_ = ros::NodeHandle(robot_namespace_);
// Initialize the emergency stop code.
e_stop_active_ = false;
last_e_stop_active_ = false;
if (sdf_->HasElement("eStopTopic"))
{
const std::string e_stop_topic = sdf_->GetElement("eStopTopic")->Get<std::string>();
e_stop_sub_ = model_nh_.subscribe(e_stop_topic, 1, &GazeboRosControlPlugin::eStopCB, this);
}
ROS_INFO_NAMED("gazebo_ros_control", "Starting gazebo_ros_control plugin in namespace: %s", robot_namespace_.c_str());
// Read urdf from ros parameter server then
// setup actuators and mechanism control node.
// This call will block if ROS is not properly initialized.
const std::string urdf_string = getURDF(robot_description_);
if (!parseTransmissionsFromURDF(urdf_string))
{
ROS_ERROR_NAMED("gazebo_ros_control", "Error parsing URDF in gazebo_ros_control plugin, plugin not active.\n");
return;
}
// Load the RobotHWSim abstraction to interface the controllers with the gazebo model
try
{
robot_hw_sim_loader_.reset
(new pluginlib::ClassLoader<gazebo_ros_control::RobotHWSim>
("gazebo_ros_control",
"gazebo_ros_control::RobotHWSim"));
robot_hw_sim_ = robot_hw_sim_loader_->createInstance(robot_hw_sim_type_str_);
urdf::Model urdf_model;
const urdf::Model *const urdf_model_ptr = urdf_model.initString(urdf_string) ? &urdf_model : NULL;
if(!robot_hw_sim_->initSim(robot_namespace_, model_nh_, parent_model_, urdf_model_ptr, transmissions_))
{
ROS_FATAL_NAMED("gazebo_ros_control","Could not initialize robot simulation interface");
return;
}
// Create the controller manager
ROS_DEBUG_STREAM_NAMED("ros_control_plugin","Loading controller_manager");
controller_manager_.reset
(new controller_manager::ControllerManager(robot_hw_sim_.get(), model_nh_));
// Listen to the update event. This event is broadcast every simulation iteration.
update_connection_ =
gazebo::event::Events::ConnectWorldUpdateBegin
(boost::bind(&GazeboRosControlPlugin::Update, this));
}
catch(pluginlib::LibraryLoadException &ex)
{
ROS_FATAL_STREAM_NAMED("gazebo_ros_control","Failed to create robot simulation interface loader: "<<ex.what());
}
ROS_INFO_NAMED("gazebo_ros_control", "Loaded gazebo_ros_control.");
}
void BenchmarkNode::run()
{
// setup visualizer
// dvo::visualization::Switch pause_switch(false), dummy_switch(true);
dvo::visualization::CameraTrajectoryVisualizerInterface* visualizer;
dvo_slam::visualization::GraphVisualizer* graph_visualizer;
if(cfg_.VisualizationRequired())
{
visualizer = new dvo_ros::visualization::RosCameraTrajectoryVisualizer(nh_vis_);
graph_visualizer = new dvo_slam::visualization::GraphVisualizer(*dynamic_cast<dvo_ros::visualization::RosCameraTrajectoryVisualizer*>(visualizer));
//((dvo::visualization::PclCameraTrajectoryVisualizer*) visualizer)->bindSwitchToKey(pause_switch, "p");
//((dvo::visualization::PclCameraTrajectoryVisualizer*) visualizer)->bindSwitchToKey(dump_camera_pose_, "s");
//
//if(cfg_.RenderVideo)
//{
// ((dvo::visualization::PclCameraTrajectoryVisualizer*) visualizer)->bindSwitchToKey(load_camera_pose_, "l");
// ((dvo::visualization::PclCameraTrajectoryVisualizer*) visualizer)->visualizer().getRenderWindow()->SetSize(1280, 960);
//}
}
else
{
visualizer = new dvo::visualization::NoopCameraTrajectoryVisualizer();
}
dvo::util::IdGenerator frame_ids(cfg_.VideoFolder + std::string("/frame_"));
// setup camera parameters
// TODO: load from file
//dvo::core::IntrinsicMatrix intrinsics = dvo::core::IntrinsicMatrix::create(525.0f, 525.0f, 320.0f, 240.0f);
//fr1
dvo::core::IntrinsicMatrix intrinsics = dvo::core::IntrinsicMatrix::create(517.3, 516.5, 318.6, 255.3);
//fr2
//dvo::core::IntrinsicMatrix intrinsics = dvo::core::IntrinsicMatrix::create(520.9f, 521.0f, 325.1f, 249.7f);
//fr3
//dvo::core::IntrinsicMatrix intrinsics = dvo::core::IntrinsicMatrix::create(535.4f, 539.2f, 320.1f, 247.6f);
dvo::core::RgbdCameraPyramid camera(640, 480, intrinsics);
// setup tracker configuration
dvo_ros::CameraDenseTrackerConfig dynreconfg_cfg = dvo_ros::CameraDenseTrackerConfig::__getDefault__();
dynreconfg_cfg.__fromServer__(nh_private_);
dvo::DenseTracker::Config cfg = dvo::DenseTracker::getDefaultConfig();
dvo_ros::util::updateConfigFromDynamicReconfigure(dynreconfg_cfg, cfg);
dvo_slam::KeyframeSlamConfig dynreconfg_slam_cfg = dvo_slam::KeyframeSlamConfig::__getDefault__();
dynreconfg_slam_cfg.__fromServer__(nh_private_);
dvo_slam::KeyframeTrackerConfig frontend_cfg;
dvo_slam::KeyframeGraphConfig backend_cfg;
dvo_slam::updateConfigFromDynamicReconfigure(dynreconfg_slam_cfg, frontend_cfg, backend_cfg);
// setup tracker
//dvo::DenseTracker dense_tracker(intrinsics, cfg);
camera.build(cfg.getNumLevels());
dvo_slam::KeyframeTracker keyframe_tracker(graph_visualizer);
keyframe_tracker.configureTracking(cfg);
keyframe_tracker.configureKeyframeSelection(frontend_cfg);
keyframe_tracker.configureMapping(backend_cfg);
ROS_WARN_STREAM_NAMED("config", "tracker config: \"" << keyframe_tracker.trackingConfiguration() << "\"");
ROS_WARN_STREAM_NAMED("config", "frontend config: \"" << keyframe_tracker.keyframeSelectionConfiguration() << "\"");
ROS_WARN_STREAM_NAMED("config", "backend config: \"" << keyframe_tracker.mappingConfiguration() << "\"");
// initialize first pose
Eigen::Affine3d trajectory, relative;
if(groundtruth_reader_ != 0)
{
dvo_benchmark::findClosestEntry(*groundtruth_reader_, rgbdpair_reader_->entry().RgbTimestamp());
dvo_benchmark::toPoseEigen(groundtruth_reader_->entry(), trajectory);
}
else
{
trajectory.setIdentity();
}
std::string optimized_trajectory_file = cfg_.RgbdPairFile.substr(0, cfg_.RgbdPairFile.find_last_of(".")) + "_opt_traj_final.txt";
std::string edge_error_file = cfg_.RgbdPairFile.substr(0, cfg_.RgbdPairFile.find_last_of(".")) + "_error.txt";
keyframe_tracker.init(trajectory);
std::string folder = cfg_.RgbdPairFile.substr(0, cfg_.RgbdPairFile.find_last_of("/") + 1);
std::vector<dvo_benchmark::RgbdPair> pairs;
rgbdpair_reader_->readAllEntries(pairs);
dvo::core::RgbdImagePyramid::Ptr current;
ROS_INFO("Start interating dataset");
dvo::util::stopwatch sw_online("online", 1), sw_postprocess("postprocess", 1);
sw_online.start();
int loop_counter = 0;
clock_t time_start = clock();
ROS_INFO("cfg_.ShowGroundtruth %i", cfg_.ShowGroundtruth);
ROS_INFO("cfg_.EstimateTrajectory %i", cfg_.EstimateTrajectory);
ROS_INFO("cfg_.ShowEstimate %i", cfg_.ShowEstimate);
for(std::vector<dvo_benchmark::RgbdPair>::iterator it = pairs.begin(); ros::ok() && it != pairs.end(); ++it)
{
loop_counter++;
if(loop_counter)
{
ROS_INFO("loop %i", loop_counter);
}
if (loop_counter == 100){
ROS_INFO("loop 100 time %i",(clock()-time_start)/CLOCKS_PER_SEC);
}
current = load(camera, folder + it->RgbFile(), folder + it->DepthFile());
if(!current) continue;
// pause in the beginning
// renderWhileSwitchAndNotTerminated(visualizer, pause_switch);
// processInput(visualizer);
if(cfg_.ShowGroundtruth)
{
ROS_INFO("enter cfg_.ShowGroundtruth");
Eigen::Affine3d groundtruth_pose;
dvo_benchmark::findClosestEntry(*groundtruth_reader_, it->RgbTimestamp());
dvo_benchmark::toPoseEigen(groundtruth_reader_->entry(), groundtruth_pose);
visualizer->trajectory("groundtruth")->
color(dvo::visualization::Color(255,255,0)).
add(groundtruth_pose);
visualizer->camera("groundtruth")->
color(dvo::visualization::Color(255,255,0)).
update(current->level(0), groundtruth_pose).
show(cfg_.ShowEstimate ? dvo::visualization::CameraVisualizer::ShowCamera : dvo::visualization::CameraVisualizer::ShowCameraAndCloud);
}
if(cfg_.EstimateRequired())
{
if((pairs.end() - it) == 1)
{
ROS_WARN("forcing keyframe");
keyframe_tracker.forceKeyframe();
}
static dvo::util::stopwatch sw_match("match", 100);
sw_match.start();
{
//ROS_INFO("keyframe_tracker before update");
keyframe_tracker.update(current, it->RgbTimestamp(), trajectory);
//ROS_INFO("keyframe_tracker after update");
}
sw_match.stopAndPrint();
if(cfg_.EstimateTrajectory)
{
Eigen::Quaterniond q(trajectory.rotation());
(*trajectory_out_)
<< it->RgbTimestamp() << " "
<< trajectory.translation()(0) << " "
<< trajectory.translation()(1) << " "
<< trajectory.translation()(2) << " "
<< q.x() << " "
<< q.y() << " "
<< q.z() << " "
<< q.w() << " "
<< std::endl;
}
if(cfg_.ShowEstimate)
{
//visualizer->trajectory("estimate")->
// color(dvo::visualization::Color::red()).
// add(trajectory);
visualizer->camera("estimate")->
color(dvo::visualization::Color::red()).
update(current->level(0), trajectory).
show(dvo::visualization::CameraVisualizer::ShowCamera);
}
}
ros::spinOnce();
//if(cfg_.RenderVideo)
//{
// ((dvo::visualization::PclCameraTrajectoryVisualizer*) visualizer)->render(5);
// ((dvo::visualization::PclCameraTrajectoryVisualizer*) visualizer)->visualizer().saveScreenshot(frame_ids.next() + std::string(".png"));
//}
}
ROS_INFO("interating dataset finished");
sw_online.stop();
//std::cerr << "input:" << std::endl;
//std::string tmp;
//std::cin >> tmp;
ROS_INFO("sw_postprocess.start();");
sw_postprocess.start();
sw_postprocess.stop();
ROS_INFO("sw_postprocess.stop();");
sw_online.print();sw_postprocess.print();
// keep visualization alive
if(cfg_.KeepAlive)
{
renderWhileSwitchAndNotTerminated(visualizer/*, dummy_switch*/);
}
ROS_INFO("before serializing");
dvo_slam::serialization::FileSerializer<dvo_slam::serialization::TrajectorySerializer> serializer(optimized_trajectory_file);
keyframe_tracker.serializeMap(serializer);
ROS_INFO("after keyframe_tracker.serializeMap(serializer);");
dvo_slam::serialization::FileSerializer<dvo_slam::serialization::EdgeErrorSerializer> error_serializer(edge_error_file);
keyframe_tracker.serializeMap(error_serializer);
ROS_INFO("after keyframe_tracker.serializeMap(error_serializer);");
}
bool SrvKinematicsPlugin::initialize(const std::string &robot_description,
const std::string& group_name,
const std::string& base_frame,
const std::vector<std::string>& tip_frames,
double search_discretization)
{
bool debug = false;
ROS_INFO_STREAM_NAMED("srv","SrvKinematicsPlugin initializing");
setValues(robot_description, group_name, base_frame, tip_frames, search_discretization);
ros::NodeHandle private_handle("~");
rdf_loader::RDFLoader rdf_loader(robot_description_);
const boost::shared_ptr<srdf::Model> &srdf = rdf_loader.getSRDF();
const boost::shared_ptr<urdf::ModelInterface>& urdf_model = rdf_loader.getURDF();
if (!urdf_model || !srdf)
{
ROS_ERROR_NAMED("srv","URDF and SRDF must be loaded for SRV kinematics solver to work."); // TODO: is this true?
return false;
}
robot_model_.reset(new robot_model::RobotModel(urdf_model, srdf));
joint_model_group_ = robot_model_->getJointModelGroup(group_name);
if (!joint_model_group_)
return false;
if (debug)
{
std::cout << std::endl << "Joint Model Variable Names: ------------------------------------------- " << std::endl;
const std::vector<std::string> jm_names = joint_model_group_->getVariableNames();
std::copy(jm_names.begin(), jm_names.end(), std::ostream_iterator<std::string>(std::cout, "\n"));
std::cout << std::endl;
}
// Get the dimension of the planning group
dimension_ = joint_model_group_->getVariableCount();
ROS_INFO_STREAM_NAMED("srv","Dimension planning group '" << group_name << "': " << dimension_
<< ". Active Joints Models: " << joint_model_group_->getActiveJointModels().size()
<< ". Mimic Joint Models: " << joint_model_group_->getMimicJointModels().size());
// Copy joint names
for (std::size_t i=0; i < joint_model_group_->getJointModels().size(); ++i)
{
ik_group_info_.joint_names.push_back(joint_model_group_->getJointModelNames()[i]);
}
if (debug)
{
ROS_ERROR_STREAM_NAMED("temp","tip links available:");
std::copy(tip_frames_.begin(), tip_frames_.end(), std::ostream_iterator<std::string>(std::cout, "\n"));
}
// Make sure all the tip links are in the link_names vector
for (std::size_t i = 0; i < tip_frames_.size(); ++i)
{
if(!joint_model_group_->hasLinkModel(tip_frames_[i]))
{
ROS_ERROR_NAMED("srv","Could not find tip name '%s' in joint group '%s'", tip_frames_[i].c_str(), group_name.c_str());
return false;
}
ik_group_info_.link_names.push_back(tip_frames_[i]);
}
// Choose what ROS service to send IK requests to
ROS_DEBUG_STREAM_NAMED("srv","Looking for ROS service name on rosparm server at location: " <<
private_handle.getNamespace() << "/" << group_name_ << "/kinematics_solver_service_name");
std::string ik_service_name;
private_handle.param(group_name_ + "/kinematics_solver_service_name", ik_service_name, std::string("solve_ik"));
// Setup the joint state groups that we need
robot_state_.reset(new robot_state::RobotState(robot_model_));
robot_state_->setToDefaultValues();
// Create the ROS service client
ros::NodeHandle nonprivate_handle("");
ik_service_client_ = boost::make_shared<ros::ServiceClient>(nonprivate_handle.serviceClient
<moveit_msgs::GetPositionIK>(ik_service_name));
if (!ik_service_client_->waitForExistence(ros::Duration(0.1))) // wait 0.1 seconds, blocking
ROS_WARN_STREAM_NAMED("srv","Unable to connect to ROS service client with name: " << ik_service_client_->getService());
else
ROS_INFO_STREAM_NAMED("srv","Service client started with ROS service name: " << ik_service_client_->getService());
active_ = true;
ROS_DEBUG_NAMED("srv","ROS service-based kinematics solver initialized");
return true;
}
void GraspFilter::filterGraspThread(IkThreadStruct ik_thread_struct)
{
// Seed state - start at zero
std::vector<double> ik_seed_state(7); // fill with zeros
// TODO do not assume 7 dof
std::vector<double> solution;
moveit_msgs::MoveItErrorCodes error_code;
geometry_msgs::PoseStamped ik_pose;
// Process the assigned grasps
for( int i = ik_thread_struct.grasps_id_start_; i < ik_thread_struct.grasps_id_end_; ++i )
{
//ROS_DEBUG_STREAM_NAMED("filter", "Checking grasp #" << i);
// Clear out previous solution just in case - not sure if this is needed
solution.clear();
// Transform current pose to frame of planning group
ik_pose = ik_thread_struct.possible_grasps_[i].grasp_pose;
Eigen::Affine3d eigen_pose;
tf::poseMsgToEigen(ik_pose.pose, eigen_pose);
eigen_pose = ik_thread_struct.link_transform_ * eigen_pose;
tf::poseEigenToMsg(eigen_pose, ik_pose.pose);
// Test it with IK
ik_thread_struct.kin_solver_->
searchPositionIK(ik_pose.pose, ik_seed_state, ik_thread_struct.timeout_, solution, error_code);
// Results
if( error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS )
{
//ROS_INFO_STREAM_NAMED("filter","Found IK Solution");
// Copy solution to seed state so that next solution is faster
ik_seed_state = solution;
// Start pre-grasp section ----------------------------------------------------------
if (ik_thread_struct.filter_pregrasp_) // optionally check the pregrasp
{
// Convert to a pre-grasp
ik_pose = SimpleGrasps::getPreGraspPose(ik_thread_struct.possible_grasps_[i], ik_thread_struct.ee_parent_link_);
// Transform current pose to frame of planning group
Eigen::Affine3d eigen_pose;
tf::poseMsgToEigen(ik_pose.pose, eigen_pose);
eigen_pose = ik_thread_struct.link_transform_ * eigen_pose;
tf::poseEigenToMsg(eigen_pose, ik_pose.pose);
// Test it with IK
ik_thread_struct.kin_solver_->
searchPositionIK(ik_pose.pose, ik_seed_state, ik_thread_struct.timeout_, solution, error_code);
// Results
if( error_code.val == moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION )
{
ROS_WARN_STREAM_NAMED("filter","Unable to find IK solution for pre-grasp pose.");
continue;
}
else if( error_code.val == moveit_msgs::MoveItErrorCodes::TIMED_OUT )
{
//ROS_DEBUG_STREAM_NAMED("filter","Unable to find IK solution for pre-grasp pose: Timed Out.");
continue;
}
else if( error_code.val != moveit_msgs::MoveItErrorCodes::SUCCESS )
{
ROS_INFO_STREAM_NAMED("filter","IK solution error for pre-grasp: MoveItErrorCodes.msg = " << error_code);
continue;
}
}
// Both grasp and pre-grasp have passed
// Lock the result vector so we can add to it for a second
{
boost::mutex::scoped_lock slock(*ik_thread_struct.lock_);
ik_thread_struct.filtered_grasps_.push_back( ik_thread_struct.possible_grasps_[i] );
trajectory_msgs::JointTrajectoryPoint point;
//point.positions = ik_seed_state; // show the grasp solution
point.positions = solution; // show the pre-grasp solution
// Copy solution so that we can optionally use it later
ik_thread_struct.ik_solutions_.push_back(point);
}
// End pre-grasp section -------------------------------------------------------
}
else if( error_code.val == moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION )
ROS_WARN_STREAM_NAMED("filter","Unable to find IK solution for pose.");
else if( error_code.val == moveit_msgs::MoveItErrorCodes::TIMED_OUT )
{
//ROS_DEBUG_STREAM_NAMED("filter","Unable to find IK solution for pose: Timed Out.");
}
else
ROS_INFO_STREAM_NAMED("filter","IK solution error: MoveItErrorCodes.msg = " << error_code);
}
//ROS_DEBUG_STREAM_NAMED("filter","Thread " << ik_thread_struct.thread_id_ << " finished");
}
bool PickPlan::plan(const planning_scene::PlanningSceneConstPtr &planning_scene, const moveit_msgs::PickupGoal &goal)
{
double timeout = goal.allowed_planning_time;
ros::WallTime endtime = ros::WallTime::now() + ros::WallDuration(timeout);
std::string planning_group = goal.group_name;
std::string end_effector = goal.end_effector;
if (end_effector.empty() && !planning_group.empty())
{
const robot_model::JointModelGroup *jmg = planning_scene->getRobotModel()->getJointModelGroup(planning_group);
if (!jmg)
{
error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
return false;
}
const std::vector<std::string> &eefs = jmg->getAttachedEndEffectorNames();
if (!eefs.empty())
{
end_effector = eefs.front();
if (eefs.size() > 1)
ROS_WARN_STREAM_NAMED("manipulation", "Choice of end-effector for group '" << planning_group << "' is ambiguous. Assuming '" << end_effector << "'");
}
}
else
if (!end_effector.empty() && planning_group.empty())
{
const robot_model::JointModelGroup *jmg = planning_scene->getRobotModel()->getEndEffector(end_effector);
if (!jmg)
{
error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
return false;
}
planning_group = jmg->getEndEffectorParentGroup().first;
if (planning_group.empty())
{
ROS_ERROR_STREAM_NAMED("manipulation", "No parent group to plan in was identified based on end-effector '" << end_effector << "'. Please define a parent group in the SRDF.");
error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
return false;
}
else
ROS_INFO_STREAM_NAMED("manipulation", "Assuming the planning group for end effector '" << end_effector << "' is '" << planning_group << "'");
}
const robot_model::JointModelGroup *eef = end_effector.empty() ? NULL : planning_scene->getRobotModel()->getEndEffector(end_effector);
if (!eef)
{
ROS_ERROR_NAMED("manipulation", "No end-effector specified for pick action");
error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
return false;
}
const std::string &ik_link = eef->getEndEffectorParentGroup().second;
ros::WallTime start_time = ros::WallTime::now();
// construct common data for possible manipulation plans
ManipulationPlanSharedDataPtr plan_data(new ManipulationPlanSharedData());
ManipulationPlanSharedDataConstPtr const_plan_data = plan_data;
plan_data->planning_group_ = planning_scene->getRobotModel()->getJointModelGroup(planning_group);
plan_data->end_effector_group_ = eef;
plan_data->ik_link_ = planning_scene->getRobotModel()->getLinkModel(ik_link);
plan_data->timeout_ = endtime;
plan_data->path_constraints_ = goal.path_constraints;
plan_data->planner_id_ = goal.planner_id;
plan_data->minimize_object_distance_ = goal.minimize_object_distance;
plan_data->max_goal_sampling_attempts_ = std::max(2u, plan_data->planning_group_->getDefaultIKAttempts());
moveit_msgs::AttachedCollisionObject &attach_object_msg = plan_data->diff_attached_object_;
// construct the attached object message that will change the world to what it would become after a pick
attach_object_msg.link_name = ik_link;
attach_object_msg.object.id = goal.target_name;
attach_object_msg.object.operation = moveit_msgs::CollisionObject::ADD;
attach_object_msg.touch_links = goal.attached_object_touch_links.empty() ? eef->getLinkModelNames() : goal.attached_object_touch_links;
collision_detection::AllowedCollisionMatrixPtr approach_grasp_acm(new collision_detection::AllowedCollisionMatrix(planning_scene->getAllowedCollisionMatrix()));
// we are allowed to touch the target object
approach_grasp_acm->setEntry(goal.target_name, attach_object_msg.touch_links, true);
// we are allowed to touch certain other objects with the gripper
approach_grasp_acm->setEntry(eef->getLinkModelNames(), goal.allowed_touch_objects, true);
if (!goal.support_surface_name.empty())
{
// we are allowed to have contact between the target object and the support surface before the grasp
approach_grasp_acm->setEntry(goal.support_surface_name, goal.target_name, true);
// optionally, it may be allowed to touch the support surface with the gripper
if (goal.allow_gripper_support_collision)
approach_grasp_acm->setEntry(goal.support_surface_name, eef->getLinkModelNames(), true);
}
// configure the manipulation pipeline
pipeline_.reset();
ManipulationStagePtr stage1(new ReachableAndValidPoseFilter(planning_scene, approach_grasp_acm, pick_place_->getConstraintsSamplerManager()));
ManipulationStagePtr stage2(new ApproachAndTranslateStage(planning_scene, approach_grasp_acm));
ManipulationStagePtr stage3(new PlanStage(planning_scene, pick_place_->getPlanningPipeline()));
pipeline_.addStage(stage1).addStage(stage2).addStage(stage3);
initialize();
pipeline_.start();
// order the grasps by quality
std::vector<std::size_t> grasp_order(goal.possible_grasps.size());
for (std::size_t i = 0 ; i < goal.possible_grasps.size() ; ++i)
grasp_order[i] = i;
OrderGraspQuality oq(goal.possible_grasps);
std::sort(grasp_order.begin(), grasp_order.end(), oq);
// feed the available grasps to the stages we set up
for (std::size_t i = 0 ; i < goal.possible_grasps.size() ; ++i)
{
ManipulationPlanPtr p(new ManipulationPlan(const_plan_data));
const moveit_msgs::Grasp &g = goal.possible_grasps[grasp_order[i]];
p->approach_ = g.pre_grasp_approach;
p->retreat_ = g.post_grasp_retreat;
p->goal_pose_ = g.grasp_pose;
p->id_ = grasp_order[i];
// if no frame of reference was specified, assume the transform to be in the reference frame of the object
if (p->goal_pose_.header.frame_id.empty())
p->goal_pose_.header.frame_id = goal.target_name;
p->approach_posture_ = g.pre_grasp_posture;
p->retreat_posture_ = g.grasp_posture;
pipeline_.push(p);
}
// wait till we're done
waitForPipeline(endtime);
pipeline_.stop();
last_plan_time_ = (ros::WallTime::now() - start_time).toSec();
if (!getSuccessfulManipulationPlans().empty())
error_code_.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
else
{
if (last_plan_time_ > timeout)
error_code_.val = moveit_msgs::MoveItErrorCodes::TIMED_OUT;
else
{
error_code_.val = moveit_msgs::MoveItErrorCodes::PLANNING_FAILED;
if (goal.possible_grasps.size() > 0)
{
ROS_WARN_NAMED("manipulation", "All supplied grasps failed. Retrying last grasp in verbose mode.");
// everything failed. we now start the pipeline again in verbose mode for one grasp
initialize();
pipeline_.setVerbose(true);
pipeline_.start();
pipeline_.reprocessLastFailure();
waitForPipeline(ros::WallTime::now() + ros::WallDuration(1.0));
pipeline_.stop();
pipeline_.setVerbose(false);
}
}
}
ROS_INFO_NAMED("manipulation", "Pickup planning completed after %lf seconds", last_plan_time_);
return error_code_.val == moveit_msgs::MoveItErrorCodes::SUCCESS;
}
