void SnapIt::convexAlignCallback(
const geometry_msgs::PoseStamped::ConstPtr& pose_msg)
{
boost::mutex::scoped_lock lock(mutex_);
if (!polygons_) {
JSK_NODELET_ERROR("no polygon is ready");
convex_aligned_pub_.publish(pose_msg);
return;
}
std::vector<jsk_recognition_utils::ConvexPolygon::Ptr> convexes
= createConvexes(pose_msg->header.frame_id,
pose_msg->header.stamp,
polygons_);
Eigen::Affine3d pose_eigend;
Eigen::Affine3f pose_eigen;
tf::poseMsgToEigen(pose_msg->pose, pose_eigend);
convertEigenAffine3(pose_eigend, pose_eigen);
Eigen::Vector3f pose_point(pose_eigen.translation());
int min_index = findNearestConvex(pose_point, convexes);
if (min_index != -1) {
jsk_recognition_utils::ConvexPolygon::Ptr min_convex = convexes[min_index];
geometry_msgs::PoseStamped aligned_pose = alignPose(pose_eigen, min_convex);
aligned_pose.header = pose_msg->header;
convex_aligned_pub_.publish(aligned_pose);
}
else {
convex_aligned_pub_.publish(pose_msg); // shoud we publish this?
}
}
void SnapIt::convexAlignPolygonCallback(
const geometry_msgs::PolygonStamped::ConstPtr& poly_msg)
{
boost::mutex::scoped_lock lock(mutex_);
geometry_msgs::PoseArray pose_array;
pose_array.header = poly_msg->header;
if (!polygons_) {
JSK_NODELET_ERROR("no polygon is ready");
return;
}
std::vector<jsk_recognition_utils::ConvexPolygon::Ptr> convexes
= createConvexes(poly_msg->header.frame_id,
poly_msg->header.stamp,
polygons_);
for (size_t i = 0; i < poly_msg->polygon.points.size(); i++) {
geometry_msgs::Point32 p = poly_msg->polygon.points[i];
Eigen::Vector3f pose_point(p.x, p.y, p.z);
int min_index = findNearestConvex(pose_point, convexes);
if (min_index == -1) {
JSK_NODELET_ERROR("cannot project onto convex");
return;
}
else {
jsk_recognition_utils::ConvexPolygon::Ptr min_convex = convexes[min_index];
Eigen::Affine3f pose_eigen = Eigen::Affine3f::Identity();
pose_eigen.translate(pose_point);
geometry_msgs::PoseStamped aligned_pose = alignPose(pose_eigen, min_convex);
aligned_pose.header = poly_msg->header;
pose_array.poses.push_back(aligned_pose.pose);
}
}
convex_aligned_pose_array_pub_.publish(pose_array);
}
bool Application::setup()
{
Ogre::OverlayManager::getSingleton().getByName("InfoPanel")->show();
m_NXOgreScene->getMaterial(0)->setStaticFriction(0.5);
m_NXOgreScene->getMaterial(0)->setDynamicFriction(0.5);
m_NXOgreScene->getMaterial(0)->setRestitution(0.1);
createCharacter();
createVolume();
createBasicScenary();
createTriangleMeshes();
createConvexes();
return true;
}
void SnapIt::polygonAlignCallback(
const geometry_msgs::PoseStamped::ConstPtr& pose_msg)
{
boost::mutex::scoped_lock lock(mutex_);
if (!polygons_) {
JSK_NODELET_ERROR("no polygon is ready");
polygon_aligned_pub_.publish(pose_msg);
return;
}
std::vector<jsk_recognition_utils::ConvexPolygon::Ptr> convexes
= createConvexes(pose_msg->header.frame_id,
pose_msg->header.stamp,
polygons_);
Eigen::Affine3d pose_eigend;
Eigen::Affine3f pose_eigen;
tf::poseMsgToEigen(pose_msg->pose, pose_eigend);
convertEigenAffine3(pose_eigend, pose_eigen);
Eigen::Vector3f pose_point(pose_eigen.translation());
double min_distance = DBL_MAX;
jsk_recognition_utils::ConvexPolygon::Ptr min_convex;
for (size_t i = 0; i < convexes.size(); i++) {
jsk_recognition_utils::ConvexPolygon::Ptr convex = convexes[i];
double d = convex->distanceToPoint(pose_point);
if (d < min_distance) {
min_convex = convex;
min_distance = d;
}
}
if (min_convex) {
geometry_msgs::PoseStamped aligned_pose = alignPose(pose_eigen, min_convex);
aligned_pose.header = pose_msg->header;
polygon_aligned_pub_.publish(aligned_pose);
}
else {
polygon_aligned_pub_.publish(pose_msg);
}
}
bool SnapIt::footstepAlignServiceCallback(
jsk_pcl_ros::SnapFootstep::Request& req,
jsk_pcl_ros::SnapFootstep::Response& res)
{
boost::mutex::scoped_lock lock(mutex_);
jsk_footstep_msgs::FootstepArray input_footsteps = req.input;
res.output.header = input_footsteps.header;
std::vector<jsk_recognition_utils::ConvexPolygon::Ptr> convexes
= createConvexes(input_footsteps.header.frame_id,
input_footsteps.header.stamp,
polygons_);
for (size_t i = 0; i < input_footsteps.footsteps.size(); i++) {
jsk_footstep_msgs::Footstep footstep = input_footsteps.footsteps[i];
Eigen::Affine3d pose_eigend;
Eigen::Affine3f pose_eigen;
tf::poseMsgToEigen(footstep.pose, pose_eigen);
Eigen::Vector3f pose_point(pose_eigen.translation());
int min_index = findNearestConvex(pose_point, convexes);
jsk_footstep_msgs::Footstep aligned_footstep;
if (min_index != -1) {
jsk_recognition_utils::ConvexPolygon::Ptr min_convex = convexes[min_index];
geometry_msgs::PoseStamped aligned_pose = alignPose(pose_eigen, min_convex);
aligned_footstep.pose = aligned_pose.pose;
}
else {
aligned_footstep.pose = footstep.pose;
//convex_aligned_pub_.publish(pose_msg); // shoud we publish this?
}
aligned_footstep.leg = footstep.leg;
aligned_footstep.dimensions = footstep.dimensions;
aligned_footstep.duration = footstep.duration;
aligned_footstep.footstep_group = footstep.footstep_group;
res.output.footsteps.push_back(aligned_footstep);
}
return true;
}
