void PlanningDisplay::update( float dt )
{
incoming_kinematic_path_message_.lock();
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;
kinematic_model_->computeTransforms(&displaying_kinematic_path_message_.start_state.vals[0]);
robot_->update( kinematic_model_, target_frame_ );
}
incoming_kinematic_path_message_.unlock();
if ( animating_path_ )
{
if ( current_state_time_ > state_display_time_ )
{
++current_state_;
calculateRobotPosition();
if ( (size_t)current_state_ < displaying_kinematic_path_message_.path.get_states_size() )
{
int group_id = kinematic_model_->getGroupID( displaying_kinematic_path_message_.model_name );
kinematic_model_->computeTransforms(&displaying_kinematic_path_message_.path.states[ current_state_ ].vals[0], group_id);
robot_->update( kinematic_model_, target_frame_ );
causeRender();
}
else
{
animating_path_ = false;
}
current_state_time_ = 0.0f;
}
current_state_time_ += dt;
}
}
void PlanningDisplay::targetFrameChanged()
{
calculateRobotPosition();
}
void PlanningDisplay::fixedFrameChanged()
{
calculateRobotPosition();
}
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;
}
}
