def odometry_callback(self, msg):
"""Handle updated measured velocity callback."""
if not bool(self.config):
return
linear = msg.twist.twist.linear
angular = msg.twist.twist.angular
v_linear = numpy.array([linear.x, linear.y, linear.z])
v_angular = numpy.array([angular.x, angular.y, angular.z])
if self.config['odom_vel_in_world']:
# This is a temp. workaround for gazebo's pos3d plugin not behaving properly:
# Twist should be provided wrt child_frame, gazebo provides it wrt world frame
# see http://docs.ros.org/api/nav_msgs/html/msg/Odometry.html
xyzw_array = lambda o: numpy.array([o.x, o.y, o.z, o.w])
q_wb = xyzw_array(msg.pose.pose.orientation)
R_bw = trans.quaternion_matrix(q_wb)[0:3, 0:3].transpose()
v_linear = R_bw.dot(v_linear)
v_angular = R_bw.dot(v_angular)
# Compute compute control output:
t = msg.header.stamp.to_sec()
e_v_linear = (self.v_linear_des - v_linear)
e_v_angular = (self.v_angular_des - v_angular)
a_linear = self.pid_linear.regulate(e_v_linear, t)
a_angular = self.pid_angular.regulate(e_v_angular, t)
# Convert and publish accel. command:
cmd_accel = geometry_msgs.Accel()
cmd_accel.linear = geometry_msgs.Vector3(*a_linear)
cmd_accel.angular = geometry_msgs.Vector3(*a_angular)
self.pub_cmd_accel.publish(cmd_accel)
def vectors2BodyInfo(translation, rotation, velocity, twist, linear_accel,
angular_accel):
header = std_msgs.Header()
header.stamp = rospy.Time.now()
pose = geo_msgs.Pose(Array2Point(translation), Array2Quaternion(rotation))
twist = geo_msgs.Twist(Array2Vector3(velocity), Array2Vector3(twist))
accel = geo_msgs.Accel(Array2Vector3(linear_accel),
Array2Vector3(angular_accel))
return uav_msgs.BodyInfo(header, pose, twist, accel)
def odometry_callback(self, msg):
"""Handle updated measured velocity callback."""
if not bool(self.config):
return
linear = msg.twist.twist.linear
angular = msg.twist.twist.angular
v_linear = numpy.array([linear.x, linear.y, linear.z])
v_angular = numpy.array([angular.x, angular.y, angular.z])
# Compute compute control output:
t = msg.header.stamp.to_sec()
e_v_linear = (self.v_linear_des - v_linear)
e_v_angular = (self.v_angular_des - v_angular)
a_linear = self.pid_linear.regulate(e_v_linear, t)
a_angular = self.pid_angular.regulate(e_v_angular, t)
# Convert and publish accel. command:
cmd_accel = geometry_msgs.Accel()
cmd_accel.linear = geometry_msgs.Vector3(*a_linear)
cmd_accel.angular = geometry_msgs.Vector3(*a_angular)
self.pub_cmd_accel.publish(cmd_accel)