C++ Duration::toSec方法代码示例

  void timer_callback(const ros::TimerEvent &) {
    if (!c3trajectory) return;

    ros::Time now = ros::Time::now();

    if (actionserver.isPreemptRequested()) {
      current_waypoint = c3trajectory->getCurrentPoint();
      current_waypoint.r.qdot = subjugator::Vector6d::Zero();  // zero velocities
      current_waypoint_t = now;

      // don't try to make output c3 continuous when cancelled - instead stop as quickly as possible
      c3trajectory.reset(new subjugator::C3Trajectory(current_waypoint.r, limits));
      c3trajectory_t = now;
    }

    if (actionserver.isNewGoalAvailable()) {
      boost::shared_ptr<const uf_common::MoveToGoal> goal = actionserver.acceptNewGoal();
      current_waypoint = subjugator::C3Trajectory::Waypoint(
          Point_from_PoseTwist(goal->posetwist.pose, goal->posetwist.twist), goal->speed,
          !goal->uncoordinated);
      current_waypoint_t = now;  // goal->header.stamp;
      this->linear_tolerance = goal->linear_tolerance;
      this->angular_tolerance = goal->angular_tolerance;
      c3trajectory_t = now;
    }

    while ((c3trajectory_t + traj_dt < now) and ros::ok()) {
      // ROS_INFO("Acting");

      c3trajectory->update(traj_dt.toSec(), current_waypoint,
                           (c3trajectory_t - current_waypoint_t).toSec());
      c3trajectory_t += traj_dt;
    }

    PoseTwistStamped msg;
    msg.header.stamp = c3trajectory_t;
    msg.header.frame_id = fixed_frame;
    msg.posetwist = PoseTwist_from_PointWithAcceleration(c3trajectory->getCurrentPoint());
    trajectory_pub.publish(msg);

    PoseStamped posemsg;
    posemsg.header.stamp = c3trajectory_t;
    posemsg.header.frame_id = fixed_frame;
    posemsg.pose = Pose_from_Waypoint(current_waypoint);
    waypoint_pose_pub.publish(posemsg);

    if (actionserver.isActive() &&
        c3trajectory->getCurrentPoint().is_approximately(
            current_waypoint.r, max(1e-3, linear_tolerance), max(1e-3, angular_tolerance)) &&
        current_waypoint.r.qdot == subjugator::Vector6d::Zero()) {
      actionserver.setSucceeded();
    }
  }

double PID::update(double input, double x, double dx, const ros::Duration& dt)
{
    if (!parameters_.enabled) return 0.0;
    double dt_sec = dt.toSec();

    // low-pass filter input
    if (std::isnan(state_.input)) state_.input = input;
    if (dt_sec + parameters_.time_constant > 0.0) {
        state_.dinput = (input - state_.input) / (dt_sec + parameters_.time_constant);
        state_.input  = (dt_sec * input + parameters_.time_constant * state_.input) / (dt_sec + parameters_.time_constant);
    }

    return update(state_.input - x, dx, dt);
}

