def show_fps(fps):
msgFps = UInt16()
msgFps.data = int(fps)
videoFpsPub.publish(msgFps)
print('FPS: %g' % fps)
def test_ros_message_with_uint16(self):
from std_msgs.msg import UInt16
expected_dictionary = { 'data': 0xFFFF }
message = UInt16(data=expected_dictionary['data'])
dictionary = message_converter.convert_ros_message_to_dictionary(message)
self.assertEqual(dictionary, expected_dictionary)
def process_key(self, ch):
#
# AM_DRIVER COMMANDS
#
if ch in self.cmd_bindings.keys():
self.command = self.cmd_bindings[ch]
elif ch in self.set_bindings.keys():
self.speed = self.speed * (1 +
self.set_bindings[ch] * self.inc_ratio)
elif ch == 'g':
self.loginfo('Quitting')
# Stop the robot
twist = Twist()
self.pub_twist.publish(twist)
# Stop following loop!
mode = UInt16()
mode.data = 0x17
self.pub_mode.publish(mode)
rospy.signal_shutdown('Shutdown')
elif ch == '1':
# Manual mode
mode = UInt16()
mode.data = Mode.MODE_MANUAL
self.pub_mode.publish(mode)
self.command = np.array([0, 0])
elif ch == '2':
# Random mode
mode = UInt16()
mode.data = Mode.MODE_RANDOM
self.pub_mode.publish(mode)
self.command = np.array([0, 0])
elif ch == 'j':
# Cutting disc ON
mode = UInt16()
mode.data = Mode.MODE_CUTTING_DISC_ON
self.pub_mode.publish(mode)
elif ch == 'k':
# Cutting disc OFF
mode = UInt16()
mode.data = Mode.MODE_CUTTING_DISC_OFF
self.pub_mode.publish(mode)
elif ch == 'i':
# Cutting height HIGH
mode = UInt16()
mode.data = Mode.MODE_CUTTING_HEIGHT_60
self.pub_mode.publish(mode)
elif ch == 'o':
# Cutting height LOW
mode = UInt16()
mode.data = Mode.MODE_CUTTING_HEIGHT_40
self.pub_mode.publish(mode)
elif ch == 'p':
# Request SEARCHING (charge)
mode = UInt16()
mode.data = Mode.MODE_PARK
# self.searching = True
#else:
# mode.data = 0x101
# self.searching = False
self.pub_mode.publish(mode)
elif ch == '8':
# Enable Loop
mode = UInt16()
mode.data = Mode.MODE_LOOP_ON
self.pub_mode.publish(mode)
elif ch == '9':
# Disable Loop
mode = UInt16()
mode.data = Mode.MODE_LOOP_OFF
self.pub_mode.publish(mode)
elif ch == '5':
# Collsion inject
mode = UInt16()
mode.data = Mode.MODE_COLLISION_INJECT
self.pub_mode.publish(mode)
elif ch == '6':
# Beep
mode = UInt16()
mode.data = Mode.MODE_SOUND_CLICK
self.pub_mode.publish(mode)
else:
self.command = np.array([0, 0])
# let the master know about our name
rospy.init_node("riss")
# define and fill out some messages
publisherDataType = rospy.get_param("~publisher_data_type")
waitDuration = rospy.get_param("~wait_duration")
# define a duration of 1 second
myDuration = rospy.Duration(waitDuration, 0)
if (publisherDataType == "bool"):
msg = Bool()
msg.data = 0
myPublisher = rospy.Publisher('~my_ros_topic', Bool, queue_size=5)
elif (publisherDataType == "uint16"):
msg = UInt16()
msg.data = 10
myPublisher = rospy.Publisher('~my_ros_topic', UInt16, queue_size=5)
else:
msg = String()
msg.data = "publisher data type was not bool or uint16."
myPublisher = rospy.Publisher('~my_ros_topic', String, queue_size=5)
# wait for Ctrl-C and loop until then
while not rospy.is_shutdown():
# print a message for feedback
print "looping..."
# publish messages
myPublisher.publish(msg)
def process_key(self, ch):
#
# AM_DRIVER COMMANDS
#
if ch == 'h':
self.print_usage()
elif ch in self.cmd_bindings.keys():
self.command = self.cmd_bindings[ch]
elif ch == 'g':
self.loginfo('Quitting')
# Stop the robot
twist = Twist()
self.pub_twist.publish(twist)
rospy.signal_shutdown('Shutdown')
elif ch == 'l':
# Drive L and estimate all visible beacons
mode = UInt16()
mode.data = 0x40
self.app_cmd_pub.publish(mode)
self.command = np.array([0, 0])
elif ch == 'u':
# CLEAR PATH/COVERAGE
mode = UInt16()
mode.data = 0x02
self.app_cmd_pub.publish(mode)
elif ch == 'o':
# Start Auto Recording confinement
mode = UInt16()
mode.data = 0x06
self.app_cmd_pub.publish(mode)
elif ch == 'O':
# Pause Auto Recording confinement
mode = UInt16()
mode.data = 0x08
self.app_cmd_pub.publish(mode)
elif ch == 'i':
# INSERT (RECORD) NEW POINT AS POLYGON
mode = UInt16()
mode.data = 0x03
self.app_cmd_pub.publish(mode)
elif ch == 'I':
# Start auto or close polygon
mode = UInt16()
mode.data = 0x07
self.app_cmd_pub.publish(mode)
elif ch == 'p':
# save installation
mode = UInt16()
mode.data = 0x09
self.app_cmd_pub.publish(mode)
elif ch == 'P':
# start mowing
mode = UInt16()
mode.data = 0x0B
self.app_cmd_pub.publish(mode)
elif ch == 'k':
# pause / resume mowing
mode = UInt16()
mode.data = 0x0C
self.app_cmd_pub.publish(mode)
else:
self.command = np.array([0, 0])
def joy_callback(data):
# We read first the data from the joystick
axe0 = data.axes[0]
axe1 = data.axes[1]
axe2 = data.axes[2]
axe3 = data.axes[3]
button0 = data.buttons[0]
button1 = data.buttons[1]
button2 = data.buttons[2]
button3 = data.buttons[3]
button4 = data.buttons[4]
button5 = data.buttons[5]
button6 = data.buttons[6]
brush_msg = UInt16()
motor_msg = UInt16()
# Do something with the joystick data
## Brush Motors
brush_msg.data = int((axe1 + 1) * 40 + 50)
if (axe1 < 0.01) and (axe1 > (-0.01)):
brush_msg.data = 95
if button3:
pub_motor2.publish(brush_msg)
if button2:
pub_motor4.publish(brush_msg)
if button0:
pub_motor3.publish(brush_msg)
## Wheel Motors
alpha = 1
beta = 1
vr = alpha * axe3 + (beta * axe2)
vl = alpha * axe3 - (beta * axe2)
r = (2 - (vr + 1)) * (60) + 30
l = (2 - (vl + 1)) * (60) + 30
if (r < 70):
r = 70
if (l < 70):
l = 70
if (r > 120):
r = 120
if (l > 120):
l = 120
motor_msg.data = int(r)
pub_motor0.publish(motor_msg)
motor_msg.data = int(l)
pub_motor1.publish(motor_msg)
## Stop All Motors
if button1:
brush_msg.data = 95
motor_msg.data = 90
pub_motor0.publish(motor_msg)
pub_motor1.publish(motor_msg)
pub_motor2.publish(brush_msg)
pub_motor3.publish(brush_msg)
pub_motor4.publish(brush_msg)
def write_gps(gps, i, bag):
utime = gps[i, 0]
mode = gps[i, 1]
lat = gps[i, 3]
lng = gps[i, 4]
alt = gps[i, 5]
timestamp = rospy.Time.from_sec(utime / 1e6)
status = NavSatStatus()
if mode == 0 or mode == 1:
status.status = NavSatStatus.STATUS_NO_FIX
else:
status.status = NavSatStatus.STATUS_FIX
status.service = NavSatStatus.SERVICE_GPS
num_sats = UInt16()
num_sats.data = gps[i, 2]
fix = NavSatFix()
fix.status = status
fix.latitude = np.rad2deg(lat)
fix.longitude = np.rad2deg(lng)
fix.altitude = alt
track = Float64()
track.data = gps[i, 6]
speed = Float64()
speed.data = gps[i, 7]
bag.write('gps_fix', fix, t=timestamp)
bag.write('gps_track', track, t=timestamp)
bag.write('gps_speed', speed, t=timestamp)
# print("package image...")
# img_path = sys.argv[1] + 'images/2012-01-08/lb3/Cam0/'
# img_list = os.listdir(img_path)
# i_img = 0
# for img_name in img_list:
# i_img = i_img + 1
# print(i_img)
# img_cv = cv2.imread(os.path.join(img_path, img_name), -1)
# br = CvBridge()
# img = Image()
# img = br.cv2_to_imgmsg(img_cv, "bgr8")
# img.header.seq = i_img
# img.header.frame_id = 'camImage'
# bag.write('/camera/image', img)
img_path = sys.argv[1] + 'images/2012-01-08/lb3/Cam0/'
img_list = os.listdir(img_path)
img_cv = cv2.imread(os.path.join(img_path, img_list[i]), -1)
img_cv = cv2.resize(img_cv, IMAGEDIM, interpolation=cv2.INTER_AREA)
br = CvBridge()
img = Image()
img = br.cv2_to_imgmsg(img_cv, "bgr8")
def callback1(self, data):
self.steer = UInt16(90 + (data.data))
self.steer_ = True
def timer_callback(self):
msg = UInt16()
msg.data = self._tuning.direction
self._pub.publish(msg)
self.get_logger().debug('Publishing DOA {} '.format(msg.data))
def stop():
global pub
print "Stopping the motor."
pub_servo_pan.publish(UInt16(90))
pub_servo_tilt.publish(UInt16(90))
pub_laser_on_off.publish(Bool(0))
def move_dolly(self):
thred = 0.05
thred_over_cnt = 0
self._pub.publish(49)
while (thred_over_cnt < 3):
print('thred_over_cnt', thred_over_cnt)
######################
_, py, _ = self.target_marker_subL.getXYZ()
current_distance = self.get_state()
print('current distance: ', current_distance, '[m]')
#change direction when over limitation
if (current_distance < self.left_thred):
self.moveDirection = 'R'
elif (current_distance > self.right_thred):
self.moveDirection = 'L'
print('move to ', self.moveDirection)
#camera recognize tomato
if (py != -1):
d_err = -py
if (self.moveDirection == 'L'):
#so as not to go right close to right thredshold
if (py < -0.10):
print(
'so as not to go right close to right thredshold')
d_err = -1
#in case the camera doesn't recognize tomato
else:
if (self.moveDirection == 'L'):
d_err = -1
#so as not to go left close to left thredshold
if (py > 0.05):
print(
'****so as not to go left close to left thredshold'
)
d_err = 1
else:
d_err = 1
print('d_err : ', d_err)
###########################################
## usual stated_err
###########################################
D_isOK = bool(math.fabs(d_err) < thred)
if (D_isOK):
print('stop')
thred_over_cnt += 1
speed_control = 49
self._pub.publish(49)
# rospy.sleep(0.2)
else:
if (d_err > 0.7):
d_err = 0.7
elif (d_err < -0.7):
d_err = -0.7
speed_control = 49 + 35 * d_err #20 before
self._pub.publish(UInt16(speed_control))
rospy.sleep(0.15)
return self.moveDirection
from geometry_msgs.msg import Twist
#Model dependend settings
PI = 3.141
ROBOT_WIDTH = 0.07
WHEEL_DIAMETER = 0.05
WHEEL_RADIUS = WHEEL_DIAMETER / 2
WHEEL_PERIMETER = 2 * PI * WHEEL_RADIUS
#RPM dependant from voltage without load
#9.0V = 120 RPM
#7.5V = 80 RPM
# with load 60-80 rpm is a good average
MAX_RPM = 250.0
RPS = MAX_RPM / 60.0
left_whell = UInt16()
left_whell.data = 0
right_whell = UInt16()
right_whell.data = 0
MPS = RPS * WHEEL_PERIMETER
PWRDIV = 1000 * RPS
rospy.loginfo("PWRDIV:" + str(PWRDIV))
class MotorController(object):
global left_speed_out
global right_speed_out
#!/usr/bin/env python
import rospy
from aruco_pkg.msg import ArucoTriggerMsg
from aruco_pkg.msg import ArucoMsg
from geometry_msgs.msg import PoseStamped
from std_msgs.msg import UInt16
from geometry_msgs.msg import Twist
from move_base_msgs.msg import MoveBaseActionResult
rospy.init_node("control", anonymous=True)
servo_pub = rospy.Publisher('/servo', UInt16, queue_size=10)
serv = UInt16()
rate = rospy.Rate(0.3)
rate.sleep()
for i in range(10):
serv.data = 50
servo_pub.publish(serv)
def test_dictionary_with_uint16(self):
from std_msgs.msg import UInt16
expected_message = UInt16(data = 0xFFFF)
dictionary = { 'data': expected_message.data }
message = message_converter.convert_dictionary_to_ros_message('std_msgs/UInt16', dictionary)
self.assertEqual(message, expected_message)
def __init__(self):
rospy.init_node("control_step", anonymous=True)
self.servo_pub = rospy.Publisher('/servo', UInt16, queue_size=10)
self.trigger = 0
self.serv = UInt16()
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 process_key(self, ch):
#
# AM_DRIVER COMMANDS
#
if ch in self.cmd_bindings.keys():
self.command = self.cmd_bindings[ch]
elif ch in self.set_bindings.keys():
self.speed = self.speed * (1 +
self.set_bindings[ch] * self.inc_ratio)
elif ch == 'g':
self.loginfo('Quitting')
# Stop the robot
twist = Twist()
self.pub_twist.publish(twist)
# Stop following loop!
mode = UInt16()
mode.data = 0x17
self.pub_mode.publish(mode)
rospy.signal_shutdown('Shutdown')
elif ch == '1':
# Manual mode
mode = UInt16()
mode.data = Mode.MODE_MANUAL
self.pub_mode.publish(mode)
self.command = np.array([0, 0])
elif ch == '2':
# Random mode
mode = UInt16()
mode.data = Mode.MODE_RANDOM
self.pub_mode.publish(mode)
self.command = np.array([0, 0])
#DET addition
elif ch == '4':
self.command = self.cmd_bindings[0, -1]
delay(2)
self.command = self.cmd_bindings[0, 1]
delay(2)
self.command = self.cmd_bindings[0, -1]
delay(2)
self.command = self.cmd_bindings[0, 1]
delay(2)
elif ch == 'j':
# Cutting disc ON
mode = UInt16()
mode.data = Mode.MODE_CUTTING_DISC_ON
self.pub_mode.publish(mode)
elif ch == 'k':
# Cutting disc OFF
mode = UInt16()
mode.data = Mode.MODE_CUTTING_DISC_OFF
self.pub_mode.publish(mode)
elif ch == 'i':
# Cutting height HIGH
mode = UInt16()
mode.data = Mode.MODE_CUTTING_HEIGHT_60
self.pub_mode.publish(mode)
elif ch == 'o':
# Cutting height LOW
mode = UInt16()
mode.data = Mode.MODE_CUTTING_HEIGHT_40
self.pub_mode.publish(mode)
elif ch == 'p':
# Request SEARCHING (charge)
mode = UInt16()
mode.data = Mode.MODE_PARK
# self.searching = True
#else:
# mode.data = 0x101
# self.searching = False
self.pub_mode.publish(mode)
elif ch == '8':
# Enable Loop
mode = UInt16()
mode.data = Mode.MODE_LOOP_ON
self.pub_mode.publish(mode)
elif ch == '9':
# Disable Loop
mode = UInt16()
mode.data = Mode.MODE_LOOP_OFF
self.pub_mode.publish(mode)
elif ch == '5':
# Collsion inject
mode = UInt16()
mode.data = Mode.MODE_COLLISION_INJECT
self.pub_mode.publish(mode)
elif ch == '6':
# Beep
mode = UInt16()
mode.data = Mode.MODE_SOUND_CLICK
self.pub_mode.publish(mode)
#DET_custom entry*****
elif ch == 'v':
try:
now = rospy.Time.now()
rate = rospy.Rate(10)
newnow = 5
for x in range(0, 100):
while rospy.Time.now(
) < now + rospy.Duration.from_sec(newnow):
self.command = np.array([0, 1])
self.update()
mode = UInt16()
mode.data = Mode.MODE_SOUND_CLICK
self.pub_mode.publish(mode)
rate.sleep()
newnow = newnow + 5
while rospy.Time.now(
) < now + rospy.Duration.from_sec(newnow):
self.command = np.array([0, -1])
self.update()
rate.sleep()
newnow = newnow + 5
except rospy.ROSInterruptException:
pass
self.command = np.array([0, 0])
#DET_custom entry*****
else:
self.command = np.array([0, 0])
def closePwm1(self):
msg = UInt16()
msg.data = 10
self.pwm1_pub.publish(msg)
def __init__(self):
rospy.init_node("sender", anonymous=True)
self.sender_pub = rospy.Publisher('/control_frame', UInt16, queue_size=10)
self.send = UInt16()
