void FootstepDisplay::update(float wall_dt, float ros_dt)
{
for (size_t i = 0; i < shapes_.size(); i++) {
ShapePtr shape = shapes_[i];
texts_[i]->setVisible(show_name_); // TODO
jsk_footstep_msgs::Footstep footstep = latest_footstep_->footsteps[i];
// color
if (use_group_coloring_) {
std_msgs::ColorRGBA color
= jsk_topic_tools::colorCategory20(footstep.footstep_group);
shape->setColor(color.r, color.g, color.b, alpha_);
}
else {
if (footstep.leg == jsk_footstep_msgs::Footstep::LEFT) {
shape->setColor(0, 1, 0, alpha_);
}
else if (footstep.leg == jsk_footstep_msgs::Footstep::RIGHT) {
shape->setColor(1, 0, 0, alpha_);
}
else {
shape->setColor(1, 1, 1, alpha_);
}
}
}
}
void FootstepDisplay::updateAlpha()
{
float alpha = alpha_property_->getFloat();
for (size_t i = 0; i < shapes_.size(); i++)
{
ShapePtr shape = shapes_[i];
jsk_footstep_msgs::Footstep footstep = latest_footstep_->footsteps[i];
if (footstep.leg == jsk_footstep_msgs::Footstep::LEFT)
{
shape->setColor(0, 1, 0, alpha);
}
else if (footstep.leg == jsk_footstep_msgs::Footstep::RIGHT)
{
shape->setColor(1, 0, 0, alpha);
}
else
{
shape->setColor(1, 1, 1, alpha);
}
}
}
SkeletonPtr createGround()
{
// Create a Skeleton to represent the ground
SkeletonPtr ground = Skeleton::create("ground");
Eigen::Isometry3d tf(Eigen::Isometry3d::Identity());
double thickness = 0.01;
tf.translation() = Eigen::Vector3d(0,0,-thickness/2.0);
WeldJoint::Properties joint;
joint.mT_ParentBodyToJoint = tf;
ground->createJointAndBodyNodePair<WeldJoint>(nullptr, joint);
ShapePtr groundShape =
std::make_shared<BoxShape>(Eigen::Vector3d(10,10,thickness));
groundShape->setColor(dart::Color::Blue(0.2));
ground->getBodyNode(0)->addVisualizationShape(groundShape);
ground->getBodyNode(0)->addCollisionShape(groundShape);
return ground;
}
SkeletonPtr createAtlas()
{
// Parse in the atlas model
DartLoader urdf;
SkeletonPtr atlas =
urdf.parseSkeleton(DART_DATA_PATH"sdf/atlas/atlas_v3_no_head.urdf");
// Add a box to the root node to make it easier to click and drag
double scale = 0.25;
ShapePtr boxShape =
std::make_shared<BoxShape>(scale*Eigen::Vector3d(1.0, 1.0, 0.5));
boxShape->setColor(dart::Color::Black());
Eigen::Isometry3d tf(Eigen::Isometry3d::Identity());
tf.translation() = Eigen::Vector3d(0.1*Eigen::Vector3d(0.0, 0.0, 1.0));
boxShape->setLocalTransform(tf);
atlas->getBodyNode(0)->addVisualizationShape(boxShape);
return atlas;
}