geometry_msgs::Pose FootstepMarker::computeLegTransformation(uint8_t leg) {
geometry_msgs::Pose new_pose;
jsk_pcl_ros::CallSnapIt srv;
srv.request.request.header.stamp = ros::Time::now();
srv.request.request.header.frame_id = marker_frame_id_;
srv.request.request.target_plane.header.stamp = ros::Time::now();
srv.request.request.target_plane.header.frame_id = marker_frame_id_;
srv.request.request.target_plane.polygon = computePolygon(leg);
if (snapit_client_.call(srv)) {
Eigen::Affine3d A, T, B, B_prime;
tf::poseMsgToEigen(srv.response.transformation, T);
tf::poseMsgToEigen(marker_pose_.pose, A);
if (leg == jsk_footstep_msgs::Footstep::LEFT) {
tf::poseMsgToEigen(lleg_pose_, B);
}
else if (leg == jsk_footstep_msgs::Footstep::RIGHT) {
tf::poseMsgToEigen(rleg_pose_, B);
}
B_prime = A.inverse() * T * A * B;
tf::poseEigenToMsg(B_prime, new_pose);
}
else {
// throw exception
ROS_ERROR("failed to call snapit");
}
return new_pose;
}
void ASprite::render()
{
m_program->bind();
shaderAction();
glDepthMask(GL_FALSE);
computeMatrix();
computePolygon();
GLuint * vboId = m_geometric.getVboId();
glBindBuffer(GL_ARRAY_BUFFER,vboId[0]);
glBindBuffer(GL_ELEMENT_ARRAY_BUFFER,vboId[1]);
quintptr offset = 0;
glBindTexture(GL_TEXTURE_2D, m_texId);
// Tell OpenGL programmable pipeline how to locate vertex position data
int vertexLocation = m_program->attributeLocation("a_position");
m_program->enableAttributeArray(vertexLocation);
glVertexAttribPointer(vertexLocation,3, GL_FLOAT, GL_FALSE, sizeof(VertexData), (const void *)offset);
offset += sizeof(AVector3D);
// Tell OpenGL programmable pipeline how to locate vertex texture coordinate data
int texcoordLocation = m_program->attributeLocation("a_texcoord");
m_program->enableAttributeArray(texcoordLocation);
glVertexAttribPointer(texcoordLocation, 2, GL_FLOAT, GL_FALSE, sizeof(VertexData), (const void *)offset);
m_program->setUniformValue("source",0);
m_program->setUniformValue("age_Opacity",m_alpha);
m_program->setUniformValue("red",m_color.red);
m_program->setUniformValue("green",m_color.green);
m_program->setUniformValue("blue",m_color.blue);
glDrawElements(GL_TRIANGLE_FAN, 4, GL_UNSIGNED_SHORT, 0);
glDepthMask(GL_TRUE);
glBindBuffer(GL_ARRAY_BUFFER,0);
glBindBuffer(GL_ELEMENT_ARRAY_BUFFER,0);
}
GlRegularPolygon::GlRegularPolygon(const Coord &position,
const Size &size,
unsigned int numberOfSides,
const Color &fillColor,
const Color &outlineColor,
bool filled,
bool outlined,
const string &textureName,
float outlineSize):
position(position),
size(size),
numberOfSides(numberOfSides),
startAngle(float(M_PI)/2.0f) {
computePolygon();
invertYTexture=false;
setFillColor(fillColor);
setOutlineColor(outlineColor);
setFillMode(filled);
setOutlineMode(outlined);
setTextureName(textureName);
setOutlineSize(outlineSize);
}
//=====================================================
void GlRegularPolygon::setNumberOfSides(unsigned int number) {
numberOfSides=number;
computePolygon();
}
//=====================================================
void GlRegularPolygon::setStartAngle(float angle) {
startAngle=angle;
computePolygon();
}
