void RobotStateVisualization::updateHelper(const robot_state::RobotStateConstPtr &kinematic_state,
const std_msgs::ColorRGBA &default_attached_object_color,
const std::map<std::string, std_msgs::ColorRGBA> *color_map)
{
robot_.update(PlanningLinkUpdater(kinematic_state));
render_shapes_->clear();
std::vector<const robot_state::AttachedBody*> attached_bodies;
kinematic_state->getAttachedBodies(attached_bodies);
for (std::size_t i = 0 ; i < attached_bodies.size() ; ++i)
{
std_msgs::ColorRGBA color = default_attached_object_color;
float alpha = robot_.getAlpha();
if (color_map)
{
std::map<std::string, std_msgs::ColorRGBA>::const_iterator it = color_map->find(attached_bodies[i]->getName());
if (it != color_map->end())
{ // render attached bodies with a color that is a bit different
color.r = std::max(1.0f, it->second.r * 1.05f);
color.g = std::max(1.0f, it->second.g * 1.05f);
color.b = std::max(1.0f, it->second.b * 1.05f);
alpha = color.a = it->second.a;
}
}
rviz::Color rcolor(color.r, color.g, color.b);
const EigenSTL::vector_Affine3d &ab_t = attached_bodies[i]->getGlobalCollisionBodyTransforms();
const std::vector<shapes::ShapeConstPtr> &ab_shapes = attached_bodies[i]->getShapes();
for (std::size_t j = 0 ; j < ab_shapes.size() ; ++j)
{
render_shapes_->renderShape(robot_.getVisualNode(), ab_shapes[j].get(), ab_t[j], octree_voxel_render_mode_, octree_voxel_color_mode_, rcolor, alpha);
render_shapes_->renderShape(robot_.getCollisionNode(), ab_shapes[j].get(), ab_t[j], octree_voxel_render_mode_, octree_voxel_color_mode_, rcolor, alpha);
}
}
robot_.setVisualVisible(visual_visible_);
robot_.setCollisionVisible(collision_visible_);
robot_.setVisible(visible_);
}
void PlanningDisplay::update(float wall_dt, float ros_dt)
{
if (!kinematic_model_)
return;
if (!animating_path_ && !new_kinematic_path_ && loop_display_ && displaying_kinematic_path_message_)
{
new_kinematic_path_ = true;
incoming_kinematic_path_message_ = displaying_kinematic_path_message_;
}
planning_models::KinematicState state(kinematic_model_);
if (!animating_path_ && new_kinematic_path_)
{
displaying_kinematic_path_message_ = incoming_kinematic_path_message_;
animating_path_ = true;
new_kinematic_path_ = false;
current_state_ = -1;
current_state_time_ = state_display_time_ + 1.0f;
for(unsigned int i = 0; i < displaying_kinematic_path_message_->robot_state.multi_dof_joint_state.joint_names.size(); i++) {
planning_models::KinematicState::JointState* js = state.getJointState(displaying_kinematic_path_message_->robot_state.multi_dof_joint_state.joint_names[i]);
if(!js) continue;
if(displaying_kinematic_path_message_->robot_state.multi_dof_joint_state.frame_ids[i] != js->getParentFrameId() ||
displaying_kinematic_path_message_->robot_state.multi_dof_joint_state.child_frame_ids[i] != js->getChildFrameId()) {
ROS_WARN_STREAM("Robot state msg has bad multi_dof transform");
} else {
tf::StampedTransform transf;
tf::poseMsgToTF(displaying_kinematic_path_message_->robot_state.multi_dof_joint_state.poses[i], transf);
js->setJointStateValues(transf);
}
}
std::map<std::string, double> joint_state_map;
for (unsigned int i = 0 ; i < displaying_kinematic_path_message_->robot_state.joint_state.name.size(); ++i)
{
joint_state_map[displaying_kinematic_path_message_->robot_state.joint_state.name[i]] = displaying_kinematic_path_message_->robot_state.joint_state.position[i];
}
//overwriting with vals in first value in trajectory
for(unsigned int i = 0; i < displaying_kinematic_path_message_->trajectory.joint_trajectory.joint_names.size(); i++) {
joint_state_map[displaying_kinematic_path_message_->trajectory.joint_trajectory.joint_names[i]] = displaying_kinematic_path_message_->trajectory.joint_trajectory.points[0].positions[i];
}
state.setKinematicState(joint_state_map);
robot_->update(PlanningLinkUpdater(&state));
}
if (animating_path_)
{
for(unsigned int i = 0; i < displaying_kinematic_path_message_->robot_state.multi_dof_joint_state.joint_names.size(); i++) {
planning_models::KinematicState::JointState* js = state.getJointState(displaying_kinematic_path_message_->robot_state.multi_dof_joint_state.joint_names[i]);
if(!js) continue;
if(displaying_kinematic_path_message_->robot_state.multi_dof_joint_state.frame_ids[i] != js->getParentFrameId() ||
displaying_kinematic_path_message_->robot_state.multi_dof_joint_state.child_frame_ids[i] != js->getChildFrameId()) {
ROS_WARN_STREAM("Robot state msg has bad multi_dof transform");
} else {
tf::StampedTransform transf;
tf::poseMsgToTF(displaying_kinematic_path_message_->robot_state.multi_dof_joint_state.poses[i], transf);
js->setJointStateValues(transf);
}
}
std::map<std::string, double> joint_state_map;
for (unsigned int i = 0 ; i < displaying_kinematic_path_message_->robot_state.joint_state.name.size(); ++i)
{
joint_state_map[displaying_kinematic_path_message_->robot_state.joint_state.name[i]] = displaying_kinematic_path_message_->robot_state.joint_state.position[i];
}
if (current_state_time_ > state_display_time_)
{
++current_state_;
calculateRobotPosition();
if ((size_t) current_state_ < displaying_kinematic_path_message_->trajectory.joint_trajectory.points.size())
{
for(unsigned int i = 0; i < displaying_kinematic_path_message_->trajectory.joint_trajectory.joint_names.size(); i++) {
joint_state_map[displaying_kinematic_path_message_->trajectory.joint_trajectory.joint_names[i]] = displaying_kinematic_path_message_->trajectory.joint_trajectory.points[current_state_].positions[i];
}
state.setKinematicState(joint_state_map);
bool updKstate = false;
for(unsigned int i = 0; i < displaying_kinematic_path_message_->trajectory.multi_dof_joint_trajectory.joint_names.size(); i++) {
planning_models::KinematicState::JointState* js = state.getJointState(displaying_kinematic_path_message_->trajectory.multi_dof_joint_trajectory.joint_names[i]);
if(!js) continue;
if(displaying_kinematic_path_message_->trajectory.multi_dof_joint_trajectory.frame_ids[i] != js->getParentFrameId() ||
displaying_kinematic_path_message_->trajectory.multi_dof_joint_trajectory.child_frame_ids[i] != js->getChildFrameId()) {
ROS_WARN_STREAM("Robot state msg has bad multi_dof transform");
} else {
updKstate = true;
tf::StampedTransform transf;
tf::poseMsgToTF(displaying_kinematic_path_message_->trajectory.multi_dof_joint_trajectory.points[current_state_].poses[i], transf);
js->setJointStateValues(transf);
}
}
if (updKstate)
state.updateKinematicLinks();
robot_->update(PlanningLinkUpdater(&state));
}
else
{
animating_path_ = false;
std_msgs::Bool done;
done.data = !animating_path_;
state_publisher_.publish(done);
}
current_state_time_ = 0.0f;
}
current_state_time_ += wall_dt;
}
}
