def handle_convert(data):
pub = rospy.Publisher('/navigation_vel', TwistStamped)
m = TwistStamped()
m.twist = data
m.header = Header()
m.header.stamp = rospy.Time.now()
pub.publish(m)
def set_position():
xc, yc = pyautogui.position()
pos = TwistStamped()
pos.header = Header()
#print("xc is " + str(xc))
if (xc < 680 and yc < 380):
pos.twist.linear.z = 0.0
pos.twist.linear.x = 1
pos.twist.linear.y = 1
print("forward_left")
elif (xc < 680 and yc > 380):
pos.twist.linear.z = 0.0
pos.twist.linear.x = 1
pos.twist.linear.y = -1
print("forward_right")
elif (xc > 680 and yc < 380):
pos.twist.linear.z = 0.0
pos.twist.linear.x = -1
pos.twist.linear.y = -1
print("backward_left")
elif (xc > 680 and yc > 380):
pos.twist.linear.z = 0.0
pos.twist.linear.x = -1
pos.twist.linear.y = 1
print("backward_right")
else:
pos.twist.linear.z = 0
pos.twist.linear.x = 0
pos.twist.linear.y = 0
print("entered default")
return pos
def pose_callback(data):
# extract x and y position information
x = data.pose.pose.position.x
y = data.pose.pose.position.y
# package and broadcast
p = Point()
p.x = x
p.y = y
# send it
pub_beacon.publish(p)
# and broadbast all odoms on a common channel
pub_odom_all.publish(data)
# publish pose info for viewing
pose_out = PoseStamped()
pose_out.header = data.header
pose_out.header.frame_id = '/world'
pose_out.pose = data.pose.pose
pub_pose.publish(pose_out)
# and twist stamped for DMS interface
vel_out = TwistStamped()
vel_out.header = data.header
vel_out.header.frame_id = '/world'
vel_out.twist = data.twist.twist
pub_vel.publish(vel_out)
# transform
tf_broadcast.sendTransform(
(x, y, 0.0),
(data.pose.pose.orientation.x, data.pose.pose.orientation.y,
data.pose.pose.orientation.z, data.pose.pose.orientation.w),
rospy.Time.now(), frame_name, "/world")
def navigate(self):
rate = self.rospy.Rate(10) # 10hz
msg = TwistStamped()
msg.header = Header()
msg.header.frame_id = "base_footprint"
msg.header.stamp = rospy.Time.now()
while 1:
print "Time since last vel:", (time.time() - self.lastVelTime)
if (self.inCollision or (time.time() - self.lastVelTime) > 1.0):
# print "inCollision"
msg.twist.linear.x = 0 #self.x
msg.twist.linear.y = 0 #self.y
msg.twist.linear.z = 0 #self.z
msg.twist.angular.z = 0
else:
msg.twist.linear.x = self.x
msg.twist.linear.y = self.y
msg.twist.linear.z = self.z
msg.twist.angular.z = self.yaw
print "Sending vel"
#print "collision:", self.inCollision
# For demo purposes we will lock yaw/heading to north.
#yaw_degrees = 0 # North
#yaw = radians(yaw_degrees)
#quaternion = quaternion_from_euler(0, 0, yaw)
#msg.pose.orientation = Quaternion(*quaternion)
self.pub.publish(msg)
rate.sleep()
def velocity_loop(self):
"""Loops in Control_Mode.VELOCITY until either the mode is switched or the set duration has been met."""
# Record start time for timeout tracking
start_time = rospy.get_time()
# Build message
msg = TwistStamped()
msg.twist.linear.x = self._target_velocity[0]
msg.twist.linear.y = self._target_velocity[1]
msg.twist.linear.z = self._target_velocity[2]
while self.is_running() and self._mode == Control_Mode.VELOCITY:
# Break if timeout is enabled and has been surpassed
if self._travel_time > 0 and (rospy.get_time() -
start_time) > self._travel_time:
break
# Update message header
msg.header = Header(stamp=rospy.get_rostime())
# Publish message
self._vel_pub.publish(msg)
# Sleep to maintain appropriate update frequency
self._rate.sleep()
# Only perform if controller is still running
if self.is_running():
# Set mode to HOLD
self.set_mode(Control_Mode.INITIAL)
# Reset velocity target
self.set_velocity((0.0, 0.0, 0.0))
def new_measurement(self, data):
'''Processes incoming measurement from Localization.
data:
meas_world: PoseStamped
yaw: float32
'''
self.pc.pose_raw = data.meas_world
self.pc.yaw = data.yaw
self.measurement_valid = self.pc.measurement_check()
measurement = self.kalman.transform_pose(self.pc.pose_raw, "world",
"world_rot")
if self.measurement_valid:
if self.kalman.init:
self.wm.yhat_r_t0.header = measurement.header
zero_input_cmd = TwistStamped()
zero_input_cmd.header = measurement.header
self.kalman.input_cmd_list = [zero_input_cmd]
self.kalman.init = False
# Apply correction step.
self.wm.yhat_r, self.wm.yhat_r_t0 = self.kalman.kalman_pos_correct(
measurement, self.wm.yhat_r_t0)
self.wm.yhat = self.transform_point(self.wm.yhat_r, "world_rot",
"world")
self.pose_pub.publish(self.wm.yhat)
def onTwist(self,msg): newMsg = TwistStamped() head = std_msgs.msg.Header() newMsg.twist = msg head.stamp = rospy.Time.now() newMsg.header = head self.twist_pub.publish(newMsg)
def veloCallback(self,velo):
self.velocity[0] = velo.twist.linear.x
self.velocity[1] = velo.twist.linear.y
self.velocity[2] = velo.twist.linear.z
randx = random.uniform(-1.,1.) * 0.1*0
randy = random.uniform(-1.,1.) * 0.1*0
randz = random.uniform(-1.,1.) * 0.1*0
randdistance = np.linalg.norm(np.array([randx, randy, randz]))
#print("RandomErrors Velo:", randx, randy, randz, randdistance)
self.velocity_rot = self.q_rot.rotate(np.array([velo.twist.linear.x+randx, velo.twist.linear.y+randy, velo.twist.linear.z+randz]))
#print("Velocity", self.velocity)
self.veloMedianX = np.roll(self.veloMedianX, 1)
self.veloMedianY = np.roll(self.veloMedianY, 1)
self.veloMedianZ = np.roll(self.veloMedianZ, 1)
self.veloMedianX[0] = self.velocity_rot[0]
self.veloMedianY[1] = self.velocity_rot[1]
self.veloMedianZ[2] = self.velocity_rot[2]
self.filteredVeloValue[0] = (np.sum(self.veloMedianX) / len(self.veloMedianX))
self.filteredVeloValue[1] = (np.sum(self.veloMedianY) / len(self.veloMedianY))
self.filteredVeloValue[2] = (np.sum(self.veloMedianZ) / len(self.veloMedianZ))
veloNED =TwistStamped()
veloNED.header = velo.header #TIMESTAMP?
veloNED.header.frame_id = 'global_tank'
veloNED.twist.linear.x = self.filteredVeloValue[0]
veloNED.twist.linear.y = self.filteredVeloValue[1]
veloNED.twist.linear.z = self.filteredVeloValue[2]
self.velocity_publish.publish(veloNED)
def navigate(self):
rate = self.rospy.Rate(10) # 10hz
msg = TwistStamped()
msg.header = Header()
msg.header.frame_id = "base_footprint"
msg.header.stamp = rospy.Time.now()
while 1:
print "Time since last vel:", (time.time() - self.lastVelTime)
if (self.inCollision or (time.time() - self.lastVelTime)>1.0):
# print "inCollision"
msg.twist.linear.x = 0 #self.x
msg.twist.linear.y = 0 #self.y
msg.twist.linear.z = 0 #self.z
msg.twist.angular.z = 0
else:
msg.twist.linear.x = self.x
msg.twist.linear.y = self.y
msg.twist.linear.z = self.z
msg.twist.angular.z = self.yaw
print "Sending vel"
#print "collision:", self.inCollision
# For demo purposes we will lock yaw/heading to north.
#yaw_degrees = 0 # North
#yaw = radians(yaw_degrees)
#quaternion = quaternion_from_euler(0, 0, yaw)
#msg.pose.orientation = Quaternion(*quaternion)
self.pub.publish(msg)
rate.sleep()
def pose_callback(data):
# extract x and y position information
x = data.pose.pose.position.x
y = data.pose.pose.position.y
# package and broadcast
p = Point()
p.x = x
p.y = y
# send it
pub_beacon.publish(p)
# and broadbast all odoms on a common channel
pub_odom_all.publish(data)
# publish pose info for viewing
pose_out = PoseStamped()
pose_out.header = data.header
pose_out.header.frame_id = '/world'
pose_out.pose = data.pose.pose
pub_pose.publish(pose_out)
# and twist stamped for DMS interface
vel_out = TwistStamped()
vel_out.header = data.header
vel_out.header.frame_id = '/world'
vel_out.twist = data.twist.twist
pub_vel.publish(vel_out)
# transform
tf_broadcast.sendTransform((x, y, 0.0),
(data.pose.pose.orientation.x, data.pose.pose.orientation.y, data.pose.pose.orientation.z, data.pose.pose.orientation.w),
rospy.Time.now(),frame_name,"/world")
def update(self, state):
if (self.target is None):
rospy.logwarn("Target position for velocity controller is none.")
return None
# simplify variables a bit
time = state.header.stamp.to_sec()
position = state.pose.position
orientation = state.pose.orientation
# create output structure
output = TwistStamped()
output.header = state.header
# output velocities
linear = Vector3()
angular = Vector3()
# Control in X vel
linear.x = self.X.update(self.target.position.x, position.x, time)
# Control in Y vel
linear.y = self.Y.update(self.target.position.y, position.y, time)
# Control in Z vel
linear.z = self.Z.update(self.target.position.z, position.z, time)
# Control yaw (no x, y angular)
# TODO
output.twist = Twist()
output.twist.linear = linear
return output
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 callBackRightEncoderCount(data):
# -----------------------------------------------------------------------------
global robot
robot.rightWheel.encoderCount = data.data
t = data.header.stamp.secs + 1.E-9 * data.header.stamp.nsecs
deltaT = t - robot.rightWheel.lastTimeCountChange
# rospy.loginfo(rospy.get_name() + " - time: %f" % t)
# angular speed computation
if (deltaT > 0.0):
# first order filter
omegaOld = robot.rightWheel.omega
omega = (robot.rightWheel.encoderCount -
robot.rightWheel.encoderCountPrev) * (
2.0 * np.pi / robot.rightWheel.encoderResolution) / deltaT
alpha = 0.3
robot.rightWheel.omega = alpha * omega + (1.0 - alpha) * omegaOld
# updates for next iteration
robot.rightWheel.lastTimeCountChange = t
robot.rightWheel.encoderCountPrev = robot.rightWheel.encoderCount
# message to be published
msgTwist = TwistStamped()
msgTwist.header = data.header
msgTwist.twist.angular.z = robot.rightWheel.omega
# publication
pubRightWheelSpeed.publish(msgTwist)
def set_position():
port = '/dev/ttyACM0'
baud = 115200
ser = serial.Serial(port, baud)
temp = (ser.readline())
a = ser.readline().rstrip()
b = ser.readline().rstrip()
c = ser.readline().rstrip()
d = ser.readline().rstrip()
e = ser.readline().rstrip()
global hold
global holdkm1
global hold_refr
hold = 0
pos = TwistStamped()
pos.twist = Twist()
pos.header = Header()
if int(a) > 1000:
pos.twist.linear.z = 1
print a
elif int(b) > 1000:
pos.twist.linear.x = 0.8
print b
elif int(c) > 1000:
pos.twist.linear.x = -0.8
print c
elif int(d) > 1000:
pos.twist.linear.y = 0.8
print d
elif int(e) > 1000:
pos.twist.linear.y = -.8
print e
return pos
def callBackLeftEncoderCount(data):
# -----------------------------------------------------------------------------
global robot
robot.leftWheel.encoderCount = data.data
t = data.header.stamp.secs + 1.E-9 * data.header.stamp.nsecs
deltaT = t - robot.leftWheel.lastTimeCountChange
# angular speed computation
if (deltaT > 0.0):
# first order filter
omegaOld = robot.leftWheel.omega
omega = (robot.leftWheel.encoderCount -
robot.leftWheel.encoderCountPrev) * (
2.0 * np.pi / robot.leftWheel.encoderResolution) / deltaT
robot.leftWheel.omega = alpha * omega + (1.0 - alpha) * omegaOld
# updates for next iteration
robot.leftWheel.lastTimeCountChange = t
robot.leftWheel.encoderCountPrev = robot.leftWheel.encoderCount
# message to be published
msgTwist = TwistStamped()
msgTwist.header = data.header
msgTwist.twist.angular.z = robot.leftWheel.omega
msgTwist.twist.linear.x = robot.leftWheel.omega * robot.leftWheel.diameter / 2.0 # TO DO: sign to be checked
# publication
pubLeftWheelSpeed.publish(msgTwist)
def publish():
header = Header()
header.stamp = rospy.Time.now()
header.frame_id = 'map'
for role, robot_id in WorldModel.assignments.items():
if robot_id is not None:
command = WorldModel.commands[role]
if command.velocity_control_enable:
msg = TwistStamped()
msg.header = header
msg.twist = command.target_velocity
pubs_velocity[robot_id].publish(msg)
else:
send_pose = WorldModel.tf_listener.transformPose(
"map", command.target_pose)
send_pose.header.stamp = rospy.Time.now()
pubs_position[robot_id].publish(send_pose)
pubs_kick_velocity[robot_id].publish(Float32(command.kick_power))
status = AIStatus()
status.avoidBall = command.avoid_ball
status.avoidDefenceArea = command.avoid_defence_area
status.do_chip = command.chip_enable
status.dribble_power = command.dribble_power
pubs_ai_status[robot_id].publish(status)
command.reset_adjustments()
def twistUpdate(self):
if self.curr_state:
x_des, v_des, R_des, w_des = self.getGoalState(self.curr_state)
e_x, e_v, e_R, e_w = self.getErrors(self.curr_state)
twist_cmd = TwistStamped()
twist_cmd.header = Header()
twist_cmd.header.stamp = rospy.Time.now()
twist_cmd.twist.linear.x = -self.k_x * e_x[0] - self.k_v * e_v[
0] + v_des[0]
twist_cmd.twist.linear.y = -self.k_x * e_x[1] - self.k_v * e_v[
1] + v_des[1]
twist_cmd.twist.linear.z = -self.k_x * e_x[2] - self.k_v * e_v[
2] + v_des[2]
twist_cmd.twist.angular.x = -self.k_R * e_R[0] - self.k_w * e_w[
0] + w_des[0]
twist_cmd.twist.angular.y = -self.k_R * e_R[1] - self.k_w * e_w[
1] + w_des[1]
twist_cmd.twist.angular.z = -self.k_R * e_R[2] - self.k_w * e_w[
2] + w_des[2]
self.twist_pub.publish(twist_cmd)
self.publishGoal(self.curr_state)
return
def turn_to_goal(self):
rx, ry, rz, r, p, y = self.parse_local_position("e")
vel = TwistStamped()
vel.header = Header()
vel.header.frame_id = "map"
vel.header.stamp = rospy.Time.now()
vel.twist.linear.x = 0
vel.twist.linear.y = 0
vel.twist.linear.z = 0
vel.twist.angular.x = 0 #pid.step(0-r)
vel.twist.angular.y = 0 #pid.step(0-p)
if self.global_goal is None:
self.global_goal = self.goal
yaw = math.atan2(self.global_goal[1] - ry, self.global_goal[0] - rx)
if abs(yaw - y) > math.pi:
yaw = (2 * math.pi - abs(yaw)) * np.sign(-yaw)
while abs(yaw - y) > 0.1:
yaw = math.atan2(self.global_goal[1] - ry,
self.global_goal[0] - rx)
if abs(yaw - y) > math.pi:
yaw = (2 * math.pi - abs(yaw)) * np.sign(-yaw)
vel.twist.angular.z = self.pid.step(yaw - y)
self.pos_setvel_pub.publish(vel)
rospy.sleep(-1)
rx, ry, rz, r, p, y = self.parse_local_position("e")
def initialise(self, measurement: Odometry):
self.prev_measurement = measurement
self.tmeas = float(measurement.header.stamp.to_sec())
zero_input_cmd = TwistStamped()
zero_input_cmd.header = measurement.header
self.cmd_list = [zero_input_cmd]
self.requires_initialisation = False
def write_wheel(wheel, i, bag):
utime = wheel[i, 0]
timestamp = rospy.Time.from_sec(utime / 1e6)
header = Header()
header.seq = i
header.stamp = timestamp
header.frame_id = 'wheelSpeed'
wheelLeft = TwistStamped()
wheelLeft.header = header
wheelLeft.twist.linear.z = float(wheel[i, 1])
wheelRight = TwistStamped()
wheelRight.header = header
wheelRight.twist.linear.z = float(wheel[i, 2])
bag.write('wheelLeft', wheelLeft, t=timestamp)
bag.write('wheelRight', wheelRight, t=timestamp)
def listener_callback(self, inMsg):
outMsg = TwistStamped()
outMsg.header = Header()
outMsg.header.stamp = self.get_clock().now().to_msg()
outMsg.header.frame_id = self.frame_id
outMsg.twist = inMsg
self.publisher_.publish(outMsg)
def create_twist(self, velocity, angular):
tw = TwistStamped()
tw.header = Header()
tw.header.stamp = rospy.Time.now()
tw.header.frame_id = '/vehicle'
tw.twist.linear.x = velocity
tw.twist.angular.z = angular
return tw
def publish(self):
print "publishing"
message = TwistStamped()
message.header = self.pose.header
message.twist.linear.x = self.pose.vector.x
message.twist.linear.y = self.pose.vector.y
message.twist.linear.z = 0.0
message.twist.angular.x = 0.0
message.twist.angular.y = 0.0
message.twist.angular.z = self.pose.vector.z
print self.pose.vector.z
self.pub.publish(message)
message = Vector3Stamped()
message.header = self.pose.header
message.vector = self.encoderSpeed
self.motor_test.publish(message)
def set_velocity(self,vx,vy,vz): vel = TwistStamped() vel.twist.linear.x = vx vel.twist.linear.y = vy vel.twist.linear.z = vz vel.header = Header() vel.header.frame_id = "map" vel.header.stamp = rospy.Time.now() self.pos_setvel_pub.publish(vel)
def default(self, ci='unused'):
twist = TwistStamped()
twist.header = self.get_ros_header()
# Fill twist-msg with the values from the sensor
twist.twist.linear.x = self.data['velocity'][0]
twist.twist.linear.y = self.data['velocity'][1]
twist.twist.linear.z = self.data['velocity'][2]
self.publish(twist)
def default(self, ci='unused'):
twist = TwistStamped()
twist.header = self.get_ros_header()
# Fill twist-msg with the values from the sensor
twist.twist.linear.x = self.data['velocity'][0]
twist.twist.linear.y = self.data['velocity'][1]
twist.twist.linear.z = self.data['velocity'][2]
self.publish(twist)
def _make_twist_stamped(self, linear, angular):
twist_stamped = TwistStamped()
header = Header()
header.stamp = rclpy.clock.Clock().now().to_msg()
header.frame_id = 'key_teleop'
twist_stamped.header = header
twist_stamped.twist.linear.x = linear
twist_stamped.twist.angular.z = angular
return twist_stamped
def update(self, state):
if (self.target is None):
rospy.logwarn("Target position for velocity controller is none.")
return None
# simplify variables a bit
time = state.header.stamp.to_sec()
position = state.pose.position
orientation = state.pose.orientation
quat_arr = np.array(
[orientation.x, orientation.y, orientation.z, orientation.w])
att = euler_from_quaternion(quat_arr, 'sxyz')
# create output structure
output = TwistStamped()
output.header = state.header
# output velocities
linear = Vector3()
angular = Vector3()
if (False): #abs(att[2]-self.heading_target) > deg2rad(5.0)):
# Control in X vel
linear.x = self.X.update(position.x, position.x, time)
# Control in Y vel
linear.y = self.Y.update(position.y, position.y, time)
# Control in Z vel
linear.z = self.Z.update(position.z, position.z, time)
else:
# Control in X vel
linear.x = self.X.update(self.target.position.x, position.x, time)
# Control in Y vel
linear.y = self.Y.update(self.target.position.y, position.y, time)
# Control in Z vel
linear.z = self.Z.update(self.target.position.z, position.z, time)
# Control yaw (no x, y angular)
delta = 0.3
#if((abs(linear.y) > delta) and (abs(linear.x) > delta)) or (self.psi_target == None):
output.twist = Twist()
#if(self.distance(self.target,state.pose)>delta):
phi_target = 0.0
theta_target = 0.0
psi_target = self.heading_target #np.arctan((self.target.position.y-self.prev_target.position.y)/(self.target.position.x-self.prev_target.position.x))#(linear.y,linear.x)
#psi_target = np.arctan((self.target.position.y-self.prev_target.position.y)/(self.target.position.x-self.prev_target.position.x))#(linear.y,linear.x)
#psi_target = math.atan2(linear.y,linear.x)
#angular.x = self.PHI.update(phi_target, att[0],time)
#angular.y = self.THETA.update(theta_target, att[1],time)
angular.z = self.PSI.update(psi_target, att[2], time)
output.twist.angular = angular
# TODO
output.twist.linear = linear
if output == None:
print "output", output
return output
def default(self, ci='unused'):
twist = TwistStamped()
twist.header = self.get_ros_header()
twist.twist.linear.x = self.data['world_linear_velocity'][0]
twist.twist.linear.y = self.data['world_linear_velocity'][1]
twist.twist.linear.z = self.data['world_linear_velocity'][2]
twist.twist.angular.x = self.data['angular_velocity'][0]
twist.twist.angular.y = self.data['angular_velocity'][1]
twist.twist.angular.z = self.data['angular_velocity'][2]
self.publish(twist)
def default(self, ci='unused'):
twist = TwistStamped()
twist.header = self.get_ros_header()
twist.twist.linear.x = self.data['linear_velocity'][0]
twist.twist.linear.y = self.data['linear_velocity'][1]
twist.twist.linear.z = self.data['linear_velocity'][2]
twist.twist.angular.x = self.data['angular_velocity'][0]
twist.twist.angular.y = self.data['angular_velocity'][1]
twist.twist.angular.z = self.data['angular_velocity'][2]
self.publish(twist)
def vec_cb(msg):
global twist_pub
twst = TwistStamped()
twst.twist.linear.x = -msg.vector.y
twst.twist.linear.y = msg.vector.x
twst.twist.linear.z = msg.vector.z
twst.header = msg.header
twst.twist.angular = Vector3(0, 0, 0)
twist_pub.publish(twst)
def angularVeloCallback(self, ang_velo):
self.angular_velo[0] = ang_velo.twist.angular.x
self.angular_velo[1] = ang_velo.twist.angular.y
self.angular_velo[2] = ang_velo.twist.angular.z
tmp_angular_velo_rot = np.array([ang_velo.twist.angular.x, ang_velo.twist.angular.y, ang_velo.twist.angular.z])
self.angular_velo_rot = self.qx_180.rotate(tmp_angular_velo_rot)
ang_veloNED = TwistStamped()
ang_veloNED.header = ang_velo.header # TIMESTAMP?
ang_veloNED.header.frame_id = 'global_tank'
ang_veloNED.twist.angular.x= self.angular_velo_rot[0]
ang_veloNED.twist.angular.y = self.angular_velo_rot[1]
ang_veloNED.twist.angular.z = self.angular_velo_rot[2]
self.angular_velocity_publish.publish(ang_veloNED)
def navigate(self):
rate = self.rospy.Rate(10) # 10hz
msg = TwistStamped()
msg.header = Header()
msg.header.frame_id = "base_footprint"
msg.header.stamp = rospy.Time.now()
while 1:
msg.twist.linear.x = self.x
msg.twist.linear.y = self.y
msg.twist.linear.z = self.z
self.pub.publish(msg)
rate.sleep()
def land_loop(self):
"""Loops in Control_Mode.TAKEOFF until either the mode is switched or the target altitude has been reached."""
# Count the number of iterations through the loop
loops = 0
# Build message
msg = TwistStamped()
msg.twist.linear.x = 0
msg.twist.linear.y = 0
msg.twist.linear.z = -0.4
while self.is_running() and self._mode == Control_Mode.LAND:
# Break if the magnitude is below the threshold for the desired amount of time
if abs(
math.sqrt(
math.pow(self._vehicle.get_velocity_x(), 2) +
math.pow(self._vehicle.get_velocity_y(), 2) +
math.pow(self._vehicle.get_velocity_z(), 2))) < 0.1:
# Check if the number of required successful loops have passed
# rospy.loginfo("X: " + str(self._vehicle.get_velocity_x()))
# rospy.loginfo("Y: " + str(self._vehicle.get_velocity_y()))
# rospy.loginfo("Z: " + str(self._vehicle.get_velocity_z()))
# rospy.loginfo("Total: " + str(abs(math.sqrt(math.pow(self._vehicle.get_velocity_x(), 2) + math.pow(self._vehicle.get_velocity_y(), 2) + math.pow(self._vehicle.get_velocity_z(), 2)))))
if loops > 10:
# Disarm
self._vehicle.disarm()
break
else:
# Increment number of loops
loops += 1
else:
# Reset loops
loops = 0
# Update message header
msg.header = Header(stamp=rospy.get_rostime())
# Publish message
self._vel_pub.publish(msg)
# Sleep to maintain appropriate update frequency
self._rate.sleep()
# Only perform if controller is still running
if self.is_running():
# Set mode to INITIAL
self.set_mode(Control_Mode.INITIAL)
def getTwistInBaseFrame(self, twistWC, header):
"""Returns a TwistWithCovarianceStamped in base frame"""
# Create the stamped object
twistS = TwistStamped()
twistS.header = header
twistS.twist = twistWC.twist
twistS_base = self.transformTwist(self.base_frame_id, twistS)
twistWC_out = TwistWithCovarianceStamped()
twistWC_out.header = twistS_base.header
twistWC_out.twist.twist = twistS_base.twist
twistWC_out.twist.covariance = twistWC.covariance
return twistWC_out
def navigate(self):
print("navigate")
rate = self.rospy.Rate(20) # 10hz
msg = TwistStamped()
msg.header = Header()
msg.header.frame_id = "base_footprint"
msg.header.stamp = rospy.Time.now()
while 1:
msg.twist.linear.x = self.x
msg.twist.linear.y = self.y
msg.twist.linear.z = self.z
msg.twist.angular.z = self.yaw
self.pub.publish(msg) # it publish it to the hardware ( the motors)
print "Published velocity to auto x:" + str(self.x) + " y:" + str(self.y) + " z:" + str(self.z) + " yaw:" + str(self.yaw)
rate.sleep()
def do_transform_twist(twist, transform):
transform = copy.deepcopy(transform)
kdl_tf = transform_to_kdl(transform)
linear = kdl_tf * PyKDL.Vector(twist.twist.linear.x, twist.twist.linear.y,
twist.twist.linear.z)
angular = kdl_tf * PyKDL.Vector(twist.twist.linear.x, twist.twist.linear.y,
twist.twist.linear.z)
res = TwistStamped()
res.twist.linear.x = linear[0]
res.twist.linear.y = linear[1]
res.twist.linear.z = linear[2]
res.twist.angular.x = angular[0]
res.twist.angular.y = angular[1]
res.twist.angular.z = angular[2]
res.header = transform.header
return res
def twist_cb(self, twist):
# get teleop twist message
twist_stamped = TwistStamped()
twist_stamped.twist = twist
if not (
twist.linear.x == 0
and twist.linear.y == 0
and twist.linear.z == 0
and twist.angular.x == 0
and twist.angular.y == 0
and twist.angular.z == 0
):
# give it a header
header = Header()
header.stamp = rospy.Time.now()
twist_stamped.header = header
# publish the command
self.teleop_stamped_pub.publish(twist_stamped)
def twistUpdate(self):
if self.curr_state:
x_des, v_des, R_des, w_des = self.getGoalState(self.curr_state)
e_x, e_v, e_R, e_w = self.getErrors(self.curr_state)
twist_cmd = TwistStamped()
twist_cmd.header = Header()
twist_cmd.header.stamp = rospy.Time.now()
twist_cmd.twist.linear.x = -self.k_x * e_x[0] - self.k_v * e_v[0] + v_des[0]
twist_cmd.twist.linear.y = -self.k_x * e_x[1] - self.k_v * e_v[1] + v_des[1]
twist_cmd.twist.linear.z = -self.k_x * e_x[2] - self.k_v * e_v[2] + v_des[2]
twist_cmd.twist.angular.x = -self.k_R * e_R[0] - self.k_w * e_w[0] + w_des[0]
twist_cmd.twist.angular.y = -self.k_R * e_R[1] - self.k_w * e_w[1] + w_des[1]
twist_cmd.twist.angular.z = -self.k_R * e_R[2] - self.k_w * e_w[2] + w_des[2]
self.twist_pub.publish(twist_cmd)
self.publishGoal(self.curr_state)
return
def navigate(self):
rate = self.rospy.Rate(10) # 10hz
msg = TwistStamped()
msg.header = Header()
msg.header.frame_id = "base_footprint"
msg.header.stamp = rospy.Time.now()
while 1:
msg.twist.linear.x = self.x
msg.twist.linear.y = self.y
msg.twist.linear.z = self.z
# For demo purposes we will lock yaw/heading to north.
#yaw_degrees = 0 # North
#yaw = radians(yaw_degrees)
#quaternion = quaternion_from_euler(0, 0, yaw)
#msg.pose.orientation = Quaternion(*quaternion)
self.pub.publish(msg)
rate.sleep()
def Driver():
global rate,pub,start_time,states_in, x_est, P_est,eye,Q,Rk,status
if status:
#===============#
# Predict #
#===============#
dy = Quad(0)
xhat = x_est + dy/rate
F = eye + Quad_Jacobian(x_est)/rate
Phat = F*P_est*F.T + Q/rate
New_measurement = CheckInnovation(xhat,states_in,Phat)
if New_measurement: # then use the measurement
#==============#
# Update #
#==============#
deg2rad = np.pi/180
# measurement
zk = np.matrix([[states_in.x],[-states_in.y],[-states_in.z],[states_in.u],[-states_in.v],[-states_in.w],[states_in.phi*deg2rad],[states_in.theta*deg2rad],[states_in.psi*deg2rad],[states_in.p*deg2rad],[states_in.q*deg2rad],[states_in.r*deg2rad]])
# residual/innovation
yk = zk - xhat
# residual/innovation Covariance
Sk = Phat + Rk
# Kalman Gain
Kk = Phat*np.linalg.inv(Sk)
x_est = xhat + Kk*yk
P_est = (eye-Kk)*Phat
else:
x_est = xhat
P_est = Phat
#=================================================#
# Check if Estimate is actually within Room #
#=================================================#
if np.linalg.norm(x_est[0:3]) >6 or (x_est != x_est).all():
x_est = np.asmatrix(np.zeros((12,1)))
x_est[0:3] = np.matrix([[0],[0],[-2]])
P_est = 3*np.asmatrix(np.eye(12))
#--------------#
# Make Message #
#--------------#
cortex = Cortex()
cortex.Obj = [States()]*1
states = States()
states.name = states_in.name
states.visible = states_in.visible
global h
h.seq = h.seq + 1
h.stamp = rospy.Time.now()
h.frame_id = 'cortex'
cortex.header = h
rad2deg = 180/np.pi
# Invert y and z position and velocity from NED to Cartesian
states.x = x_est[0]
states.y = -x_est[1]
states.z = -x_est[2]
states.u = x_est[3]
states.v = -x_est[4]
states.w = -x_est[5]
states.phi = rad2deg*x_est[6]
states.theta = rad2deg*x_est[7]
states.psi = rad2deg*x_est[8]
states.p = rad2deg*x_est[9]
states.q = rad2deg*x_est[10]
states.r = rad2deg*x_est[11]
cortex.Obj[0] = states
global acc
acc.header = h
cov = TwistStamped()
cov.header = h
cov.twist.linear.x = x_est[0]
cov.twist.linear.y = -x_est[1]
cov.twist.linear.z = -x_est[2]
cov.twist.angular.x = P_est[0,0]
cov.twist.angular.y = P_est[1,1]
cov.twist.angular.z = P_est[2,2]
#---------#
# Publish #
#---------#
pub_cov.publish(cov)
pub_acc.publish(acc)
pub.publish(cortex)
def spin_once(self):
'''Read data from device and publishes ROS messages.'''
# common header
h = Header()
h.stamp = rospy.Time.now()
h.frame_id = self.frame_id
# create messages and default values
imu_msg = Imu()
imu_msg.orientation_covariance = (-1., )*9
imu_msg.angular_velocity_covariance = (-1., )*9
imu_msg.linear_acceleration_covariance = (-1., )*9
pub_imu = False
gps_msg = NavSatFix()
xgps_msg = GPSFix()
pub_gps = False
vel_msg = TwistStamped()
pub_vel = False
mag_msg = Vector3Stamped()
pub_mag = False
temp_msg = Float32()
pub_temp = False
press_msg = Float32()
pub_press = False
anin1_msg = UInt16()
pub_anin1 = False
anin2_msg = UInt16()
pub_anin2 = False
pub_diag = False
def fill_from_raw(raw_data):
'''Fill messages with information from 'raw' MTData block.'''
# don't publish raw imu data anymore
# TODO find what to do with that
pass
def fill_from_rawgps(rawgps_data):
'''Fill messages with information from 'rawgps' MTData block.'''
global pub_hps, xgps_msg, gps_msg
if rawgps_data['bGPS']<self.old_bGPS:
pub_gps = True
# LLA
xgps_msg.latitude = gps_msg.latitude = rawgps_data['LAT']*1e-7
xgps_msg.longitude = gps_msg.longitude = rawgps_data['LON']*1e-7
xgps_msg.altitude = gps_msg.altitude = rawgps_data['ALT']*1e-3
# NED vel # TODO?
# Accuracy
# 2 is there to go from std_dev to 95% interval
xgps_msg.err_horz = 2*rawgps_data['Hacc']*1e-3
xgps_msg.err_vert = 2*rawgps_data['Vacc']*1e-3
self.old_bGPS = rawgps_data['bGPS']
def fill_from_Temp(temp):
'''Fill messages with information from 'temperature' MTData block.'''
global pub_temp, temp_msg
pub_temp = True
temp_msg.data = temp
def fill_from_Calib(imu_data):
'''Fill messages with information from 'calibrated' MTData block.'''
global pub_imu, imu_msg, pub_vel, vel_msg, pub_mag, mag_msg
try:
pub_imu = True
imu_msg.angular_velocity.x = imu_data['gyrX']
imu_msg.angular_velocity.y = imu_data['gyrY']
imu_msg.angular_velocity.z = imu_data['gyrZ']
imu_msg.angular_velocity_covariance = (radians(0.025), 0., 0., 0.,
radians(0.025), 0., 0., 0., radians(0.025))
pub_vel = True
vel_msg.twist.angular.x = imu_data['gyrX']
vel_msg.twist.angular.y = imu_data['gyrY']
vel_msg.twist.angular.z = imu_data['gyrZ']
except KeyError:
pass
try:
pub_imu = True
imu_msg.linear_acceleration.x = imu_data['accX']
imu_msg.linear_acceleration.y = imu_data['accY']
imu_msg.linear_acceleration.z = imu_data['accZ']
imu_msg.linear_acceleration_covariance = (0.0004, 0., 0., 0.,
0.0004, 0., 0., 0., 0.0004)
except KeyError:
pass
try:
pub_mag = True
mag_msg.vector.x = imu_data['magX']
mag_msg.vector.y = imu_data['magY']
mag_msg.vector.z = imu_data['magZ']
except KeyError:
pass
def fill_from_Vel(velocity_data):
'''Fill messages with information from 'velocity' MTData block.'''
global pub_vel, vel_msg
pub_vel = True
vel_msg.twist.linear.x = velocity_data['Vel_X']
vel_msg.twist.linear.y = velocity_data['Vel_Y']
vel_msg.twist.linear.z = velocity_data['Vel_Z']
def fill_from_Orient(orient_data):
'''Fill messages with information from 'orientation' MTData block.'''
global pub_imu, imu_msg
pub_imu = True
if orient.has_key('quaternion'):
w, x, y, z = orient['quaternion']
elif orient.has_key('roll'):
# FIXME check that Euler angles are in radians
x, y, z, w = quaternion_from_euler(radians(orient['roll']),
radians(orient['pitch']), radians(orient['yaw']))
elif orient.has_key('matrix'):
m = identity_matrix()
m[:3,:3] = orient['matrix']
x, y, z, w = quaternion_from_matrix(m)
imu_msg.orientation.x = x
imu_msg.orientation.y = y
imu_msg.orientation.z = z
imu_msg.orientation.w = w
imu_msg.orientation_covariance = (radians(1.), 0., 0., 0.,
radians(1.), 0., 0., 0., radians(9.))
def fill_from_Pos(position_data):
'''Fill messages with information from 'position' MTData block.'''
global pub_gps, xgps_msg, gps_msg
pub_gps = True
xgps_msg.latitude = gps_msg.latitude = position_data['Lat']
xgps_msg.longitude = gps_msg.longitude = position_data['Lon']
xgps_msg.altitude = gps_msg.altitude = position_data['Alt']
def fill_from_Stat(status):
'''Fill messages with information from 'status' MTData block.'''
global pub_diag, pub_gps, gps_msg, xgps_msg
pub_diag = True
if status & 0b0001:
self.stest_stat.level = DiagnosticStatus.OK
self.stest_stat.message = "Ok"
else:
self.stest_stat.level = DiagnosticStatus.ERROR
self.stest_stat.message = "Failed"
if status & 0b0010:
self.xkf_stat.level = DiagnosticStatus.OK
self.xkf_stat.message = "Valid"
else:
self.xkf_stat.level = DiagnosticStatus.WARN
self.xkf_stat.message = "Invalid"
if status & 0b0100:
self.gps_stat.level = DiagnosticStatus.OK
self.gps_stat.message = "Ok"
gps_msg.status.status = NavSatStatus.STATUS_FIX
xgps_msg.status.status = GPSStatus.STATUS_FIX
gps_msg.status.service = NavSatStatus.SERVICE_GPS
xgps_msg.status.position_source = 0b01101001
xgps_msg.status.motion_source = 0b01101010
xgps_msg.status.orientation_source = 0b01101010
else:
self.gps_stat.level = DiagnosticStatus.WARN
self.gps_stat.message = "No fix"
gps_msg.status.status = NavSatStatus.STATUS_NO_FIX
xgps_msg.status.status = GPSStatus.STATUS_NO_FIX
gps_msg.status.service = 0
xgps_msg.status.position_source = 0b01101000
xgps_msg.status.motion_source = 0b01101000
xgps_msg.status.orientation_source = 0b01101000
def fill_from_Auxiliary(aux_data):
'''Fill messages with information from 'Auxiliary' MTData block.'''
global pub_anin1, pub_anin2, anin1_msg, anin2_msg
try:
anin1_msg.data = o['Ain_1']
pub_anin1 = True
except KeyError:
pass
try:
anin2_msg.data = o['Ain_2']
pub_anin2 = True
except KeyError:
pass
def fill_from_Temperature(o):
'''Fill messages with information from 'Temperature' MTData2 block.'''
global pub_temp, temp_msg
pub_temp = True
temp_msg.data = o['Temp']
def fill_from_Timestamp(o):
'''Fill messages with information from 'Timestamp' MTData2 block.'''
global h
try:
# put timestamp from gps UTC time if available
y, m, d, hr, mi, s, ns, f = o['Year'], o['Month'], o['Day'],\
o['Hour'], o['Minute'], o['Second'], o['ns'], o['Flags']
if f&0x4:
secs = time.mktime((y, m, d, hr, mi, s, 0, 0, 0))
h.stamp.secs = secs
h.stamp.nsecs = ns
except KeyError:
pass
# TODO find what to do with other kind of information
pass
def fill_from_Orientation_Data(o):
'''Fill messages with information from 'Orientation Data' MTData2 block.'''
global pub_imu, imu_msg
pub_imu = True
try:
x, y, z, w = o['Q1'], o['Q2'], o['Q3'], o['Q0']
except KeyError:
pass
try:
# FIXME check that Euler angles are in radians
x, y, z, w = quaternion_from_euler(radians(o['Roll']),
radians(o['Pitch']), radians(o['Yaw']))
except KeyError:
pass
try:
a, b, c, d, e, f, g, h, i = o['a'], o['b'], o['c'], o['d'],\
o['e'], o['f'], o['g'], o['h'], o['i']
m = identity_matrix()
m[:3,:3] = ((a, b, c), (d, e, f), (g, h, i))
x, y, z, w = quaternion_from_matrix(m)
except KeyError:
pass
imu_msg.orientation.x = x
imu_msg.orientation.y = y
imu_msg.orientation.z = z
imu_msg.orientation.w = w
imu_msg.orientation_covariance = (radians(1.), 0., 0., 0.,
radians(1.), 0., 0., 0., radians(9.))
def fill_from_Pressure(o):
'''Fill messages with information from 'Pressure' MTData2 block.'''
global pub_press, press_msg
press_msg.data = o['Pressure']
def fill_from_Acceleration(o):
'''Fill messages with information from 'Acceleration' MTData2 block.'''
global pub_imu, imu_msg
pub_imu = True
# FIXME not sure we should treat all in that same way
try:
x, y, z = o['Delta v.x'], o['Delta v.y'], o['Delta v.z']
except KeyError:
pass
try:
x, y, z = o['freeAccX'], o['freeAccY'], o['freeAccZ']
except KeyError:
pass
try:
x, y, z = o['accX'], o['accY'], o['accZ']
except KeyError:
pass
imu_msg.linear_acceleration.x = x
imu_msg.linear_acceleration.y = y
imu_msg.linear_acceleration.z = z
imu_msg.linear_acceleration_covariance = (0.0004, 0., 0., 0.,
0.0004, 0., 0., 0., 0.0004)
def fill_from_Position(o):
'''Fill messages with information from 'Position' MTData2 block.'''
global pub_gps, xgps_msg, gps_msg
# TODO publish ECEF
try:
xgps_msg.latitude = gps_msg.latitude = o['lat']
xgps_msg.longitude = gps_msg.longitude = o['lon']
pub_gps = True
alt = o.get('altMsl', o.get('altEllipsoid', 0))
xgps_msg.altitude = gps_msg.altitude = alt
except KeyError:
pass
def fill_from_Angular_Velocity(o):
'''Fill messages with information from 'Angular Velocity' MTData2 block.'''
global pub_imu, imu_msg, pub_vel, vel_msg
try:
imu_msg.angular_velocity.x = o['gyrX']
imu_msg.angular_velocity.y = o['gyrY']
imu_msg.angular_velocity.z = o['gyrZ']
imu_msg.angular_velocity_covariance = (radians(0.025), 0., 0., 0.,
radians(0.025), 0., 0., 0., radians(0.025))
pub_imu = True
vel_msg.twist.angular.x = o['gyrX']
vel_msg.twist.angular.y = o['gyrY']
vel_msg.twist.angular.z = o['gyrZ']
pub_vel = True
except KeyError:
pass
# TODO decide what to do with 'Delta q'
def fill_from_GPS(o):
'''Fill messages with information from 'GPS' MTData2 block.'''
global pub_gps, h, xgps_msg
try: # DOP
xgps_msg.gdop = o['gDOP']
xgps_msg.pdop = o['pDOP']
xgps_msg.hdop = o['hDOP']
xgps_msg.vdop = o['vDOP']
xgps_msg.tdop = o['tDOP']
pub_gps = True
except KeyError:
pass
try: # Time UTC
y, m, d, hr, mi, s, ns, f = o['year'], o['month'], o['day'],\
o['hour'], o['min'], o['sec'], o['nano'], o['valid']
if f&0x4:
secs = time.mktime((y, m, d, hr, mi, s, 0, 0, 0))
h.stamp.secs = secs
h.stamp.nsecs = ns
except KeyError:
pass
# TODO publish SV Info
def fill_from_SCR(o):
'''Fill messages with information from 'SCR' MTData2 block.'''
# TODO that's raw information
pass
def fill_from_Analog_In(o):
'''Fill messages with information from 'Analog In' MTData2 block.'''
global pub_anin1, pub_anin2, anin1_msg, anin2_msg
try:
anin1_msg.data = o['analogIn1']
pub_anin1 = True
except KeyError:
pass
try:
anin2_msg.data = o['analogIn2']
pub_anin2 = True
except KeyError:
pass
def fill_from_Magnetic(o):
'''Fill messages with information from 'Magnetic' MTData2 block.'''
global pub_mag, mag_msg
mag_msg.vector.x = o['magX']
mag_msg.vector.y = o['magY']
mag_msg.vector.z = o['magZ']
pub_mag = True
def fill_from_Velocity(o):
'''Fill messages with information from 'Velocity' MTData2 block.'''
global pub_vel, vel_msg
vel_msg.twist.linear.x = o['velX']
vel_msg.twist.linear.y = o['velY']
vel_msg.twist.linear.z = o['velZ']
pub_vel = True
def fill_from_Status(o):
'''Fill messages with information from 'Status' MTData2 block.'''
try:
status = o['StatusByte']
fill_from_Stat(status)
except KeyError:
pass
try:
status = o['StatusWord']
fill_from_Stat(status)
except KeyError:
pass
# TODO RSSI
def find_handler_name(name):
return "fill_from_%s"%(name.replace(" ", "_"))
# get data
data = self.mt.read_measurement()
# fill messages based on available data fields
for n, o in data:
try:
locals()[find_handler_name(n)](o)
except KeyError:
rospy.logwarn("Unknown MTi data packet: '%s', ignoring."%n)
# publish available information
if pub_imu:
imu_msg.header = h
self.imu_pub.publish(imu_msg)
if pub_gps:
xgps_msg.header = gps_msg.header = h
self.gps_pub.publish(gps_msg)
self.xgps_pub.publish(xgps_msg)
if pub_vel:
vel_msg.header = h
self.vel_pub.publish(vel_msg)
if pub_mag:
mag_msg.header = h
self.mag_pub.publish(mag_msg)
if pub_temp:
self.temp_pub.publish(temp_msg)
if pub_press:
self.press_pub.publish(press_msg)
if pub_anin1:
self.analog_in1_pub.publish(anin1_msg)
if pub_anin2:
self.analog_in2_pub.publish(anin2_msg)
if pub_diag:
self.diag_msg.header = h
self.diag_pub.publish(self.diag_msg)
# publish string representation
self.str_pub.publish(str(data))
def spin_once(self):
def quat_from_orient(orient):
'''Build a quaternion from orientation data.'''
try:
w, x, y, z = orient['quaternion']
return (x, y, z, w)
except KeyError:
pass
try:
return quaternion_from_euler(pi*orient['roll']/180.,
pi*orient['pitch']/180, pi*orient['yaw']/180.)
except KeyError:
pass
try:
m = identity_matrix()
m[:3,:3] = orient['matrix']
return quaternion_from_matrix(m)
except KeyError:
pass
# get data
data = self.mt.read_measurement()
# common header
h = Header()
h.stamp = rospy.Time.now()
h.frame_id = self.frame_id
# get data (None if not present)
temp = data.get('Temp') # float
raw_data = data.get('RAW')
imu_data = data.get('Calib')
orient_data = data.get('Orient')
velocity_data = data.get('Velocity')
position_data = data.get('Position')
rawgps_data = data.get('RAWGPS')
status = data.get('Status') # int
sample = data.get('Sample')
# TODO by @demmeln: use sample counter for timestamp
# correction. Watch out for counter wrap.
# create messages and default values
imu_msg = Imu()
imu_msg.orientation_covariance = (-1., )*9
imu_msg.angular_velocity_covariance = (-1., )*9
imu_msg.linear_acceleration_covariance = (-1., )*9
pub_imu = False
gps_msg = NavSatFix()
xgps_msg = GPSFix()
pub_gps = False
vel_msg = TwistStamped()
pub_vel = False
mag_msg = Vector3Stamped()
pub_mag = False
temp_msg = Float32()
pub_temp = False
# fill information where it's due
# start by raw information that can be overriden
if raw_data: # TODO warn about data not calibrated
pub_imu = True
pub_vel = True
pub_mag = True
pub_temp = True
# acceleration
imu_msg.linear_acceleration.x = raw_data['accX']
imu_msg.linear_acceleration.y = raw_data['accY']
imu_msg.linear_acceleration.z = raw_data['accZ']
imu_msg.linear_acceleration_covariance = (0., )*9
# gyroscopes
imu_msg.angular_velocity.x = raw_data['gyrX']
imu_msg.angular_velocity.y = raw_data['gyrY']
imu_msg.angular_velocity.z = raw_data['gyrZ']
imu_msg.angular_velocity_covariance = (0., )*9
vel_msg.twist.angular.x = raw_data['gyrX']
vel_msg.twist.angular.y = raw_data['gyrY']
vel_msg.twist.angular.z = raw_data['gyrZ']
# magnetometer
mag_msg.vector.x = raw_data['magX']
mag_msg.vector.y = raw_data['magY']
mag_msg.vector.z = raw_data['magZ']
# temperature
# 2-complement decoding and 1/256 resolution
x = raw_data['temp']
if x&0x8000:
temp_msg.data = (x - 1<<16)/256.
else:
temp_msg.data = x/256.
if rawgps_data:
if rawgps_data['bGPS']<self.old_bGPS:
pub_gps = True
# LLA
xgps_msg.latitude = gps_msg.latitude = rawgps_data['LAT']*1e-7
xgps_msg.longitude = gps_msg.longitude = rawgps_data['LON']*1e-7
xgps_msg.altitude = gps_msg.altitude = rawgps_data['ALT']*1e-3
# NED vel # TODO?
# Accuracy
# 2 is there to go from std_dev to 95% interval
xgps_msg.err_horz = 2*rawgps_data['Hacc']*1e-3
xgps_msg.err_vert = 2*rawgps_data['Vacc']*1e-3
self.old_bGPS = rawgps_data['bGPS']
if temp is not None:
pub_temp = True
temp_msg.data = temp
if imu_data:
try:
imu_msg.angular_velocity.x = imu_data['gyrX']
imu_msg.angular_velocity.y = imu_data['gyrY']
imu_msg.angular_velocity.z = imu_data['gyrZ']
imu_msg.angular_velocity_covariance = (radians(0.025), 0., 0., 0.,
radians(0.025), 0., 0., 0., radians(0.025))
pub_imu = True
vel_msg.twist.angular.x = imu_data['gyrX']
vel_msg.twist.angular.y = imu_data['gyrY']
vel_msg.twist.angular.z = imu_data['gyrZ']
pub_vel = True
except KeyError:
pass
try:
imu_msg.linear_acceleration.x = imu_data['accX']
imu_msg.linear_acceleration.y = imu_data['accY']
imu_msg.linear_acceleration.z = imu_data['accZ']
imu_msg.linear_acceleration_covariance = (0.0004, 0., 0., 0.,
0.0004, 0., 0., 0., 0.0004)
pub_imu = True
except KeyError:
pass
try:
mag_msg.vector.x = imu_data['magX']
mag_msg.vector.y = imu_data['magY']
mag_msg.vector.z = imu_data['magZ']
pub_mag = True
except KeyError:
pass
if velocity_data:
pub_vel = True
vel_msg.twist.linear.x = velocity_data['Vel_X']
vel_msg.twist.linear.y = velocity_data['Vel_Y']
vel_msg.twist.linear.z = velocity_data['Vel_Z']
if orient_data:
pub_imu = True
orient_quat = quat_from_orient(orient_data)
imu_msg.orientation.x = orient_quat[0]
imu_msg.orientation.y = orient_quat[1]
imu_msg.orientation.z = orient_quat[2]
imu_msg.orientation.w = orient_quat[3]
imu_msg.orientation_covariance = (radians(1.), 0., 0., 0.,
radians(1.), 0., 0., 0., radians(9.))
if position_data:
pub_gps = True
xgps_msg.latitude = gps_msg.latitude = position_data['Lat']
xgps_msg.longitude = gps_msg.longitude = position_data['Lon']
xgps_msg.altitude = gps_msg.altitude = position_data['Alt']
if status is not None:
if status & 0b0001:
self.stest_stat.level = DiagnosticStatus.OK
self.stest_stat.message = "Ok"
else:
self.stest_stat.level = DiagnosticStatus.ERROR
self.stest_stat.message = "Failed"
if status & 0b0010:
self.xkf_stat.level = DiagnosticStatus.OK
self.xkf_stat.message = "Valid"
else:
self.xkf_stat.level = DiagnosticStatus.WARN
self.xkf_stat.message = "Invalid"
if status & 0b0100:
self.gps_stat.level = DiagnosticStatus.OK
self.gps_stat.message = "Ok"
else:
self.gps_stat.level = DiagnosticStatus.WARN
self.gps_stat.message = "No fix"
self.diag_msg.header = h
self.diag_pub.publish(self.diag_msg)
if pub_gps:
if status & 0b0100:
gps_msg.status.status = NavSatStatus.STATUS_FIX
xgps_msg.status.status = GPSStatus.STATUS_FIX
gps_msg.status.service = NavSatStatus.SERVICE_GPS
xgps_msg.status.position_source = 0b01101001
xgps_msg.status.motion_source = 0b01101010
xgps_msg.status.orientation_source = 0b01101010
else:
gps_msg.status.status = NavSatStatus.STATUS_NO_FIX
xgps_msg.status.status = GPSStatus.STATUS_NO_FIX
gps_msg.status.service = 0
xgps_msg.status.position_source = 0b01101000
xgps_msg.status.motion_source = 0b01101000
xgps_msg.status.orientation_source = 0b01101000
# publish available information
if pub_imu:
imu_msg.header = h
self.imu_pub.publish(imu_msg)
if pub_gps:
xgps_msg.header = gps_msg.header = h
self.gps_pub.publish(gps_msg)
self.xgps_pub.publish(xgps_msg)
if pub_vel:
vel_msg.header = h
self.vel_pub.publish(vel_msg)
if pub_mag:
mag_msg.header = h
self.mag_pub.publish(mag_msg)
if pub_temp:
self.temp_pub.publish(temp_msg)
def copy_vel(self, vel):
copied_vel = TwistStamped()
copied_vel.header= vel.header
return copied_vel
def onTwistStamped(self,msg): out_msg = TwistStamped() out_msg.header = rospy.time.now() out_msg.twist = msg self.twist_pub.publish(out_msg)
