bool PR2ArmKinematics::getPositionFK(kinematics_msgs::GetPositionFK::Request &request,
kinematics_msgs::GetPositionFK::Response &response)
{
if(!active_)
{
ROS_ERROR("FK service not active");
return true;
}
if(!checkFKService(request,response,fk_solver_info_))
return true;
KDL::Frame p_out;
KDL::JntArray jnt_pos_in;
geometry_msgs::PoseStamped pose;
tf::Stamped<tf::Pose> tf_pose;
jnt_pos_in.resize(dimension_);
for(int i=0; i < (int) request.robot_state.joint_state.position.size(); i++)
{
int tmp_index = getJointIndex(request.robot_state.joint_state.name[i],fk_solver_info_);
if(tmp_index >=0)
jnt_pos_in(tmp_index) = request.robot_state.joint_state.position[i];
}
response.pose_stamped.resize(request.fk_link_names.size());
response.fk_link_names.resize(request.fk_link_names.size());
bool valid = true;
for(unsigned int i=0; i < request.fk_link_names.size(); i++)
{
ROS_DEBUG("End effector index: %d",pr2_arm_kinematics::getKDLSegmentIndex(kdl_chain_,request.fk_link_names[i]));
ROS_DEBUG("Chain indices: %d",kdl_chain_.getNrOfSegments());
if(jnt_to_pose_solver_->JntToCart(jnt_pos_in,p_out,pr2_arm_kinematics::getKDLSegmentIndex(kdl_chain_,request.fk_link_names[i])) >=0)
{
tf_pose.frame_id_ = root_name_;
tf_pose.stamp_ = ros::Time();
tf::PoseKDLToTF(p_out,tf_pose);
tf::poseStampedTFToMsg(tf_pose,pose);
if(!transformPose(request.header.frame_id, pose, response.pose_stamped[i])) {
response.error_code.val = response.error_code.FRAME_TRANSFORM_FAILURE;
return false;
}
response.fk_link_names[i] = request.fk_link_names[i];
response.error_code.val = response.error_code.SUCCESS;
}
else
{
ROS_ERROR("Could not compute FK for %s",request.fk_link_names[i].c_str());
response.error_code.val = response.error_code.NO_FK_SOLUTION;
valid = false;
}
}
return true;
}
void Visualization::resetData(const PlanningPipeline& dp, const GraspAnalysis& ref,
const GraspAnalysis& trgt_templt)
{
boost::mutex::scoped_lock lock(mutex_);
/* target_normals */
string frame_id = dp.target_object_.header.frame_id;
target_normals_ = dp.templt_generator_->getVisualizationNormals("target_obj_normals_viz", frame_id);
/* viewpoint */
viewpoint_pose_.header.stamp = ros::Time::now();
viewpoint_pose_.header.frame_id = dp.templt_generator_->frameBase();
viewpoint_pose_.pose.orientation.w = dp.templt_generator_->viewp_rot_.w();
viewpoint_pose_.pose.orientation.x = dp.templt_generator_->viewp_rot_.x();
viewpoint_pose_.pose.orientation.y = dp.templt_generator_->viewp_rot_.y();
viewpoint_pose_.pose.orientation.z = dp.templt_generator_->viewp_rot_.z();
viewpoint_pose_.pose.position.x = dp.templt_generator_->viewp_trans_.x();
viewpoint_pose_.pose.position.y = dp.templt_generator_->viewp_trans_.y();
viewpoint_pose_.pose.position.z = dp.templt_generator_->viewp_trans_.z();
table_pose_.header.stamp = ros::Time::now();
table_pose_.header.frame_id = dp.templt_generator_->frameBase();
table_pose_.pose = dp.templt_generator_->table_pose_;
sensor_msgs::PointCloud2 pc2_tmp;
pcl::toROSMsg(dp.templt_generator_->getConvexHullPoints(), pc2_tmp);
sensor_msgs::convertPointCloud2ToPointCloud(pc2_tmp, target_hull_);
computeHullMesh(dp.templt_generator_->getConvexHullPoints(), dp.templt_generator_->getConvexHullVertices());
pcl::toROSMsg(dp.templt_generator_->getSearchPoints(), pc2_tmp);
sensor_msgs::convertPointCloud2ToPointCloud(pc2_tmp, search_points_);
sensor_msgs::ChannelFloat32 sp_intens;
sp_intens.name = "intensity";
sp_intens.values.resize(search_points_.points.size());
for (unsigned int i = 0; i < search_points_.points.size(); i++)
{
sp_intens.values[i] = i;
}
search_points_.channels.push_back(sp_intens);
/* target object */
sensor_msgs::convertPointCloud2ToPointCloud(dp.target_object_, target_object_);
/* reference object */
dp.getRelatedObject(ref, pc2_tmp);
sensor_msgs::convertPointCloud2ToPointCloud(pc2_tmp, matching_object_);
/* target and matching gripper-poses */
matching_gripper_ = ref.gripper_pose;
target_gripper_ = trgt_templt.gripper_pose;
/* target template */
GraspTemplate t(ref.grasp_template, ref.template_pose.pose);
DismatchMeasure match_handl(t, matching_gripper_.pose);
GraspTemplate current_candidate(trgt_templt.grasp_template, trgt_templt.template_pose.pose);
match_handl.applyDcMask(current_candidate);
target_templt_ = current_candidate.getVisualization("grasp_decision_target_viz", target_object_.header.frame_id);
/* target templt */
target_templt_pose_.header.stamp = ros::Time::now();
target_templt_pose_.header.frame_id = target_object_.header.frame_id;
target_templt_pose_.pose.position.x = current_candidate. object_to_template_translation_.x();
target_templt_pose_.pose.position.y = current_candidate. object_to_template_translation_.y();
target_templt_pose_.pose.position.z = current_candidate. object_to_template_translation_.z();
target_templt_pose_.pose.orientation.x = current_candidate. object_to_template_rotation_.x();
target_templt_pose_.pose.orientation.y = current_candidate. object_to_template_rotation_.y();
target_templt_pose_.pose.orientation.z = current_candidate. object_to_template_rotation_.z();
target_templt_pose_.pose.orientation.w = current_candidate. object_to_template_rotation_.w();
/* target heightmap */
target_hm_
= current_candidate.heightmap_.getVisualization("templt_comp_target", ref.template_pose.header.frame_id, 1);
/* matching template */
matching_templt_ = match_handl.getLibTemplt().getVisualization("grasp_decision_match_viz",
ref.template_pose.header.frame_id);
/* matching heightmap */
matching_hm_ = t.heightmap_.getVisualization("templt_comp_match", ref.template_pose.header.frame_id);
/* transform matchings for better visualization */
Vector3d viz_trans(0, -0.5, 0.5);
Quaterniond viz_rot = Quaterniond::Identity();
transformMarkers(matching_templt_, viz_trans, viz_rot);
transformPose(matching_gripper_.pose, viz_trans, viz_rot);
transformPointCloud(matching_object_, viz_trans, viz_rot);
viz_trans.x() = matching_gripper_.pose.position.x;
viz_trans.y() = matching_gripper_.pose.position.y + 0.35;
viz_trans.z() = matching_gripper_.pose.position.z - 0.2;
adaptForTemplateComparison(matching_hm_, target_hm_, viz_trans, Quaterniond::Identity());
/* visualize gripper in form of a mesh */
string gripper_arch;
double gripper_state = 0;
ros::param::get("~hand_architecture", gripper_arch);
if (gripper_arch == "armrobot" && ref.fingerpositions.vals.size() >= 1)
{
gripper_state = ref.fingerpositions.vals[0];
}
else
{
gripper_state = ref.gripper_joint_state;
}
matching_gripper_mesh_ = visualizeGripper("matching_gripper_mesh",
matching_object_.header.frame_id, matching_gripper_.pose, gripper_state);
target_gripper_mesh_ = visualizeGripper("target_gripper_mesh",
target_object_.header.frame_id, target_gripper_.pose, gripper_state);
}
