Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[23260] pkg/trunk
|
From: <rob...@us...> - 2009-08-28 16:32:12
|
Revision: 23260
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23260&view=rev
Author: rob_wheeler
Date: 2009-08-28 16:31:54 +0000 (Fri, 28 Aug 2009)
Log Message:
-----------
Change HardwareInterface::current_time_ from double to ros::Time.
Modified Paths:
--------------
pkg/trunk/controllers/control_toolbox/include/control_toolbox/base_position_pid.h
pkg/trunk/controllers/control_toolbox/include/control_toolbox/pid.h
pkg/trunk/controllers/control_toolbox/include/control_toolbox/sine_sweep.h
pkg/trunk/controllers/control_toolbox/src/base_position_pid.cpp
pkg/trunk/controllers/control_toolbox/src/pid.cpp
pkg/trunk/controllers/control_toolbox/src/sine_sweep.cpp
pkg/trunk/controllers/mechanism_controller_test/src/mechanism_controller_test.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/caster_calibration_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/gripper_calibration_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_traj_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/pr2_base_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/pr2_odometry.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/wrist_calibration_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/src/caster_calibration_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/gripper_calibration_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_traj_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/pr2_base_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/pr2_odometry.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/wrist_calibration_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_pose_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_twist_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_position_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_ud_calibration_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_velocity_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_pose_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_twist_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/joint_position_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/joint_ud_calibration_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/joint_velocity_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/trigger_controller.cpp
pkg/trunk/nav/teleop_base/src/ground_truth_controller.cpp
pkg/trunk/sandbox/dallas_controller/include/dallas_controller/dallas_controller.h
pkg/trunk/sandbox/dallas_controller/src/dallas_controller.cpp
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/arm_trajectory_controller.h
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/base_trajectory_controller.h
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/cartesian_hybrid_controller.h
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/cartesian_pose_twist_controller.h
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/cartesian_spline_trajectory_controller.h
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/cartesian_tff_controller.h
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/cartesian_trajectory_controller.h
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/cartesian_twist_controller_ik.h
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/joint_autotuner.h
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/joint_calibration_controller.h
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/joint_chain_constraint_controller.h
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/joint_chain_sine_controller.h
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/joint_inverse_dynamics_controller.h
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/joint_limit_calibration_controller.h
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/joint_pd_controller.h
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/joint_position_smoothing_controller.h
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/joint_trajectory_controller.h
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/pid_position_velocity_controller.h
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/pr2_gripper_controller.h
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/trajectory_controller.h
pkg/trunk/sandbox/experimental_controllers/src/arm_trajectory_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/base_trajectory_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/cartesian_hybrid_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/cartesian_pose_twist_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/cartesian_spline_trajectory_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/cartesian_tff_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/cartesian_trajectory_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/cartesian_twist_controller_ik.cpp
pkg/trunk/sandbox/experimental_controllers/src/joint_autotuner.cpp
pkg/trunk/sandbox/experimental_controllers/src/joint_calibration_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/joint_chain_constraint_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/joint_chain_sine_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/joint_limit_calibration_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/joint_pd_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/joint_position_smoothing_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/joint_trajectory_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/pid_position_velocity_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/pr2_gripper_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/probe.cpp
pkg/trunk/sandbox/experimental_controllers/src/trajectory_controller.cpp
pkg/trunk/sandbox/experimental_controllers/test/test_trajectory_srv.cpp
pkg/trunk/sandbox/manipulation_msgs/msg/SplineTrajSegment.msg
pkg/trunk/sandbox/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/checkout_controller.h
pkg/trunk/sandbox/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/hold_set_controller.h
pkg/trunk/sandbox/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/hysteresis_controller.h
pkg/trunk/sandbox/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/sine_sweep_controller.h
pkg/trunk/sandbox/qualification_controllers/joint_qualification_controllers/src/checkout_controller.cpp
pkg/trunk/sandbox/qualification_controllers/joint_qualification_controllers/src/hold_set_controller.cpp
pkg/trunk/sandbox/qualification_controllers/joint_qualification_controllers/src/hysteresis_controller.cpp
pkg/trunk/sandbox/qualification_controllers/joint_qualification_controllers/src/sine_sweep_controller.cpp
pkg/trunk/sandbox/trajectory_controllers/include/trajectory_controllers/joint_trajectory_controller2.h
pkg/trunk/sandbox/trajectory_controllers/src/joint_trajectory_controller2.cpp
pkg/trunk/stacks/pr2_ethercat_drivers/ethercat_hardware/include/ethercat_hardware/ethercat_hardware.h
pkg/trunk/stacks/pr2_ethercat_drivers/ethercat_hardware/src/ethercat_hardware.cpp
pkg/trunk/stacks/pr2_mechanism/mechanism_control/src/recorder.cpp
pkg/trunk/stacks/pr2_mechanism/mechanism_model/include/mechanism_model/robot.h
pkg/trunk/stacks/pr2_simulator/gazebo_plugin/src/gazebo_mechanism_control.cpp
Modified: pkg/trunk/controllers/control_toolbox/include/control_toolbox/base_position_pid.h
===================================================================
--- pkg/trunk/controllers/control_toolbox/include/control_toolbox/base_position_pid.h 2009-08-28 16:13:07 UTC (rev 23259)
+++ pkg/trunk/controllers/control_toolbox/include/control_toolbox/base_position_pid.h 2009-08-28 16:31:54 UTC (rev 23260)
@@ -61,7 +61,7 @@
* \param actual_pos The [x,y,w] position we're currently at
* \param elapsed_time The time between our current update and the previous update
*/
- tf::Vector3 updateControl(const tf::Vector3& commanded_pos, const tf::Vector3& actual_pos, double elapsed_time) ;
+ tf::Vector3 updateControl(const tf::Vector3& commanded_pos, const tf::Vector3& actual_pos, ros::Duration elapsed_time) ;
private:
control_toolbox::Pid pid_x_ ; //!< Input: odom x error. Output: Odom x velocity command
Modified: pkg/trunk/controllers/control_toolbox/include/control_toolbox/pid.h
===================================================================
--- pkg/trunk/controllers/control_toolbox/include/control_toolbox/pid.h 2009-08-28 16:13:07 UTC (rev 23259)
+++ pkg/trunk/controllers/control_toolbox/include/control_toolbox/pid.h 2009-08-28 16:31:54 UTC (rev 23260)
@@ -111,7 +111,7 @@
* \param p_error Error since last call (p_state-p_target)
* \param dt Change in time since last call
*/
- double updatePid(double p_error, double dt);
+ double updatePid(double p_error, ros::Duration dt);
/*!
* \brief Initialize PID-gains and integral term limits:[iMax:iMin]-[I1:I2]
@@ -178,7 +178,7 @@
* \param error_dot d(Error)/dt since last call
* \param dt Change in time since last call
*/
- double updatePid(double error, double error_dot, double dt);
+ double updatePid(double error, double error_dot, ros::Duration dt);
Pid &operator =(const Pid& p)
{
Modified: pkg/trunk/controllers/control_toolbox/include/control_toolbox/sine_sweep.h
===================================================================
--- pkg/trunk/controllers/control_toolbox/include/control_toolbox/sine_sweep.h 2009-08-28 16:13:07 UTC (rev 23259)
+++ pkg/trunk/controllers/control_toolbox/include/control_toolbox/sine_sweep.h 2009-08-28 16:31:54 UTC (rev 23260)
@@ -33,6 +33,8 @@
*********************************************************************/
#pragma once
+#include <ros/ros.h>
+
namespace control_toolbox {
/***************************************************/
/*! \class SineSweep
@@ -71,7 +73,7 @@
*
* \param dt Change in time since last call
*/
- double update(double dt);
+ double update(ros::Duration dt);
/*!
* \brief Intializes everything and calculates the constants for the sweep.
@@ -85,7 +87,7 @@
private:
double amplitude_; /**< Amplitude of the sweep. */
- double duration_; /**< Duration of the sweep. */
+ ros::Duration duration_; /**< Duration of the sweep. */
double start_angular_freq_; /**< Start angular frequency of the sweep. */
double end_angular_freq_; /**< End angular frequency of the sweep. */
double K_; /**< Constant \f$K\f$. */
Modified: pkg/trunk/controllers/control_toolbox/src/base_position_pid.cpp
===================================================================
--- pkg/trunk/controllers/control_toolbox/src/base_position_pid.cpp 2009-08-28 16:13:07 UTC (rev 23259)
+++ pkg/trunk/controllers/control_toolbox/src/base_position_pid.cpp 2009-08-28 16:31:54 UTC (rev 23260)
@@ -73,7 +73,7 @@
return true ;
}
-tf::Vector3 BasePositionPid::updateControl(const tf::Vector3& commanded_pos, const tf::Vector3& actual_pos, double time_elapsed)
+tf::Vector3 BasePositionPid::updateControl(const tf::Vector3& commanded_pos, const tf::Vector3& actual_pos, ros::Duration time_elapsed)
{
double err_x = actual_pos.x() - commanded_pos.x() ;
double err_y = actual_pos.y() - commanded_pos.y() ;
Modified: pkg/trunk/controllers/control_toolbox/src/pid.cpp
===================================================================
--- pkg/trunk/controllers/control_toolbox/src/pid.cpp 2009-08-28 16:13:07 UTC (rev 23259)
+++ pkg/trunk/controllers/control_toolbox/src/pid.cpp 2009-08-28 16:31:54 UTC (rev 23260)
@@ -138,7 +138,7 @@
}
-double Pid::updatePid(double error, double dt)
+double Pid::updatePid(double error, ros::Duration dt)
{
double p_term, d_term, i_term;
p_error_ = error; //this is pError = pState-pTarget
@@ -155,7 +155,7 @@
p_term = p_gain_ * p_error_;
// Calculate the integral error
- i_error_ = i_error_ + dt * p_error_;
+ i_error_ = i_error_ + dt.toSec() * p_error_;
//Calculate integral contribution to command
i_term = i_gain_ * i_error_;
@@ -173,9 +173,9 @@
}
// Calculate the derivative error
- if (dt != 0)
+ if (dt.toSec() != 0)
{
- d_error_ = (p_error_ - p_error_last_) / dt;
+ d_error_ = (p_error_ - p_error_last_) / dt.toSec();
p_error_last_ = p_error_;
}
// Calculate derivative contribution to command
@@ -186,7 +186,7 @@
}
-double Pid::updatePid(double error, double error_dot, double dt)
+double Pid::updatePid(double error, double error_dot, ros::Duration dt)
{
double p_term, d_term, i_term;
p_error_ = error; //this is pError = pState-pTarget
@@ -203,7 +203,7 @@
p_term = p_gain_ * p_error_;
// Calculate the integral error
- i_error_ = i_error_ + dt * p_error_;
+ i_error_ = i_error_ + dt.toSec() * p_error_;
//Calculate integral contribution to command
i_term = i_gain_ * i_error_;
Modified: pkg/trunk/controllers/control_toolbox/src/sine_sweep.cpp
===================================================================
--- pkg/trunk/controllers/control_toolbox/src/sine_sweep.cpp 2009-08-28 16:13:07 UTC (rev 23259)
+++ pkg/trunk/controllers/control_toolbox/src/sine_sweep.cpp 2009-08-28 16:31:54 UTC (rev 23260)
@@ -44,7 +44,7 @@
K_=0.0;
L_=0.0;
amplitude_=0.0;
- duration_=0.0;
+ duration_ = ros::Duration(0.0);
cmd_ = 0.0;
}
@@ -60,7 +60,7 @@
return false;
amplitude_ = amplitude;
- duration_ = duration;
+ duration_ = ros::Duration(duration);
//calculate the angular fequencies
start_angular_freq_ =2*M_PI*start_freq;
end_angular_freq_ =2*M_PI*end_freq;
@@ -75,11 +75,11 @@
return true;
}
-double SineSweep::update( double dt)
+double SineSweep::update( ros::Duration dt)
{
if(dt<=duration_)
{
- cmd_= amplitude_*sin(K_*(exp((dt)/(L_))-1));
+ cmd_= amplitude_*sin(K_*(exp((dt.toSec())/(L_))-1));
}
else
{
Modified: pkg/trunk/controllers/mechanism_controller_test/src/mechanism_controller_test.cpp
===================================================================
--- pkg/trunk/controllers/mechanism_controller_test/src/mechanism_controller_test.cpp 2009-08-28 16:13:07 UTC (rev 23259)
+++ pkg/trunk/controllers/mechanism_controller_test/src/mechanism_controller_test.cpp 2009-08-28 16:31:54 UTC (rev 23260)
@@ -97,7 +97,7 @@
int count = 0;
while(ros::ok())
{
- hw.hw_->current_time_ = count / 1.0e-3;
+ hw.hw_->current_time_ = ros::Time(count / 1.0e-3);
mc.update();
if(count % 1000000 == 0)
printf("%d seconds simulated \n", count / 1000);
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/caster_calibration_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/caster_calibration_controller.h 2009-08-28 16:13:07 UTC (rev 23259)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/caster_calibration_controller.h 2009-08-28 16:31:54 UTC (rev 23260)
@@ -80,7 +80,7 @@
controller::CasterController cc_;
- double last_publish_time_;
+ ros::Time last_publish_time_;
boost::scoped_ptr<realtime_tools::RealtimePublisher<std_msgs::Empty> > pub_calibrated_;
};
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/gripper_calibration_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/gripper_calibration_controller.h 2009-08-28 16:13:07 UTC (rev 23259)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/gripper_calibration_controller.h 2009-08-28 16:31:54 UTC (rev 23260)
@@ -69,7 +69,7 @@
ros::NodeHandle node_;
mechanism::RobotState *robot_;
- double last_publish_time_;
+ ros::Time last_publish_time_;
boost::scoped_ptr<realtime_tools::RealtimePublisher<std_msgs::Empty> > pub_calibrated_;
double search_velocity_;
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_traj_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_traj_controller.h 2009-08-28 16:13:07 UTC (rev 23259)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_traj_controller.h 2009-08-28 16:31:54 UTC (rev 23260)
@@ -109,7 +109,7 @@
std::string name_ ; // The controller name. Used for ROS_INFO Messages
- double traj_start_time_ ; // The time that the trajectory was started (in seconds)
+ ros::Time traj_start_time_ ; // The time that the trajectory was started (in seconds)
double traj_duration_ ; // The length of the current profile (in seconds)
@@ -119,7 +119,7 @@
// Control loop state
control_toolbox::Pid pid_controller_ ; // Position PID Controller
filters::TransferFunctionFilter<double> d_error_filter ; // Filter on derivative term of error
- double last_time_ ; // The previous time at which the control loop was run
+ ros::Time last_time_ ; // The previous time at which the control loop was run
double last_error_ ; // Error for the previous time at which the control loop was run
double tracking_offset_ ; // Position cmd generated by the track_link code
double traj_command_ ; // Position cmd generated by the trajectory code
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/pr2_base_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/pr2_base_controller.h 2009-08-28 16:13:07 UTC (rev 23259)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/pr2_base_controller.h 2009-08-28 16:31:54 UTC (rev 23260)
@@ -140,12 +140,12 @@
/*!
* \brief time corresponding to when update was last called
*/
- double last_time_;
+ ros::Time last_time_;
/*!
* \brief timestamp corresponding to when the command received by the node
*/
- double cmd_received_timestamp_;
+ ros::Time cmd_received_timestamp_;
/*!
* \brief Input speed command vector represents the desired speed requested by the node. Note that this may differ from the
@@ -296,7 +296,7 @@
/*!
* \brief Time interval between state publishing
*/
- double last_publish_time_;
+ ros::Time last_publish_time_;
/*!
* \brief minimum tranlational velocity value allowable
@@ -311,7 +311,7 @@
/*!
* \brief Publish the state
*/
- void publishState(double current_time);
+ void publishState(ros::Time current_time);
};
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/pr2_odometry.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/pr2_odometry.h 2009-08-28 16:13:07 UTC (rev 23259)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/pr2_odometry.h 2009-08-28 16:31:54 UTC (rev 23260)
@@ -176,7 +176,7 @@
/*!
* \brief Stores the last update time and the current update time
*/
- double last_time_,current_time_;
+ ros::Time last_time_,current_time_;
/*!
* \brief Matricies used in the computation of the iterative least squares and related functions
@@ -226,7 +226,7 @@
/*!
* \brief The last time the odometry information was published
*/
- double last_publish_time_;
+ ros::Time last_publish_time_;
/*!
* \brief The time that the odometry is expected to be published next
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/wrist_calibration_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/wrist_calibration_controller.h 2009-08-28 16:13:07 UTC (rev 23259)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/wrist_calibration_controller.h 2009-08-28 16:31:54 UTC (rev 23260)
@@ -78,7 +78,7 @@
mechanism::RobotState *robot_;
ros::NodeHandle node_;
- double last_publish_time_;
+ ros::Time last_publish_time_;
boost::scoped_ptr<realtime_tools::RealtimePublisher<std_msgs::Empty> > pub_calibrated_;
double search_velocity_;
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/caster_calibration_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/caster_calibration_controller.cpp 2009-08-28 16:13:07 UTC (rev 23259)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/caster_calibration_controller.cpp 2009-08-28 16:31:54 UTC (rev 23260)
@@ -249,7 +249,7 @@
if (pub_calibrated_)
{
- if (last_publish_time_ + 0.5 < robot_->getTime())
+ if (last_publish_time_ + ros::Duration(0.5) < robot_->getTime())
{
if (pub_calibrated_->trylock())
{
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/gripper_calibration_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/gripper_calibration_controller.cpp 2009-08-28 16:13:07 UTC (rev 23259)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/gripper_calibration_controller.cpp 2009-08-28 16:31:54 UTC (rev 23260)
@@ -191,7 +191,7 @@
case CALIBRATED:
if (pub_calibrated_)
{
- if (last_publish_time_ + 0.5 < robot_->getTime())
+ if (last_publish_time_ + ros::Duration(0.5) < robot_->getTime())
{
if (pub_calibrated_->trylock())
{
@@ -209,3 +209,4 @@
}
+
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_traj_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_traj_controller.cpp 2009-08-28 16:13:07 UTC (rev 23259)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_traj_controller.cpp 2009-08-28 16:31:54 UTC (rev 23260)
@@ -235,16 +235,14 @@
// ***** Run the position control loop *****
double cmd = traj_command_ + tracking_offset_ ;
- double time = robot_->getTime() ;
+ ros::Time time = robot_->getTime();
double error(0.0) ;
angles::shortest_angular_distance_with_limits(cmd, joint_state_->position_,
joint_state_->joint_->joint_limit_min_,
joint_state_->joint_->joint_limit_max_,
error) ;
-
-
- double dt = time - last_time_ ;
- double d_error = (error-last_error_)/dt ;
+ ros::Duration dt = time - last_time_ ;
+ double d_error = (error-last_error_)/dt.toSec();
std::vector<double> vin;
vin.push_back(d_error);
std::vector<double> vout = vin;
@@ -262,8 +260,8 @@
double LaserScannerTrajController::getCurProfileTime()
{
- double time = robot_->getTime() ;
- double time_from_start = time - traj_start_time_ ;
+ ros::Time time = robot_->getTime();
+ double time_from_start = (time - traj_start_time_).toSec();
double mod_time = time_from_start - floor(time_from_start/traj_.getTotalTime())*traj_.getTotalTime() ;
return mod_time ;
}
@@ -484,8 +482,7 @@
if (cur_profile_segment != prev_profile_segment_)
{
// Should we be populating header.stamp here? Or, we can simply let ros take care of the timestamp
- ros::Time cur_time ;
- cur_time.fromSec(robot_->getTime()) ;
+ ros::Time cur_time(robot_->getTime()) ;
m_scanner_signal_.header.stamp = cur_time ;
m_scanner_signal_.signal = cur_profile_segment ;
need_to_send_msg_ = true ;
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/pr2_base_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/pr2_base_controller.cpp 2009-08-28 16:13:07 UTC (rev 23259)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/pr2_base_controller.cpp 2009-08-28 16:31:54 UTC (rev 23260)
@@ -61,7 +61,7 @@
// state_publisher_ = NULL;
- last_publish_time_ = 0.0;
+ last_publish_time_ = ros::Time(0.0);
pthread_mutex_init(&pr2_base_controller_lock_, NULL);
}
@@ -268,8 +268,8 @@
void Pr2BaseController::update()
{
- double current_time = base_kin_.robot_state_->getTime();
- double dT = std::min<double>(current_time - last_time_, base_kin_.MAX_DT_);
+ ros::Time current_time = base_kin_.robot_state_->getTime();
+ double dT = std::min<double>((current_time - last_time_).toSec(), base_kin_.MAX_DT_);
if(new_cmd_available_)
{
@@ -283,7 +283,7 @@
}
}
- if((current_time - cmd_received_timestamp_) > timeout_)
+ if((current_time - cmd_received_timestamp_).toSec() > timeout_)
{
cmd_vel_.linear.x = 0;
cmd_vel_.linear.y = 0;
@@ -306,10 +306,10 @@
}
-void Pr2BaseController::publishState(double time)
+void Pr2BaseController::publishState(ros::Time time)
{
- if(time - last_publish_time_ < state_publish_time_)
+ if((time - last_publish_time_).toSec() < state_publish_time_)
{
return;
}
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/pr2_odometry.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/pr2_odometry.cpp 2009-08-28 16:13:07 UTC (rev 23259)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/pr2_odometry.cpp 2009-08-28 16:31:54 UTC (rev 23260)
@@ -123,7 +123,7 @@
void Pr2Odometry::updateOdometry()
{
- double dt = current_time_ - last_time_;
+ double dt = (current_time_ - last_time_).toSec();
double theta = odom_.z;
double costh = cos(theta);
double sinth = sin(theta);
@@ -152,7 +152,7 @@
void Pr2Odometry::getOdometryMessage(nav_msgs::Odometry &msg)
{
msg.header.frame_id = odom_frame_;
- msg.header.stamp.fromSec(current_time_);
+ msg.header.stamp = current_time_;
msg.pose.pose.position.x = odom_.x;
msg.pose.pose.position.y = odom_.y;
msg.pose.pose.position.z = 0.0;
@@ -341,7 +341,7 @@
void Pr2Odometry::publish()
{
- if(fabs(last_publish_time_ - current_time_) < expected_publish_time_)
+ if(fabs((last_publish_time_ - current_time_).toSec()) < expected_publish_time_)
return;
if(odometry_publisher_->trylock())
@@ -357,7 +357,7 @@
this->getOdometry(x, y, yaw, vx, vy, vyaw);
geometry_msgs::TransformStamped &out = transform_publisher_->msg_.transforms[0];
- out.header.stamp.fromSec(current_time_);
+ out.header.stamp = current_time_;
out.header.frame_id = base_footprint_frame_;
out.child_frame_id = odom_frame_;
out.transform.translation.x = -x * cos(yaw) - y * sin(yaw);
@@ -371,7 +371,7 @@
out.transform.rotation.w = quat_trans.w();
geometry_msgs::TransformStamped &out2 = transform_publisher_->msg_.transforms[1];
- out2.header.stamp.fromSec(current_time_);
+ out2.header.stamp = current_time_;
out2.header.frame_id = base_footprint_frame_;
out2.child_frame_id = base_link_frame_;
out2.transform.translation.x = 0;
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/wrist_calibration_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/wrist_calibration_controller.cpp 2009-08-28 16:13:07 UTC (rev 23259)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/wrist_calibration_controller.cpp 2009-08-28 16:31:54 UTC (rev 23260)
@@ -362,7 +362,7 @@
if (pub_calibrated_)
{
- if (last_publish_time_ + 0.5 < robot_->getTime())
+ if (last_publish_time_ + ros::Duration(0.5) < robot_->getTime())
{
assert(pub_calibrated_);
if (pub_calibrated_->trylock())
@@ -387,3 +387,4 @@
}
+
Modified: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_pose_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_pose_controller.h 2009-08-28 16:13:07 UTC (rev 23259)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_pose_controller.h 2009-08-28 16:31:54 UTC (rev 23260)
@@ -112,7 +112,7 @@
ros::NodeHandle node_;
std::string controller_name_, root_name_;
- double last_time_;
+ ros::Time last_time_;
// robot structure
mechanism::RobotState *robot_state_;
Modified: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_twist_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_twist_controller.h 2009-08-28 16:13:07 UTC (rev 23259)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_twist_controller.h 2009-08-28 16:31:54 UTC (rev 23260)
@@ -97,7 +97,8 @@
ros::NodeHandle node_;
ros::Subscriber sub_command_;
void command(const geometry_msgs::TwistConstPtr& twist_msg);
- double last_time_, ff_trans_, ff_rot_;
+ double ff_trans_, ff_rot_;
+ ros::Time last_time_;
// pid controllers
std::vector<control_toolbox::Pid> fb_pid_controller_;
Modified: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_position_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_position_controller.h 2009-08-28 16:13:07 UTC (rev 23259)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_position_controller.h 2009-08-28 16:31:54 UTC (rev 23260)
@@ -109,7 +109,7 @@
std::string getJointName();
mechanism::JointState *joint_state_; /**< Joint we're controlling. */
- double dt_;
+ ros::Duration dt_;
double command_; /**< Last commanded position. */
private:
@@ -117,7 +117,7 @@
bool initialized_;
mechanism::RobotState *robot_; /**< Pointer to robot structure. */
control_toolbox::Pid pid_controller_; /**< Internal PID controller. */
- double last_time_; /**< Last time stamp of update. */
+ ros::Time last_time_; /**< Last time stamp of update. */
ros::NodeHandle node_;
Modified: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_ud_calibration_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_ud_calibration_controller.h 2009-08-28 16:13:07 UTC (rev 23259)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_ud_calibration_controller.h 2009-08-28 16:31:54 UTC (rev 23260)
@@ -67,7 +67,7 @@
mechanism::RobotState* robot_;
ros::NodeHandle node_;
boost::scoped_ptr<realtime_tools::RealtimePublisher<std_msgs::Empty> > pub_calibrated_;
- double last_publish_time_;
+ ros::Time last_publish_time_;
enum { INITIALIZED, BEGINNING, MOVING_TO_LOW, MOVING_TO_HIGH, CALIBRATED };
int state_;
Modified: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_velocity_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_velocity_controller.h 2009-08-28 16:13:07 UTC (rev 23259)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_velocity_controller.h 2009-08-28 16:31:54 UTC (rev 23260)
@@ -122,14 +122,14 @@
std::string getJointName();
mechanism::JointState *joint_state_; /**< Joint we're controlling. */
- double dt_;
+ ros::Duration dt_;
double command_; /**< Last commanded position. */
private:
ros::NodeHandle node_;
mechanism::RobotState *robot_; /**< Pointer to robot structure. */
control_toolbox::Pid pid_controller_; /**< Internal PID controller. */
- double last_time_; /**< Last time stamp of update. */
+ ros::Time last_time_; /**< Last time stamp of update. */
int loop_count_;
friend class JointVelocityControllerNode;
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_pose_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_pose_controller.cpp 2009-08-28 16:13:07 UTC (rev 23259)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_pose_controller.cpp 2009-08-28 16:31:54 UTC (rev 23260)
@@ -138,8 +138,8 @@
void CartesianPoseController::update()
{
// get time
- double time = robot_state_->getTime();
- double dt = time - last_time_;
+ ros::Time time = robot_state_->getTime();
+ ros::Duration dt = time - last_time_;
last_time_ = time;
// get current pose
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_twist_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_twist_controller.cpp 2009-08-28 16:13:07 UTC (rev 23259)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_twist_controller.cpp 2009-08-28 16:31:54 UTC (rev 23260)
@@ -159,8 +159,8 @@
return;
// get time
- double time = robot_state_->getTime();
- double dt = time - last_time_;
+ ros::Time time = robot_state_->getTime();
+ ros::Duration dt = time - last_time_;
last_time_ = time;
// get the joint positions and velocities
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/joint_position_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/joint_position_controller.cpp 2009-08-28 16:13:07 UTC (rev 23259)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/joint_position_controller.cpp 2009-08-28 16:31:54 UTC (rev 23260)
@@ -158,7 +158,7 @@
assert(robot_ != NULL);
double error(0);
- double time = robot_->getTime();
+ ros::Time time = robot_->getTime();
assert(joint_state_->joint_);
dt_= time - last_time_;
@@ -192,7 +192,7 @@
controller_state_publisher_->msg_.set_point = command_;
controller_state_publisher_->msg_.process_value = joint_state_->position_;
controller_state_publisher_->msg_.error = error;
- controller_state_publisher_->msg_.time_step = dt_;
+ controller_state_publisher_->msg_.time_step = dt_.toSec();
double dummy;
getGains(controller_state_publisher_->msg_.p,
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/joint_ud_calibration_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/joint_ud_calibration_controller.cpp 2009-08-28 16:13:07 UTC (rev 23259)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/joint_ud_calibration_controller.cpp 2009-08-28 16:31:54 UTC (rev 23260)
@@ -245,7 +245,7 @@
case CALIBRATED:
if (pub_calibrated_)
{
- if (last_publish_time_ + 0.5 < robot_->getTime())
+ if (last_publish_time_ + ros::Duration(0.5) < robot_->getTime())
{
assert(pub_calibrated_);
if (pub_calibrated_->trylock())
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/joint_velocity_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/joint_velocity_controller.cpp 2009-08-28 16:13:07 UTC (rev 23259)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/joint_velocity_controller.cpp 2009-08-28 16:31:54 UTC (rev 23260)
@@ -160,7 +160,7 @@
void JointVelocityController::update()
{
assert(robot_ != NULL);
- double time = robot_->getTime();
+ ros::Time time = robot_->getTime();
double error = joint_state_->velocity_ - command_;
dt_ = time - last_time_;
@@ -173,7 +173,7 @@
controller_state_publisher_->msg_.set_point = command_;
controller_state_publisher_->msg_.process_value = joint_state_->velocity_;
controller_state_publisher_->msg_.error = error;
- controller_state_publisher_->msg_.time_step = dt_;
+ controller_state_publisher_->msg_.time_step = dt_.toSec();
double dummy;
getGains(controller_state_publisher_->msg_.p,
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/trigger_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/trigger_controller.cpp 2009-08-28 16:13:07 UTC (rev 23259)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/trigger_controller.cpp 2009-08-28 16:31:54 UTC (rev 23260)
@@ -82,7 +82,7 @@
void TriggerController::update()
{
- double curtime = robot_->getTime();
+ ros::Time curtime = robot_->getTime();
double tick = getTick(curtime, config_);
bool active = false;
Modified: pkg/trunk/nav/teleop_base/src/ground_truth_controller.cpp
===================================================================
--- pkg/trunk/nav/teleop_base/src/ground_truth_controller.cpp 2009-08-28 16:13:07 UTC (rev 23259)
+++ pkg/trunk/nav/teleop_base/src/ground_truth_controller.cpp 2009-08-28 16:31:54 UTC (rev 23260)
@@ -128,7 +128,7 @@
// Determine next velocity to command
tf::Vector3 vel_cmd ;
- vel_cmd = base_position_pid_.updateControl(xyt_target_, xyt_current, time_elapsed.toSec()) ;
+ vel_cmd = base_position_pid_.updateControl(xyt_target_, xyt_current, time_elapsed) ;
geometry_msgs::Twist base_vel ;
base_vel.linear.x = vel_cmd.x() ;
Modified: pkg/trunk/sandbox/dallas_controller/include/dallas_controller/dallas_controller.h
===================================================================
--- pkg/trunk/sandbox/dallas_controller/include/dallas_controller/dallas_controller.h 2009-08-28 16:13:07 UTC (rev 23259)
+++ pkg/trunk/sandbox/dallas_controller/include/dallas_controller/dallas_controller.h 2009-08-28 16:31:54 UTC (rev 23260)
@@ -60,14 +60,14 @@
mechanism::RobotState *robot_;
ros::NodeHandle node_;
- double last_time_;
+ ros::Time last_time_;
ros::Subscriber sub_command_;
void command(const geometry_msgs::TwistConstPtr &msg);
// For pushing commands into realtime atomically
struct Command {
double vx, va;
- double received_time;
+ ros::Time received_time;
};
realtime_tools::RealtimeInfuser<Command> infuser_;
Modified: pkg/trunk/sandbox/dallas_controller/src/dallas_controller.cpp
===================================================================
--- pkg/trunk/sandbox/dallas_controller/src/dallas_controller.cpp 2009-08-28 16:13:07 UTC (rev 23259)
+++ pkg/trunk/sandbox/dallas_controller/src/dallas_controller.cpp 2009-08-28 16:31:54 UTC (rev 23260)
@@ -73,12 +73,10 @@
void DallasController::update()
{
- double time = robot_->getTime();
- double dt = time - last_time_;
+ ros::Time time = robot_->getTime();
-
Command &command = infuser_.next();
- if (true || command.received_time <= time + 1.0)
+ if (true || command.received_time <= time + ros::Duration(1.0))
{
// Where should the caster be facing?
double caster_angle = cc_.getSteerPosition();
@@ -113,7 +111,7 @@
c.received_time = last_time_;
c.vx = msg->linear.x;
c.va = msg->angular.z;
- ROS_ERROR("Command: %.3lf (%.3lf, %.3lf)", c.received_time, c.vx, c.va);
+ ROS_ERROR("Command: %.3lf (%.3lf, %.3lf)", c.received_time.toSec(), c.vx, c.va);
infuser_.set(c);
}
Modified: pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/arm_trajectory_controller.h
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/arm_trajectory_controller.h 2009-08-28 16:13:07 UTC (rev 23259)
+++ pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/arm_trajectory_controller.h 2009-08-28 16:31:54 UTC (rev 23260)
@@ -137,11 +137,11 @@
// Indicates if goals_ and error_margins_ should be copied into goals_rt_ and error_margins_rt_
bool refresh_rt_vals_;
- double trajectory_start_time_;
+ ros::Time trajectory_start_time_;
- double trajectory_end_time_;
+ ros::Time trajectory_end_time_;
- double current_time_;
+ ros::Time current_time_;
bool trajectory_done_;
@@ -296,7 +296,7 @@
double trajectory_wait_timeout_;
- double last_diagnostics_publish_time_;
+ ros::Time last_diagnostics_publish_time_;
double diagnostics_publish_delta_time_;
Modified: pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/base_trajectory_controller.h
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/base_trajectory_controller.h 2009-08-28 16:13:07 UTC (rev 23259)
+++ pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/base_trajectory_controller.h 2009-08-28 16:31:54 UTC (rev 23260)
@@ -92,10 +92,10 @@
ros::Node& ros_node_;
tf::TransformListener& tf_;
int dimension_;
- double current_time_;
+ ros::Time current_time_;
double sample_time_;
- double last_update_time_;
- double trajectory_start_time_;
+ ros::Time last_update_time_;
+ ros::Time trajectory_start_time_;
trajectory::Trajectory *trajectory_;
bool stop_motion_;
int stop_motion_count_;
@@ -109,7 +109,7 @@
double xy_goal_tolerance_;
boost::mutex ros_lock_;
- double path_updated_time_;
+ ros::Time path_updated_time_;
double max_update_time_;
geometry_msgs::Twist checkCmd(const geometry_msgs::Twist &cmd);
Modified: pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/cartesian_hybrid_controller.h
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/cartesian_hybrid_controller.h 2009-08-28 16:13:07 UTC (rev 23259)
+++ pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/cartesian_hybrid_controller.h 2009-08-28 16:31:54 UTC (rev 23260)
@@ -88,7 +88,7 @@
mechanism::Chain chain_;
KDL::Chain kdl_chain_;
mechanism::RobotState *robot_;
- double last_time_;
+ ros::Time last_time_;
int initial_mode_;
Modified: pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/cartesian_pose_twist_controller.h
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/cartesian_pose_twist_controller.h 2009-08-28 16:13:07 UTC (rev 23259)
+++ pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/cartesian_pose_twist_controller.h 2009-08-28 16:31:54 UTC (rev 23260)
@@ -80,7 +80,7 @@
ros::NodeHandle node_;
std::string controller_name_, root_name_;
- double last_time_;
+ ros::Time last_time_;
// output of the controller
KDL::Wrench wrench_out_;
Modified: pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/cartesian_spline_trajectory_controller.h
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/cartesian_spline_trajectory_controller.h 2009-08-28 16:13:07 UTC (rev 23259)
+++ pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/cartesian_spline_trajectory_controller.h 2009-08-28 16:31:54 UTC (rev 23260)
@@ -97,7 +97,8 @@
bool new_cmd_available_, spline_done_;
- double last_time_, spline_time_;
+ ros::Time last_time_;
+ ros::Duration spline_time_;
manipulation_msgs::Waypoint getCommand(const manipulation_msgs::SplineTrajSegment &spline, const double t);
Modified: pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/cartesian_tff_controller.h
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/cartesian_tff_controller.h 2009-08-28 16:13:07 UTC (rev 23259)
+++ pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/cartesian_tff_controller.h 2009-08-28 16:31:54 UTC (rev 23260)
@@ -64,7 +64,7 @@
private:
ros::NodeHandle node_;
ros::Subscriber sub_command_;
- double last_time_;
+ ros::Time last_time_;
// pid controllers
std::vector<control_toolbox::Pid> vel_pid_controller_, pos_pid_controller_;
Modified: pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/cartesian_trajectory_controller.h
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/cartesian_trajectory_controller.h 2009-08-28 16:13:07 UTC (rev 23259)
+++ pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/cartesian_trajectory_controller.h 2009-08-28 16:31:54 UTC (rev 23260)
@@ -81,7 +81,8 @@
ros::ServiceServer move_to_srv_, preempt_srv_;
std::string controller_name_;
- double last_time_, time_started_, time_passed_, max_duration_;
+ ros::Time last_time_, time_started_;
+ double time_passed_, max_duration_;
bool is_moving_, request_preempt_, exceed_tolerance_;
KDL::Frame pose_begin_, pose_end_, pose_current_;
Modified: pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/cartesian_twist_controller_ik.h
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/cartesian_twist_controller_ik.h 2009-08-28 16:13:07 UTC (rev 23259)
+++ pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/cartesian_twist_controller_ik.h 2009-08-28 16:31:54 UTC (rev 23260)
@@ -73,7 +73,8 @@
ros::NodeHandle node_;
ros::Subscriber sub_command_;
- double last_time_,ff_trans_,ff_rot_;
+ ros::Time last_time_;
+ double ff_trans_,ff_rot_;
// pid controllers
std::vector<control_toolbox::Pid> fb_pid_controller_;
Modified: pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/joint_autotuner.h
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/joint_autotuner.h 2009-08-28 16:13:07 UTC (rev 23259)
+++ pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/joint_autotuner.h 2009-08-28 16:31:54 UTC (rev 23260)
@@ -106,7 +106,7 @@
/*!
* \brief Get latest time..
*/
- double getTime();
+ ros::Time getTime();
/*!
@@ -126,7 +126,7 @@
mechanism::Joint* joint_; /**< Joint we're controlling.> */
mechanism::JointState* joint_state_; /**< Joint we're controlling.> */
control_toolbox::Pid pid_controller_; /**< Internal PID controller.> */
- double last_time_; /**< Last time stamp of update.> */
+ ros::Time last_time_; /**< Last time stamp of update.> */
double command_; /**< Last commanded position.> */
mechanism::Robot *robot_; /**< Pointer to robot structure.> */
mechanism::RobotState *robot_state_; /**< Pointer to robot structure.> */
@@ -144,7 +144,7 @@
double relay_height_;/**< Amount of relay input> */
int successful_cycles_;/**< Number of matching cycles > */
double crossing_point_;/**< Location of crossover point for relay test> */
- double cycle_start_time_;/**< Mark time of cycle start> */
+ ros::Time cycle_start_time_;/**< Mark time of cycle start> */
int num_cycles_; /*!<Number of cycles that need to match for autotuner to read as stable>!*/
double amplitude_tolerance_; /*!<% variation amplitude allowed between successful cycles>!*/
Modified: pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/joint_calibration_controller.h
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/joint_calibration_controller.h 2009-08-28 16:13:07 UTC (rev 23259)
+++ pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/joint_calibration_controller.h 2009-08-28 16:31:54 UTC (rev 23260)
@@ -114,7 +114,7 @@
mechanism::RobotState* robot_;
JointCalibrationController c_;
- double last_publish_time_;
+ ros::Time last_publish_time_;
realtime_tools::RealtimePublisher<std_msgs::Empty> *pub_calibrated_;
};
Modified: pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/joint_chain_constraint_controller.h
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/joint_chain_constraint_controller.h 2009-08-28 16:13:07 UTC (rev 23259)
+++ pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/joint_chain_constraint_controller.h 2009-08-28 16:31:54 UTC (rev 23260)
@@ -145,7 +145,7 @@
Eigen::MatrixXf joint_constraint_jacobian_;
Eigen::MatrixXf joint_constraint_null_space_;
- double last_time_;
+ ros::Time last_time_;
bool initialized_;
Modified: pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/joint_chain_sine_controller.h
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/joint_chain_sine_controller.h 2009-08-28 16:13:07 UTC (rev 23259)
+++ pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/joint_chain_sine_controller.h 2009-08-28 16:31:54 UTC (rev 23260)
@@ -89,7 +89,7 @@
private:
mechanism::Chain mechanism_chain_; /**< Kinematic chain */
mechanism::RobotState *robot_state_; /**< Pointer to robot structure. */
- double last_time_; /**< Last time stamp of update. */
+ ros::Time last_time_; /**< Last time stamp of update. */
bool initialized_; /**< Flag which indicates whether this class has been initialized */
KDL::Chain kdl_chain_; /**< KDL chain */
KDL::JntArrayVel jnt_pos_vel_; /**< Joint positions and velocities */
Modified: pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/joint_inverse_dynamics_controller.h
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/joint_inverse_dynamics_controller.h 2009-08-28 16:13:07 UTC (rev 23259)
+++ pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/joint_inverse_dynamics_controller.h 2009-08-28 16:31:54 UTC (rev 23260)
@@ -78,7 +78,7 @@
int counter;
bool publishDiagnostics(int level, const std::string& message);
- double last_time_;
+ ros::Time last_time_;
ros::Publisher pub_pos_desi_, pub_pos_meas_, pub_vel_desi_, pub_vel_meas_;
ros::Publisher pub_acc_desi_, pub_acc_control_, pub_eff_calculated_, pub_eff_sent_;
Modified: pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/joint_limit_calibration_controller.h
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/joint_limit_calibration_controller.h 2009-08-28 16:13:07 UTC (rev 23259)
+++ pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/joint_limit_calibration_controller.h 2009-08-28 16:31:54 UTC (rev 23260)
@@ -123,7 +123,7 @@
mechanism::RobotState* robot_;
JointLimitCalibrationController c_;
- double last_publish_time_;
+ ros::Time last_publish_time_;
realtime_tools::RealtimePublisher<std_msgs::Empty> *pub_calibrated_;
};
Modified: pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/joint_pd_controller.h
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/joint_pd_controller.h 2009-08-28 16:13:07 UTC (rev 23259)
+++ pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/joint_pd_controller.h 2009-08-28 16:31:54 UTC (rev 23260)
@@ -115,7 +115,7 @@
mechanism::RobotState *robot_; /**< Pointer to robot structure. */
control_toolbox::Pid pid_controller_; /**< Internal PID controller. */
- double last_time_; /**< Last time stamp of update. */
+ ros::Time last_time_; /**< Last time stamp of update. */
double command_; /**< Last commanded position. */
double command_dot_;
double command_t_; /**< Last commanded position. */
Modified: pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/joint_position_smoothing_controller.h
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/joint_position_smoothing_controller.h 2009-08-28 16:13:07 UTC (rev 23259)
+++ pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/joint_position_smoothing_controller.h 2009-08-28 16:31:54 UTC (rev 23260)
@@ -107,7 +107,7 @@
mechanism::RobotState *robot_; /**< Pointer to robot structure. */
control_toolbox::Pid pid_controller_; /**< Internal PID controller. */
- double last_time_; /**< Last time stamp of update. */
+ ros::Time last_time_; /**< Last time stamp of update. */
double command_; /**< Last commanded position. */
double smoothed_error_;
double smoothing_factor_;
Modified: pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/joint_trajectory_controller.h
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/joint_trajectory_controller.h 2009-08-28 16:13:07 UTC (rev 23259)
+++ pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/joint_trajectory_controller.h 2009-08-28 16:31:54 UTC (rev 23260)
@@ -118,7 +118,7 @@
boost::mutex arm_controller_lock_; /**< Mutex lock for sharing information between services and realtime */
- double last_time_; /**< last time update was called */
+ ros::Time last_time_; /**< last time update was called */
std::vector<std::string> joint_name_; /**< names of joints controlled by this controller */
@@ -148,11 +148,11 @@
bool refresh_rt_vals_; /**< Indicates that a new trajectory has been received. */
- double trajectory_start_time_; /**< Start time for the current trajectory */
+ ros::Time trajectory_start_time_; /**< Start time for the current trajectory */
- double trajectory_end_time_; /**< End time for the current trajectory */
+ ros::Time trajectory_end_time_; /**< End time for the current trajectory */
- double current_time_; /**< Current time */
+ ros::Time current_time_; /**< Current time */
int num_joints_; /**< Number of joints controlled by this controller */
@@ -170,7 +170,7 @@
double max_update_time_; /**< maximum time (over the complete history of the run) taken for the update loop of this controller*/
- double last_traj_req_time_; /**< Last time a trajectory command was received on a topic or service */
+ ros::Time last_traj_req_time_; /**< Last time a trajectory command was received on a topic or service */
double max_allowed_update_time_; /**< Safety behavior: Max allowed time between commands */
@@ -294,7 +294,7 @@
* (a) max_allowed_update_time_ which can be set using a ROS param call, e.g. <param name="max_allowed_update_time" type="double" value="0.1"/>
* (b) max_allowable_joint_errors_ which can be set individually for each joint using a ROS param call, e.g. <param name="r_shoulder_pan_joint/joint_error_threshold" type="double" value="0.1"/>
*/
- bool checkWatchDog(double current_time);
+ bool checkWatchDog(ros::Time current_time);
/**
* @brief Stop the motion of all joints. In addition to setting all desired joint positions to the current position, it also sets velocities for the base to zero
@@ -466,7 +466,7 @@
*/
double trajectory_wait_timeout_;
- double last_diagnostics_publish_time_; /**< Last time diagnostics infomation was published */
+ ros::Time last_diagnostics_publish_time_; /**< Last time diagnostics infomation was published */
double diagnostics_publish_delta_time_; /**< Expected rate at which diagnostics information should be published. This can be published using a ROS param call <param name="diagnostics_publish_delta_time" type="double" value="0.5"/> */
Modified: pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/pid_position_velocity_controller.h
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/pid_position_velocity_controller.h 2009-08-28 16:13:07 UTC (rev 23259)
+++ pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/pid_position_velocity_controller.h 2009-08-28 16:31:54 UTC (rev 23260)
@@ -121,14 +121,14 @@
std::string getJointName();
me...
[truncated message content] |
