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()