def motion_data_callback(elapsed_time, position, setpoint, sensor):
if not self.have_sensor:
sensor = 0.0
header = std_msgs.msg.Header()
header.stamp = rospy.Time.now()
self.motion_pub.publish(
MotionData(header, elapsed_time, position, setpoint,
sensor))
def on_tracking_data_callback(self, msg):
with self.lock:
tracking_mode_enabled = self.tracking_mode_enabled
if self.tracking_mode_enabled:
if self.tracking_mode_is_first:
self.tracking_mode_is_first = False
self.tracking_mode_first_update_t = rospy.get_time()
self.tracking_mode_last_update_t = self.tracking_mode_first_update_t
self.tracking_mode_position_start = self.autostep.get_position(
)
self.tracking_mode_position = self.tracking_mode_position_start
predicted_position = self.tracking_mode_position
self.tracking_mode_velocity = 0.0
else:
current_time = rospy.get_time()
update_dt = current_time - self.tracking_mode_last_update_t
predicted_position = self.tracking_mode_position + update_dt * self.tracking_mode_velocity
if self.tracking_mode_absolute:
position_error = msg.position - predicted_position
else:
position_error = msg.position - (
predicted_position - self.tracking_mode_position_start)
new_velocity = self.tracking_mode_gain * position_error + msg.velocity
true_position = self.autostep.run_with_feedback(new_velocity)
self.tracking_mode_position = true_position
self.tracking_mode_velocity = new_velocity
self.tracking_mode_last_update_t = current_time
#rospy.logwarn(position_error)
header = std_msgs.msg.Header()
header.stamp = rospy.Time.now()
elapsed_time = self.tracking_mode_last_update_t - self.tracking_mode_first_update_t
self.motion_pub.publish(
MotionData(header, elapsed_time, self.tracking_mode_position,
predicted_position, 0.0))
def motion_data_callback(elapsed_time, position, setpoint):
header = std_msgs.msg.Header()
header.stamp = rospy.Time.now()
self.motion_pub.publish(
MotionData(header, elapsed_time, position, setpoint, 0.0))