double Pid::updatePid(double error, ros::Duration dt)
{
  // Get the gain parameters from the realtime buffer
  Gains gains = *gains_buffer_.readFromRT();

  double p_term, d_term, i_term;
  p_error_ = error; //this is pError = pState-pTarget

  if (dt == ros::Duration(0.0) || std::isnan(error) || std::isinf(error))
    return 0.0;

  // Calculate proportional contribution to command
  p_term = gains.p_gain_ * p_error_;

  // Calculate the integral of the position error
  i_error_ += dt.toSec() * p_error_;
  
  // Calculate integral contribution to command
  i_term = gains.i_gain_ * i_error_;

  // Limit i_term so that the limit is meaningful in the output
  i_term = std::max( gains.i_min_, std::min( i_term, gains.i_max_) );

  // Calculate the derivative error
  if (dt.toSec() > 0.0)
  {
    d_error_ = (p_error_ - p_error_last_) / dt.toSec();
    p_error_last_ = p_error_;
  }
  // Calculate derivative contribution to command
  d_term = gains.d_gain_ * d_error_;

  // Compute the command
  cmd_ = - p_term - i_term - d_term;

  return cmd_;
}

	int executeCB(ros::Duration dt)
	{
		std::cout << "**Walk -%- Executing Main Task, elapsed_time: "
		          << dt.toSec() << std::endl;
		std::cout << "**Walk -%- execute_time: "
		          << execute_time_.toSec() << std::endl;
		execute_time_ += dt;

		if(!init_)
		{
			set_feedback(RUNNING);
			initialize();
		}

		double x = motion_proxy_ptr->getRobotPosition(true).at(0);
		double y = motion_proxy_ptr->getRobotPosition(true).at(1);
		double z = motion_proxy_ptr->getRobotPosition(true).at(2);

		// Walk forward
		double angular = 0.1*modulo2Pi(z_0-z);
		//ROS_INFO("theta = %f",angular);
		if(angular > 1) {angular = 1;}
		if(angular < -1) {angular = -1;}
		geometry_msgs::Twist cmd;
		cmd.linear.x = 0.3; //0.5
		cmd.angular.z = angular;
		cmd_pub.publish(cmd);

		if(sqrt((x-x_0)*(x-x_0) + (y-y_0)*(y-y_0)) > dist)
		{
			set_feedback(SUCCESS);
			finalize();
			return 1;
		}

		return 0;
	}

string duration_string(const ros::Duration &d) {
  char buf[40];
  boost::posix_time::time_duration td = d.toBoost();

  if (td.hours() > 0) {
    snprintf(buf, sizeof(buf) / sizeof(buf[0]), "%dhr %d:%02ds (%ds)",
             td.hours(), td.minutes(), td.seconds(), td.total_seconds());
  } else if (td.minutes() > 0) {
    snprintf(buf, sizeof(buf) / sizeof(buf[0]), "%d:%02ds (%ds)",
             td.minutes(), td.seconds(), td.total_seconds());
  } else {
    snprintf(buf, sizeof(buf) / sizeof(buf[0]), "%.1fs", d.toSec());
  }
  return string(buf);
}

/**
 * @brief TimeManipulator::str
 * @param duration
 * @return
 */
std::string TimeManipulator::str(const ros::Duration& duration)
{
  double seconds = duration.toSec();
  int hours = floor(seconds / 3600);
  int minutes = floor(seconds / 60);
  seconds = (seconds / 60 - minutes) * 60;
  minutes -= hours * 60;
  std::stringstream ss;
  ss << hours << (minutes < 10 ? ":0" : ":") << minutes;
  if (seconds > 0)
  {
    ss << (seconds < 10 ? ":0" : ":") << seconds;
  }
  return ss.str();
}

// TEST CASES
TEST_F(FourWheelSteeringControllerTest, testForward)
{
  // wait for ROS
  waitForController();

  // zero everything before test
  geometry_msgs::Twist cmd_vel;
  cmd_vel.linear.x = 0.0;
  cmd_vel.angular.z = 0.0;
  publish(cmd_vel);
  ros::Duration(0.1).sleep();
  // get initial odom
  nav_msgs::Odometry old_odom = getLastOdom();
  // send a velocity command of 0.1 m/s
  cmd_vel.linear.x = 0.1;
  publish(cmd_vel);
  // wait for 10s
  ros::Duration(10.0).sleep();

  nav_msgs::Odometry new_odom = getLastOdom();

  const ros::Duration actual_elapsed_time = new_odom.header.stamp - old_odom.header.stamp;

  const double expected_distance = cmd_vel.linear.x * actual_elapsed_time.toSec();

  // check if the robot traveled 1 meter in XY plane, changes in z should be ~~0
  const double dx = new_odom.pose.pose.position.x - old_odom.pose.pose.position.x;
  const double dy = new_odom.pose.pose.position.y - old_odom.pose.pose.position.y;
  const double dz = new_odom.pose.pose.position.z - old_odom.pose.pose.position.z;
  EXPECT_NEAR(sqrt(dx*dx + dy*dy), expected_distance, POSITION_TOLERANCE);
  EXPECT_LT(fabs(dz), EPS);

  // convert to rpy and test that way
  double roll_old, pitch_old, yaw_old;
  double roll_new, pitch_new, yaw_new;
  tf::Matrix3x3(tfQuatFromGeomQuat(old_odom.pose.pose.orientation)).getRPY(roll_old, pitch_old, yaw_old);
  tf::Matrix3x3(tfQuatFromGeomQuat(new_odom.pose.pose.orientation)).getRPY(roll_new, pitch_new, yaw_new);
  EXPECT_LT(fabs(roll_new - roll_old), EPS);
  EXPECT_LT(fabs(pitch_new - pitch_old), EPS);
  EXPECT_LT(fabs(yaw_new - yaw_old), EPS);
  EXPECT_NEAR(fabs(new_odom.twist.twist.linear.x), cmd_vel.linear.x, EPS);
  EXPECT_LT(fabs(new_odom.twist.twist.linear.y), EPS);
  EXPECT_LT(fabs(new_odom.twist.twist.linear.z), EPS);

  EXPECT_LT(fabs(new_odom.twist.twist.angular.x), EPS);
  EXPECT_LT(fabs(new_odom.twist.twist.angular.y), EPS);
  EXPECT_LT(fabs(new_odom.twist.twist.angular.z), EPS);
}

