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; } }