def publish_Transform_Pose_Twist_AccelStamped(trans, ang, timestamp, header_frame_id='world'):
transform_msg = TransformStamped()
transform_msg.header.frame_id = header_frame_id
transform_msg.header.stamp = timestamp
transform_msg.child_frame_id = "x1"
transform_msg.transform.translation.x = trans.x
transform_msg.transform.translation.y = trans.y
transform_msg.transform.rotation = th_to_quat(ang.th)
tf_broadcaster.sendTransform(transform_msg)
pose_msg = PoseStamped()
pose_msg.header = transform_msg.header
pose_msg.pose.position.x = trans.x
pose_msg.pose.position.y = trans.y
pose_msg.pose.orientation = th_to_quat(ang.th)
x1_pose_pub.publish(pose_msg)
vel_msg = TwistStamped()
vel_msg.header = transform_msg.header
vel_msg.twist.linear.x = trans.xd
vel_msg.twist.linear.y = trans.yd
vel_msg.twist.angular.z = ang.thd
x1_vel_pub.publish(vel_msg)
accel_msg = AccelStamped()
accel_msg.header = transform_msg.header
accel_msg.accel.linear.x = trans.xdd
accel_msg.accel.linear.y = trans.ydd
x1_accel_pub.publish(accel_msg)
def _calc_accel(self):
"""
Calculate Acceleration Setpoint from Twist Setpoint PID
"""
if self._latest_vel_sp is None:
rospy.logwarn_throttle(
10.0, "{} | No vel setpoint.".format(rospy.get_name()))
return False
if self._latest_odom_fb is None:
rospy.logwarn_throttle(
10.0, "{} | No vel feedback.".format(rospy.get_name()))
return False
# Convert setpoint velocity into the correct frame
if self._latest_vel_sp.header.frame_id != self._latest_odom_fb.child_frame_id and len(
self._latest_vel_sp.header.frame_id) > 0:
if self._tf_buff.can_transform(self._latest_odom_fb.child_frame_id,
self._latest_vel_sp.header.frame_id,
rospy.Time.from_sec(0.0)):
cmd_vel = TwistStamped()
header = Header(self._latest_vel_sp.header.seq,
rospy.Time.from_sec(0.0),
self._latest_vel_sp.header.frame_id)
cmd_vel.twist.linear = self._tf_buff.transform(
Vector3Stamped(header, self._latest_vel_sp.twist.linear),
self._latest_odom_fb.child_frame_id,
rospy.Duration(5)).vector
cmd_vel.twist.angular = self._tf_buff.transform(
Vector3Stamped(header, self._latest_vel_sp.twist.angular),
self._latest_odom_fb.child_frame_id,
rospy.Duration(5)).vector
cmd_vel.header.stamp = rospy.Time.now()
else:
rospy.logwarn_throttle(
5, "{} | Cannot TF Velocity SP frame_id: {}".format(
rospy.get_name(), self._latest_vel_sp.header.frame_id))
return False
else:
cmd_vel = self._latest_vel_sp
clean_linear_vel = self._enforce_deadband(
self._latest_odom_fb.twist.twist.linear)
clean_angular_vel = self._enforce_deadband(
self._latest_odom_fb.twist.twist.angular)
vel_err = np.array([[cmd_vel.twist.linear.x - clean_linear_vel.x],
[cmd_vel.twist.linear.y - clean_linear_vel.y],
[cmd_vel.twist.linear.z - clean_linear_vel.z],
[
cmd_vel.twist.angular.z -
self._latest_odom_fb.twist.twist.angular.z
]],
dtype=float)
accel_sp_msg = AccelStamped()
accel_sp_msg.header = cmd_vel.header
accel_sp_msg.accel.linear.x, accel_sp_msg.accel.linear.y, accel_sp_msg.accel.linear.z, accel_sp_msg.accel.angular.z = self._vel_pids(
vel_err, self._latest_odom_fb.header.stamp.to_sec())
accel_sp_msg.accel.linear.y = 0 if not self._control_sway else accel_sp_msg.accel.linear.y
accel_sp_msg.accel.linear.z = 0 if not self._control_depth else accel_sp_msg.accel.linear.z
accel_sp_msg.accel.angular.z = 0 if not self._control_yaw else accel_sp_msg.accel.angular.z
self._latest_accel_sp = accel_sp_msg
return True
def publishAccel():
a = AccelStamped()
a.header.frame_id = "map"
a.header.stamp = rospy.Time.now()
a.accel.linear.x = AccX
a.accel.linear.y = AccY
a.accel.linear.z = AccZ
return a
def _calc_surface(self):
cmd_accel = AccelStamped(Header(0, rospy.Time.now(), ""), None)
if self._latest_odom_fb is None:
rospy.logwarn_throttle(
10.0,
"{} | SURFACING! No odom feedback.".format(rospy.get_name()))
cmd_accel.accel.linear.z = 10
else:
cmd_accel.accel.linear.z = 10 if self._latest_odom_fb.pose.pose.position.z < -0.5 else 0
self._latest_accel_sp = cmd_accel
return True
def publish_accel_stamped(index, iter):
accel_stamped = AccelStamped()
accel_stamped.header.frame_id = 'world'
accel_stamped.header.stamp = rospy.get_rostime()
accel_stamped.accel.linear.x = random.uniform(-2, 2)
accel_stamped.accel.linear.y = random.uniform(-2, 2)
accel_stamped.accel.linear.z = random.uniform(-2, 2)
accel_stamped.accel.angular.x = random.uniform(-2, 2)
accel_stamped.accel.angular.y = random.uniform(-2, 2)
accel_stamped.accel.angular.z = random.uniform(-2, 2)
geometry_publisher[index].publish(accel_stamped)
def run(self, frequency=0):
while not rospy.is_shutdown():
x, y, z = self.accel.read()
msg = AccelStamped()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = "accel_frame"
msg.accel.linear.x = x
msg.accel.linear.y = y
msg.accel.angular.z = z
msg.header.seq = self.seq
self.seq = self.seq + 1
self.acc_pub.publish(msg)
def _publish_accel(self, timestamp):
'''
See base class comment
'''
# One of the kalman filters have not been initialized yet, don't publish
if not self._imu_kf or not self._chassis_kf:
return
accel_msg = AccelStamped()
accel_msg.header.frame_id = "odom"
accel_msg.header.stamp = timestamp
(accel_msg.accel.linear.x, accel_msg.accel.linear.y,
accel_msg.accel.linear.z) = self._chassis_kf.statePost[8:11, 0]
(accel_msg.accel.angular.x, accel_msg.accel.angular.y,
accel_msg.accel.angular.z) = self._imu_kf.statePost[6:9, 0]
self._accel_pub.publish(accel_msg)
def eraseGravity(self):
q1 = [self.Quat.x, self.Quat.y, self.Quat.z, self.Quat.w]
q1 = list(q1)
acc = [self.Acc.x, self.Acc.y, self.Acc.z]
acc = numpy.array(acc, dtype=numpy.float64, copy=True)
acc_scalar = sqrt(numpy.dot(acc, acc))
unit_acc = acc / acc_scalar
q2 = list(unit_acc)
q2.append(0.0)
acc_no_grav = tf.transformations.quaternion_multiply(
tf.transformations.quaternion_multiply(q1, q2),
tf.transformations.quaternion_conjugate(q1))[:3]
acc_no_grav *= acc_scalar
acc_no_grav -= [0.0, 0.0, 9.8]
PubAcc = Vector3(acc_no_grav[0], acc_no_grav[1], acc_no_grav[2])
acc_msg = AccelStamped()
acc_msg.header.stamp = rospy.Time.now()
acc_msg.accel.linear = PubAcc
self.accPub.publish(acc_msg)
def pose_twist_accelStamped_pub(self, state, pose_pub, vel_pub, accel_pub, xythv_pub, header_frame_id, timestamp):
self.tf_broadcaster.sendTransform((state.position.x, state.position.y, 0),
state.orientation.to_list(),
timestamp,
"tracked_object",
header_frame_id)
pose_msg = PoseStamped()
pose_msg.header.frame_id = header_frame_id
pose_msg.header.stamp = timestamp
pose_msg.pose.position.x = state.position.x
pose_msg.pose.position.y = state.position.y
pose_msg.pose.orientation = Quaternion(*state.orientation.to_list())
pose_pub.publish(pose_msg)
vel_msg = TwistStamped()
vel_msg.header.frame_id = header_frame_id
vel_msg.header.stamp = timestamp
vel_msg.twist.linear.x = state.position.xd
vel_msg.twist.linear.y = state.position.yd
vel_msg.twist.angular.z = state.orientation.dtheta
vel_pub.publish(vel_msg)
accel_msg = AccelStamped()
accel_msg.header.frame_id = header_frame_id
accel_msg.header.stamp = timestamp
accel_msg.accel.linear.x = state.position.xdd
accel_msg.accel.linear.y = state.position.ydd
accel_pub.publish(accel_msg)
xythv_msg = XYThV()
xythv_msg.x = state.position.x
xythv_msg.y = state.position.y
xythv_msg.th = np.arctan2(state.position.yd, state.position.xd)
xythv_msg.v = np.hypot(state.position.xd, state.position.yd)
xythv_pub.publish(xythv_msg)
msg = PoseStamped()
msg.header.seq = 0
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = worldFrame
msg.pose.position.x = x
msg.pose.position.y = y
msg.pose.position.z = z
quaternion = tf.transformations.quaternion_from_euler(0, 0, 0)
msg.pose.orientation.x = quaternion[0]
msg.pose.orientation.y = quaternion[1]
msg.pose.orientation.z = quaternion[2]
msg.pose.orientation.w = quaternion[3]
pub = rospy.Publisher(name, PoseStamped, queue_size=1)
acc = AccelStamped()
acc.header.seq = 0
acc.header.frame_id = worldFrame
pubAccel = rospy.Publisher("feedforwardAcceleration", AccelStamped, queue_size=1)
vel = TwistStamped()
vel.header.seq = 0
vel.header.frame_id = worldFrame
pubVel = rospy.Publisher("feedforwardVelocity", TwistStamped, queue_size=1)
while not rospy.is_shutdown():
msg.header.seq += 1
msg.header.stamp = rospy.Time.now()
pub.publish(msg)
def publisher():
global drone_pose, flag_rec_info, drone_yaw
act_pose = PoseStamped()
act_vel = TwistStamped()
act_accel = AccelStamped()
drone_yaw = 0
flag_rec_info = 0
# subscribers
if gazebo_info == 1:
# subscribers gazebo
print ("Listening to Gazebo \n")
rospy.Subscriber("/drone_gazebo" + str(num_drone) + "/pose", PoseStamped, gz_callback)
else:
print ("Listening to Optitrack \n")
# subscribers optitrack
# cambiar nombre de topics
rospy.Subscriber('/drone_optitrack' + str(num_drone) + '/pose', PoseStamped, ot_callback)
# Vel minimos cuadrados
j = 0
n = 30
t = np.zeros(n)
x = np.zeros(n)
y = np.zeros(n)
z = np.zeros(n)
yaw = np.zeros(n)
# Accel minimos cuadrados
vt = np.zeros(n)
vx = np.zeros(n)
vy = np.zeros(n)
vz = np.zeros(n)
rate = rospy.Rate(60) # 60hz
while not rospy.is_shutdown():
if flag_rec_info == 1:
mutex.acquire()
act_pose.pose.position.x = drone_pose.pose.position.x
act_pose.pose.position.y = drone_pose.pose.position.y
act_pose.pose.position.z = drone_pose.pose.position.z
act_pose.header.stamp = drone_pose.header.stamp
act_pose.pose.orientation.z = drone_yaw
mutex.release()
# estimacion vel por minimos cuadrados
for i in range(0, n - 1):
x[i] = x[i + 1]
y[i] = y[i + 1]
z[i] = z[i + 1]
t[i] = t[i + 1]
yaw[i] = yaw[i + 1]
x[n - 1] = float(act_pose.pose.position.x)
y[n - 1] = float(act_pose.pose.position.y)
z[n - 1] = float(act_pose.pose.position.z)
yaw[n - 1] = float(act_pose.pose.orientation.z)
t[n - 1] = float(act_pose.header.stamp.to_sec())
# igual es prescindible
# j contador para no empezar ajuste por minimos cuadrados hasta que no esten n primeras medidas copiadas
j = j + 1
if j >= n:
j = n
# print("New measure")
# print("t = ["+str(t[0])+", "+str(t[1])+", "+str(t[2])+", "+str(t[3])+", "+str(t[4])+", "+str(t[5])+", "+str(t[6])+", "+str(t[7])+", "+str(t[8])+", "+str(t[9])+", "+str(t[10])+", "+str(t[11])+", "+str(t[12])+", "+str(t[13])+", "+str(t[14])+", "+str(t[15])+", "+str(t[16])+", "+str(t[17])+", "+str(t[18])+", "+str(t[19])+"]")
# print("x = [" + str(x[0]) + ", " + str(x[1]) + ", " + str(x[2]) + ", " + str(x[3]) + ", " + str(x[4]) + ", " + str(x[5]) + ", " + str(x[6]) + ", " + str(x[7]) + ", " + str(x[8]) + ", " + str(x[9]) + ", " + str(x[10]) + ", " + str(x[11]) + ", " + str(x[12]) + ", " + str(x[13]) + ", " + str(x[14]) + ", " + str(x[15]) + ", " + str(x[16]) + ", " + str(x[17]) + ", " + str(x[18]) + ", " + str(x[19]) + "]")
# Coefs pol posicion
cx = np.polyfit(t, x, 2)
cy = np.polyfit(t, y, 2)
cz = np.polyfit(t, z, 2)
cyaw = np.polyfit(t, yaw, 2)
# pols
polvx = np.polyder(np.poly1d(cx))
polvy = np.polyder(np.poly1d(cy))
polvz = np.polyder(np.poly1d(cz))
polvyaw = np.polyder(np.poly1d(cyaw))
act_vel.twist.linear.x = polvx(t[n-1])
act_vel.twist.linear.y = polvy(t[n-1])
act_vel.twist.linear.z = polvz(t[n-1])
act_vel.twist.angular.z = polvyaw(t[n-1])
act_vel.header.stamp = act_pose.header.stamp
polax = np.polyder(polvx)
polay = np.polyder(polvy)
polaz = np.polyder(polvz)
act_accel.accel.linear.x = polax(t[n-1])
act_accel.accel.linear.y = polay(t[n-1])
act_accel.accel.linear.z = polaz(t[n-1])
act_accel.header.stamp = act_pose.header.stamp
# print ("vx: " + str(act_vel.twist.linear.x) + " vy: " + str(act_vel.twist.linear.y) + "vz: " + str(act_vel.twist.linear.z))
pub_pose.publish(act_pose)
pub_twist.publish(act_vel)
pub_accel.publish(act_accel)
print('New measure')
print ('time = ' + str(act_pose.header.stamp.to_sec()) + 'secs')
print ('x = ' + str(act_pose.pose.position.x)+', y = ' + str(act_pose.pose.position.y)+', z = '+ str(act_pose.pose.position.z))
print ('yaw = ' + str(act_pose.pose.orientation.z))
print ('vx = ' + str(act_vel.twist.linear.x) + ', vy = ' + str(act_vel.twist.linear.y) + ', vz = ' + str(act_vel.twist.linear.z))
print ('vyaw = ' + str(act_vel.twist.angular.z))
print ('ax = ' + str(act_accel.accel.linear.x) + ', ay = ' + str(act_accel.accel.linear.y) + ', az = ' + str(act_accel.accel.linear.z))
if x[n-1] > 0.1 and x[n-1] < 0.4:
np.savetxt('plot_mmc_x1.txt', x)
np.savetxt('plot_mmc_yaw1.txt', yaw)
np.savetxt('plot_mmc_t1.txt', t)
np.savetxt('plot_mmc_vx1.txt', vx)
np.savetxt('plot_mmc_vt1.txt', vt)
plot_calculo_vels1 = [act_vel.twist.linear.x, act_vel.twist.angular.z]
np.savetxt('plot_calculo_vels1.txt', plot_calculo_vels1)
if x[n-1] > 0.5 and x[n-1] < 0.7:
np.savetxt('plot_mmc_x2.txt', x)
np.savetxt('plot_mmc_yaw2.txt', yaw)
np.savetxt('plot_mmc_t2.txt', t)
np.savetxt('plot_mmc_vx2.txt', vx)
np.savetxt('plot_mmc_vt2.txt', vt)
plot_calculo_vels2 = [act_vel.twist.linear.x, act_vel.twist.angular.z]
np.savetxt('plot_calculo_vels2.txt', plot_calculo_vels2)
if x[n-1] > 0.8 and x[n-1] < 1:
np.savetxt('plot_mmc_x3.txt', x)
np.savetxt('plot_mmc_yaw3.txt', yaw)
np.savetxt('plot_mmc_t3.txt', t)
np.savetxt('plot_mmc_vx3.txt', vx)
np.savetxt('plot_mmc_vt3.txt', vt)
plot_calculo_vels3 = [act_vel.twist.linear.x, act_vel.twist.angular.z]
np.savetxt('plot_calculo_vels3.txt', plot_calculo_vels3)
rate.sleep()
def run(self):
self.listener.waitForTransform(self.worldFrame, self.frame,
rospy.Time(), rospy.Duration(5.0))
goal = PoseStamped()
goal.header.seq = 0
goal.header.frame_id = self.worldFrame
circleFlag = 0
acc = AccelStamped()
acc.header.seq = 0
acc.header.frame_id = self.worldFrame
vel = TwistStamped()
vel.header.seq = 0
vel.header.frame_id = self.worldFrame
doCircle = Int8()
doCircle.data = 0
# rate = rospy.Rate(200) # control the publish rate?
while not rospy.is_shutdown():
# hovering at the first given position
goal.header.seq += 1
goal.header.stamp = rospy.Time.now()
goal.pose.position.x = self.goals[self.goalIndex][0]
goal.pose.position.y = self.goals[self.goalIndex][1]
goal.pose.position.z = self.goals[self.goalIndex][2]
quaternion = tf.transformations.quaternion_from_euler(
0, 0, self.goals[self.goalIndex][3])
goal.pose.orientation.x = quaternion[0]
goal.pose.orientation.y = quaternion[1]
goal.pose.orientation.z = quaternion[2]
goal.pose.orientation.w = quaternion[3]
acc.header.seq += 1
acc.header.stamp = rospy.Time.now()
acc.accel.linear.z = 0
vel.header.seq += 1
vel.header.stamp = rospy.Time.now()
vel.twist.linear.x = 0
vel.twist.linear.y = 0
self.pubGoal.publish(goal)
self.pubAccel.publish(acc)
self.pubVeloc.publish(vel)
self.pubCircle.publish(doCircle)
# rate.sleep()
t = self.listener.getLatestCommonTime(self.worldFrame, self.frame)
if self.listener.canTransform(self.worldFrame, self.frame, t):
position, quaternion = self.listener.lookupTransform(
self.worldFrame, self.frame, t)
rpy = tf.transformations.euler_from_quaternion(quaternion)
# if math.fabs(position[0] - self.goals[self.goalIndex][0]) < 0.1 \
# and math.fabs(position[1] - self.goals[self.goalIndex][1]) < 0.1 \
# and math.fabs(position[2] - self.goals[self.goalIndex][2]) < 0.1 \
# and math.fabs(rpy[2] - self.goals[self.goalIndex][3]) < math.radians(5):
if math.fabs(position[0] - self.goals[self.goalIndex][0]) < 0.1 \
and math.fabs(position[1] - self.goals[self.goalIndex][1]) < 0.1 \
and math.fabs(position[2] - self.goals[self.goalIndex][2]) < 0.1 \
and math.fabs(rpy[2] - 0) < math.radians(5):
rospy.sleep(5)
circleFlag += 1
doCircle.data = 1
t0 = rospy.get_time()
while circleFlag == 1:
goal.header.seq += 1
goal.header.stamp = rospy.Time.now()
t = rospy.get_time() - t0
# n = 0.5
# if t > 10 and t <= 20:
# n = 1
# else:
# n = 2
# change the rate
if t > 32:
circleFlag = 2
docircle = 0
# continuously generate the setpoint
goal.pose.position.x = 0.5 * math.cos(math.radians(45 * 2 * t))
goal.pose.position.y = 0.5 * math.sin(math.radians(
45 * 2 * t)) - 0.5
goal.pose.position.z = self.goals[self.goalIndex][2]
x_dt = -0.5 * math.radians(45 * 2) * math.sin(
math.radians(45 * 2 * t))
y_dt = 0.5 * math.radians(45 * 2) * math.cos(
math.radians(45 * 2 * t))
#
x_ddt = -0.5 * math.radians(45 * 2) * math.radians(
45 * 2) * math.cos(math.radians(45 * 2 * t))
y_ddt = -0.5 * math.radians(45 * 2) * math.radians(
45 * 2) * math.sin(math.radians(45 * 2 * t))
# publish x_acc and y_acc
acc.header.seq += 1
acc.header.stamp = rospy.Time.now()
# add scaling factor
acc.accel.linear.x = x_ddt * 180 / math.pi / 9.8
acc.accel.linear.y = y_ddt * 180 / math.pi / 9.8
acc.accel.linear.z = 1
z_ddt = 0
# publish x_vel and y_vel
vel.header.seq += 1
vel.header.stamp = rospy.Time.now()
vel.twist.linear.x = x_dt
vel.twist.linear.y = y_dt
# m = 0.023
########################differential flatness##########################
# g = 9.8
# fzb = np.array([[x_ddt,y_ddt,z_ddt+g]])
# zb = fzb/np.linalg.norm(fzb)
# zbx = zb[0][0]
# zby = zb[0][1]
# zbz = zb[0][2]
# #roll
# roll = math.atan2(zbz,zby)-math.pi/2
# #pitch
# pitch = math.atan2(zbx,zbz)
#######################################################################
quaternion = tf.transformations.quaternion_from_euler(0, 0, 0)
# acc.accel.angular.x = roll
# acc.accel.angular.y = pitch
goal.pose.orientation.x = quaternion[0]
goal.pose.orientation.y = quaternion[1]
goal.pose.orientation.z = quaternion[2]
goal.pose.orientation.w = quaternion[3]
self.pubGoal.publish(goal)
self.pubAccel.publish(acc)
self.pubVeloc.publish(vel)
self.pubCircle.publish(doCircle)
def compute_accel(current_vel, topic_name):
current_accel = AccelStamped()
current_accel.header.stamp.secs = current_vel.header.stamp.secs
current_accel.header.stamp.nsecs = current_vel.header.stamp.nsecs
ave_accel = AccelStamped()
ave_accel.header.stamp.secs = current_vel.header.stamp.secs
ave_accel.header.stamp.nsecs = current_vel.header.stamp.nsecs
# Only compute once we have two samples to compute acceleration.
if 'ave_vel' in past[topic_name]:
past_vel = past[topic_name]['ave_vel']
# Calculate times, measured in seconds
current_t = float(current_vel.header.stamp.secs +
1e-9 * current_vel.header.stamp.nsecs)
past_t = float(past_vel.header.stamp.secs +
1e-9 * past_vel.header.stamp.nsecs)
delta_t = current_t - past_t
#Calculate current acceleration
current_accel.accel.linear.x = (current_vel.twist.linear.x -
past_vel.twist.linear.x) / delta_t
current_accel.accel.linear.y = (current_vel.twist.linear.y -
past_vel.twist.linear.y) / delta_t
current_accel.accel.linear.z = (current_vel.twist.linear.z -
past_vel.twist.linear.z) / delta_t
current_accel.accel.angular.x = (current_vel.twist.angular.x -
past_vel.twist.angular.x) / delta_t
current_accel.accel.angular.y = (current_vel.twist.angular.y -
past_vel.twist.angular.y) / delta_t
current_accel.accel.angular.z = (current_vel.twist.angular.z -
past_vel.twist.angular.z) / delta_t
# Update acceleration buffer
if (len(past[topic_name]['accel']) < RUNNING_AVE_SAMPLES):
past[topic_name]['accel'].append(current_accel)
else:
del past[topic_name]['accel'][0]
past[topic_name]['accel'].append(current_accel)
# Compute average
sum = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
for i in range(0, len(past[topic_name]['accel'])):
sample = past[topic_name]['accel'][i]
sum[0] = sum[0] + sample.accel.linear.x
sum[1] = sum[1] + sample.accel.linear.y
sum[2] = sum[2] + sample.accel.linear.z
sum[3] = sum[3] + sample.accel.angular.x
sum[4] = sum[4] + sample.accel.angular.y
sum[5] = sum[5] + sample.accel.angular.z
average_accel = sum / RUNNING_AVE_SAMPLES
ave_accel.accel.linear.x = average_accel[0]
ave_accel.accel.linear.y = average_accel[1]
ave_accel.accel.linear.z = average_accel[2]
ave_accel.accel.angular.x = average_accel[3]
ave_accel.accel.angular.y = average_accel[4]
ave_accel.accel.angular.z = average_accel[5]
else:
past[topic_name]['accel'] = []
past[topic_name]['ave_vel'] = current_vel
return ave_accel
def _calc_wrench(self):
"""
Calculate Wrench Setpoint from Accel Message
Uses the odometry child_frame_id as the body-fixed frame (ASSUMED FLU)
Will transform the requested setpoint into the body-fixed frame.
"""
if self._latest_accel_sp is None:
rospy.logwarn_throttle(
10.0, "{} | No accel setpoint.".format(rospy.get_name()))
return
if self._latest_accel_fb is None:
rospy.logwarn_throttle(
10.0, "{} | No accel feedback.".format(rospy.get_name()))
return
# Convert setpoint acceleration into the correct frame
if self._latest_accel_sp.header.frame_id != self._latest_accel_fb.header.frame_id and len(
self._latest_accel_sp.header.frame_id) > 0:
if self._tf_buff.can_transform(
self._latest_accel_fb.header.frame_id,
self._latest_accel_sp.header.frame_id,
rospy.Time.from_sec(0.0), rospy.Duration(5)):
header = Header(self._latest_accel_sp.header.seq,
rospy.Time.from_sec(0.0),
self._latest_accel_sp.header.frame_id)
cmd_accel = AccelStamped()
cmd_accel.accel.linear = self._tf_buff.transform(
Vector3Stamped(header, self._latest_accel_sp.accel.linear),
self._latest_accel_fb.header.frame_id,
rospy.Duration(5)).vector
cmd_accel.accel.angular = self._tf_buff.transform(
Vector3Stamped(header,
self._latest_accel_sp.accel.angular),
self._latest_accel_fb.header.frame_id,
rospy.Duration(5)).vector
else:
rospy.logwarn_throttle(
5, "{} | Cannot TF Acceleration SP frame_id: {}".format(
rospy.get_name(),
self._latest_accel_sp.header.frame_id))
return False
else:
cmd_accel = self._latest_accel_sp # If it's already in the correct frame then never mind.
if self._use_accel_fb:
accel_err = np.array(
[[
cmd_accel.accel.linear.x -
self._latest_accel_fb.accel.accel.linear.x
],
[
cmd_accel.accel.linear.y -
self._latest_accel_fb.accel.accel.linear.y
],
[
cmd_accel.accel.linear.z -
self._latest_accel_fb.accel.accel.linear.z
],
[
cmd_accel.accel.angular.z -
self._latest_accel_fb.accel.accel.angular.z
]],
dtype=float)
tau = self._accel_pids(accel_err,
self._latest_accel_fb.header.stamp.to_sec())
else:
accel = np.array([[cmd_accel.accel.linear.x],
[cmd_accel.accel.linear.y],
[cmd_accel.accel.linear.z],
[cmd_accel.accel.angular.x],
[cmd_accel.accel.angular.y],
[cmd_accel.accel.angular.z]])
tau = self.mass_inertial_matrix.dot(accel)[[0, 1, 2, -1], :]
self._wrench.header.stamp = rospy.Time.now()
self._wrench.header.frame_id = cmd_accel.header.frame_id
self._wrench.wrench.force.x, self._wrench.wrench.force.y, self._wrench.wrench.force.z, self._wrench.wrench.torque.z = tau.squeeze(
)
self._wrench_pub.publish(self._wrench)