int main( int argc, char** argv )
{
    const int startup_delay = 5;
    ros::init(argc, argv, "rx60_hardware");
    ros::NodeHandle nh_("robot_rx60b");
    RX60Robot robot;
    RX60_wrapper wrapper;
    controller_manager::ControllerManager cm(&robot, nh_);
    ros::Publisher state_publisher = nh_.advertise<sensor_msgs::JointState>("/robot_rx60b/controller_joint_states", 10);

    ros::AsyncSpinner spinner(0);
    spinner.start();
    //ros::MultiThreadedSpinner spinner(4);
    //spinner.spin();

    ros::Time last = ros::Time::now();
    ros::Rate r(10);
    int alive_count = 0;

    ros::Time startup_time = ros::Time::now();

    while (ros::ok())
    {
        ros::Duration period = ros::Time::now() - last;

        const sensor_msgs::JointState::Ptr robotState = wrapper.getJointState();
        robot.read(robotState);

        // Wait for valid jointstates
        const ros::Duration time_since_start = ros::Time::now() - startup_time;
        if (time_since_start.toSec() > startup_delay)
        {
            cm.update(ros::Time::now(), period);
            const sensor_msgs::JointState::Ptr robotCmd = robot.write();
            wrapper.setJointState(robotCmd);
        }
        else
            ROS_INFO("Waiting %i seconds before controlling the robot (%f elapsed)", startup_delay, time_since_start.toSec());
        last = ros::Time::now();

        state_publisher.publish(robotState);

        ros::spinOnce();
        r.sleep();
    }
}

 void jointStateCallback(const sensor_msgs::JointStateConstPtr& joint_state) {
   if((ros::Time::now()-last_update_time_).toSec() < (MARKER_DUR.toSec()/2.0)) {
     return;
   }
   last_update_time_ = ros::Time::now();
   if(!send_markers_) return;
   collision_models_->bodiesLock();
   if(group_name_1_.empty() && group_name_2_.empty()) {
     sendMarkersForGroup("");
   } 
   if(!group_name_1_.empty()) {
     sendMarkersForGroup(group_name_1_);
   } 
   if(!group_name_2_.empty()) {
     sendMarkersForGroup(group_name_2_);
   }
   collision_models_->bodiesUnlock();
 }

void JointVelocityController::update(const ros::Time& time, const ros::Duration& period)
{
    double error = command_ - joint_.getVelocity();

    double current_effort = joint_.getEffort();

    double commanded_effort;
    if(current_effort>effort_threshold)
    {
        commanded_effort=0.0;
    }
    else
    {
        // Set the PID error and compute the PID command with nonuniform time
        // step size. The derivative error is computed from the change in the error
        // and the timestep dt.
        commanded_effort = pid_controller_.computeCommand(error, period);
    }

    joint_.setCommand(commanded_effort);

    if(loop_count_ % 10 == 0)
    {
        if(controller_state_publisher_ && controller_state_publisher_->trylock())
        {
            controller_state_publisher_->msg_.header.stamp = time;
            controller_state_publisher_->msg_.set_point = command_;
            controller_state_publisher_->msg_.process_value = joint_.getVelocity();
            controller_state_publisher_->msg_.error = error;
            controller_state_publisher_->msg_.time_step = period.toSec();
            controller_state_publisher_->msg_.command = commanded_effort;

            double dummy;
            getGains(controller_state_publisher_->msg_.p,
                     controller_state_publisher_->msg_.i,
                     controller_state_publisher_->msg_.d,
                     controller_state_publisher_->msg_.i_clamp,
                     dummy);
            controller_state_publisher_->unlockAndPublish();
        }
    }
    loop_count_++;
}

void NodeClass::sendEmergencyStopStates()
{
	requestBoardStatus();

	if(!relayboard_available) return;
	
	
	bool EM_signal;
	ros::Duration duration_since_EM_confirmed;
	cob_relayboard::EmergencyStopState EM_msg;

	// assign input (laser, button) specific EM state
	EM_msg.emergency_button_stop = m_SerRelayBoard->isEMStop();
	EM_msg.scanner_stop = m_SerRelayBoard->isScannerStop();

	// determine current EMStopState
	EM_signal = (EM_msg.emergency_button_stop || EM_msg.scanner_stop);

	switch (EM_stop_status_)
	{
		case ST_EM_FREE:
		{
			if (EM_signal == true)
			{
				ROS_INFO("Emergency stop was issued");
				EM_stop_status_ = EM_msg.EMSTOP;
			}
			break;
		}
		case ST_EM_ACTIVE:
		{
			if (EM_signal == false)
			{
				ROS_INFO("Emergency stop was confirmed");
				EM_stop_status_ = EM_msg.EMCONFIRMED;
				time_of_EM_confirmed_ = ros::Time::now();
			}
			break;
		}
		case ST_EM_CONFIRMED:
		{
			if (EM_signal == true)
			{
				ROS_INFO("Emergency stop was issued");
				EM_stop_status_ = EM_msg.EMSTOP;
			}
			else
			{
				duration_since_EM_confirmed = ros::Time::now() - time_of_EM_confirmed_;
				if( duration_since_EM_confirmed.toSec() > duration_for_EM_free_.toSec() )
				{
					ROS_INFO("Emergency stop released");
					EM_stop_status_ = EM_msg.EMFREE;
				}
			}
			break;
		}
	};

	
	EM_msg.emergency_state = EM_stop_status_;

	//publish EM-Stop-Active-messages, when connection to relayboard got cut
	if(relayboard_online == false) {
		EM_msg.emergency_state = EM_msg.EMSTOP;
	}
	topicPub_isEmergencyStop.publish(EM_msg);
}

void NodeClass::sendEmergencyStopStates()
{
	requestBoardStatus();

	if(!relayboard_available) return;

	sendBatteryVoltage();

	
	bool EM_signal;
	ros::Duration duration_since_EM_confirmed;
	cob_relayboard::EmergencyStopState EM_msg;
	pr2_msgs::PowerBoardState pbs;
	pbs.header.stamp = ros::Time::now();

	// assign input (laser, button) specific EM state TODO: Laser and Scanner stop can't be read independently (e.g. if button is stop --> no informtion about scanner, if scanner ist stop --> no informtion about button stop)
	EM_msg.emergency_button_stop = m_SerRelayBoard->isEMStop();
	EM_msg.scanner_stop = m_SerRelayBoard->isScannerStop();

	// determine current EMStopState
	EM_signal = (EM_msg.emergency_button_stop || EM_msg.scanner_stop);

	switch (EM_stop_status_)
	{
		case ST_EM_FREE:
		{
			if (EM_signal == true)
			{
				ROS_INFO("Emergency stop was issued");
				EM_stop_status_ = EM_msg.EMSTOP;
			}
			break;
		}
		case ST_EM_ACTIVE:
		{
			if (EM_signal == false)
			{
				ROS_INFO("Emergency stop was confirmed");
				EM_stop_status_ = EM_msg.EMCONFIRMED;
				time_of_EM_confirmed_ = ros::Time::now();
			}
			break;
		}
		case ST_EM_CONFIRMED:
		{
			if (EM_signal == true)
			{
				ROS_INFO("Emergency stop was issued");
				EM_stop_status_ = EM_msg.EMSTOP;
			}
			else
			{
				duration_since_EM_confirmed = ros::Time::now() - time_of_EM_confirmed_;
				if( duration_since_EM_confirmed.toSec() > duration_for_EM_free_.toSec() )
				{
					ROS_INFO("Emergency stop released");
					EM_stop_status_ = EM_msg.EMFREE;
				}
			}
			break;
		}
	};

	
	EM_msg.emergency_state = EM_stop_status_;
	
	// pr2 power_board_state
	if(EM_msg.emergency_button_stop)
	  pbs.run_stop = false;
	else
	  pbs.run_stop = true;
	
	//for cob the wireless stop field is misused as laser stop field
	if(EM_msg.scanner_stop)
	  pbs.wireless_stop = false; 
	else
	  pbs.wireless_stop = true;
  

	//publish EM-Stop-Active-messages, when connection to relayboard got cut
	if(relayboard_online == false) {
		EM_msg.emergency_state = EM_msg.EMSTOP;
	}
	topicPub_isEmergencyStop.publish(EM_msg);
	topicPub_PowerBoardState.publish(pbs);
}

void targetsCallback(const mtt::TargetList& list)
{
  //will print information that should be stored in a file
  //file format: id, good/bad tag, time, pos x, pos y, vel, theta
  //             position_diff, heading_diff, angle_to_robot, velocity_diff
  static ros::Time start_time = ros::Time::now();
  time_elapsed = ros::Time::now() - start_time;
  
  /// /// ROBOT PART //////
  //use transformations to extract robot features
  try{
    p_listener->lookupTransform("/map", "/base_link", ros::Time(0), transform);
    p_listener->lookupTwist("/map", "/base_link", ros::Time(0), ros::Duration(0.5), twist);
  }
  catch (tf::TransformException ex){
    ROS_ERROR("%s",ex.what());
  }

  robot_x = transform.getOrigin().x();
  
  robot_posex_buffer.push_back(robot_x);
  
  robot_y = transform.getOrigin().y();
  robot_theta = tf::getYaw(transform.getRotation());
  robot_vel = sqrt(pow(twist.linear.x, 2) + pow(twist.linear.y, 2));
   
  //robot output line 
  /// uncomment the following for training!
  printf("%d,%d,%.10f,%.10f,%.10f,%.10f,%.10f,0,0,0,0\n",
         -1, leader_tag, time_elapsed.toSec(),
         robot_x, robot_y, robot_vel, robot_theta); 

  
  //testing new features extraction
//   accumulator_set<double, stats<tag::variance> > acc;
//   for_each(robot_posex_buffer.begin(), robot_posex_buffer.end(), boost::bind<void>(boost::ref(acc), _1));
//   printf("%f,%f,%f\n", robot_x, mean(acc), sqrt(variance(acc))); 
  
  /// /// TARGETS PART //////
  //sweeps target list and extract features
  for(uint i = 0; i < list.Targets.size(); i++){
    target_id = list.Targets[i].id;
    target_x = list.Targets[i].pose.position.x;
    target_y = list.Targets[i].pose.position.y;
    target_theta = tf::getYaw(list.Targets[i].pose.orientation);
    target_vel = sqrt(pow(list.Targets[i].velocity.linear.x,2)+
                      pow(list.Targets[i].velocity.linear.y,2));
    position_diff = sqrt(pow(robot_x - target_x,2)+
                        pow(robot_y - target_y,2));
    heading_diff = robot_theta - target_theta;
    angle_to_robot = -robot_theta + atan2(target_y - robot_y, 
                                        target_x - robot_x );
    velocity_diff = robot_vel - target_vel;
          
    //target output (to be used in adaboost training)
    
    // % output file format: 
    // % 1: id
    // % 2: good/bad tag
    // % 3: time
    // % 4: pos x
    // % 5: pos y
    // % 6: vel
    // % 7: theta
    // % 8: pos diff
    // % 9: head diff
    // %10: angle 2 robot 
    // %11: velocity diff 
    
    /// uncomment the following to generate training file!
    printf("%d,%d,%.10f,%.10f,%.10f,%.10f,%.10f,%.10f,%.10f,%.10f,%.10f\n",
      target_id, leader_tag, time_elapsed.toSec(),
      target_x, target_y, target_vel, target_theta,
      position_diff, heading_diff, angle_to_robot, velocity_diff);
    
//     if(position_diff < 6.0 && target_vel > 0.5){
//       //if inside boundaries (in meters)
//       //store features in a covariance struct to send to matlab
//       nfeatures.pose.position.x = target_x;
//       nfeatures.pose.position.y = target_y;
//       nfeatures.pose.position.z = target_id;
// 
//       nfeatures.covariance[0] = target_vel;
//       nfeatures.covariance[1] = velocity_diff;
//       nfeatures.covariance[2] = heading_diff;
//       nfeatures.covariance[3] = angle_to_robot;
//       nfeatures.covariance[4] = position_diff;
//       
//       matlab_list.push_back(nfeatures);
// //       counter++;
//     }
  }
//   printf("targets within range: %d\n",counter);
//   counter = 0;
  
  //check if enough time has passed 
  //and send batch of msgs to matlab
//   duration_btw_msg = ros::Time::now() - time_last_msg;
//   
//   if(duration_btw_msg.toSec() > 0.01){
//.........这里部分代码省略.........

	int executeCB(ros::Duration dt)
		{
			std::cout << "**GoToWaypoint -%- Executing Main Task, elapsed_time: "
			          << dt.toSec() << std::endl;
			std::cout << "**GoToWaypoint -%- execute_time: "
			          << execute_time_.toSec() << std::endl;
			execute_time_ += dt;

			std::cout << "**Counter value:" << counter << std::endl;
			if (counter > 1)
				std::cout << "********************************************************************************" << std::endl;


			if (!init_)
			{
				initialize();
				init_ = true;
			}
			if ( (ros::Time::now() - time_at_pos_).toSec() < 0.2 )
			{
				if( message_.type == "goto")
				{
						// Goal position of ball relative to ROBOT_FRAME
						float goal_x = 0.00;
						float goal_y = 0.00;
						float error_x = message_.x - goal_x;
						float error_y = message_.y - goal_y;
						if (fabs(error_x) < 0.12 && fabs(error_y) < 0.12)
						{
							std::cout << "Closeness count " << closeness_count << std::endl;
							closeness_count++;
							//If the NAO has been close for enough iterations, we consider to goal reached
							if (closeness_count > 0)
							{
								motion_proxy_ptr->stopMove();
								set_feedback(SUCCESS);
								// std::cout << "sleeeping for 2 second before destroying thread" << std::endl;
								// sleep(2.0);
								finalize();
								return 1;
							}
						}
						else
						{
							closeness_count = 0;
						}
						//Limit the "error" in order to limit the walk speed
						error_x = error_x >  0.6 ?  0.6 : error_x;
						error_x = error_x < -0.6 ? -0.6 : error_x;
						error_y = error_y >  0.6 ?  0.6 : error_y;
						error_y = error_y < -0.6 ? -0.6 : error_y;
						// float speed_x = error_x * 1.0/(2+5*closeness_count);
						// float speed_y = error_y * 1.0/(2+5*closeness_count);
						// float frequency = 0.1/(5*closeness_count+(1.0/(fabs(error_x)+fabs(error_y)))); //Frequency of foot steps
						// motion_proxy_ptr->setWalkTargetVelocity(speed_x, speed_y, 0.0, frequency);
						// ALMotionProxy::setWalkTargetVelocity(const float& x, const float& y, const float& theta, const float& frequency)
						AL::ALValue walk_config;
						//walk_config.arrayPush(AL::ALValue::array("MaxStepFrequency", frequency));
						//Lower value of step height gives smoother walking
						// std::cout << "y " << message_.y << std::endl;
						if (fabs(message_.y) < 0.10)
						{
							// walk_config.arrayPush(AL::ALValue::array("StepHeight", 0.01));
							// motion_proxy_ptr->post.moveTo(message_.x, 0.0, 0.0, walk_config);
							motion_proxy_ptr->post.moveTo(message_.x, 0.0, 0.0);
							sleep(2.0);
							//motion_proxy_ptr->post.stopMove();
						}
						else
						{
							// walk_config.arrayPush(AL::ALValue::array("StepHeight", 0.005));
							// motion_proxy_ptr->post.moveTo(0.0, 0.0, message_.y/fabs(message_.y)*0.1, walk_config);
							motion_proxy_ptr->post.moveTo(0.0, 0.0, message_.y/fabs(message_.y)*0.1);
							//sleep(3.0);
							//motion_proxy_ptr->post.stopMove();
						}
				}

			}
			set_feedback(RUNNING);
			return 0;
		}

  void DiffDriveController::update(const ros::Time& time, const ros::Duration& period)
  {
    // COMPUTE AND PUBLISH ODOMETRY
    if (open_loop_)
    {
      odometry_.updateOpenLoop(last0_cmd_.lin, last0_cmd_.ang, time);
    }
    else
    {
      double left_pos  = 0.0;
      double right_pos = 0.0;
      for (size_t i = 0; i < wheel_joints_size_; ++i)
      {
        const double lp = left_wheel_joints_[i].getPosition();
        const double rp = right_wheel_joints_[i].getPosition();
        if (std::isnan(lp) || std::isnan(rp))
          return;

        left_pos  += lp;
        right_pos += rp;
      }
      left_pos  /= wheel_joints_size_;
      right_pos /= wheel_joints_size_;

      // Estimate linear and angular velocity using joint information
      odometry_.update(left_pos, right_pos, time);
    }

    // Publish odometry message
    if (last_state_publish_time_ + publish_period_ < time)
    {
      last_state_publish_time_ += publish_period_;
      // Compute and store orientation info
      const geometry_msgs::Quaternion orientation(
            tf::createQuaternionMsgFromYaw(odometry_.getHeading()));

      // Populate odom message and publish
      if (odom_pub_->trylock())
      {
        odom_pub_->msg_.header.stamp = time;
        odom_pub_->msg_.pose.pose.position.x = odometry_.getX();
        odom_pub_->msg_.pose.pose.position.y = odometry_.getY();
        odom_pub_->msg_.pose.pose.orientation = orientation;
        odom_pub_->msg_.twist.twist.linear.x  = odometry_.getLinear();
        odom_pub_->msg_.twist.twist.angular.z = odometry_.getAngular();
        odom_pub_->unlockAndPublish();
      }

      // Publish tf /odom frame
      if (enable_odom_tf_ && tf_odom_pub_->trylock())
      {
        geometry_msgs::TransformStamped& odom_frame = tf_odom_pub_->msg_.transforms[0];
        odom_frame.header.stamp = time;
        odom_frame.transform.translation.x = odometry_.getX();
        odom_frame.transform.translation.y = odometry_.getY();
        odom_frame.transform.rotation = orientation;
        tf_odom_pub_->unlockAndPublish();
      }
    }

    // MOVE ROBOT
    // Retreive current velocity command and time step:
    Commands curr_cmd = *(command_.readFromRT());
    const double dt = (time - curr_cmd.stamp).toSec();

    // Brake if cmd_vel has timeout:
    if (dt > cmd_vel_timeout_)
    {
      curr_cmd.lin = 0.0;
      curr_cmd.ang = 0.0;
    }

    // Limit velocities and accelerations:
    const double cmd_dt(period.toSec());

    limiter_lin_.limit(curr_cmd.lin, last0_cmd_.lin, last1_cmd_.lin, cmd_dt);
    limiter_ang_.limit(curr_cmd.ang, last0_cmd_.ang, last1_cmd_.ang, cmd_dt);

    last1_cmd_ = last0_cmd_;
    last0_cmd_ = curr_cmd;

    // Apply multipliers:
    const double ws = wheel_separation_multiplier_ * wheel_separation_;
    const double wr = wheel_radius_multiplier_     * wheel_radius_;

    // Compute wheels velocities:
    const double vel_left  = (curr_cmd.lin - curr_cmd.ang * ws / 2.0)/wr;
    const double vel_right = (curr_cmd.lin + curr_cmd.ang * ws / 2.0)/wr;

    // Set wheels velocities:
    for (size_t i = 0; i < wheel_joints_size_; ++i)
    {
      left_wheel_joints_[i].setCommand(vel_left);
      right_wheel_joints_[i].setCommand(vel_right);
    }
  }