def handleTimeRequest(self, data): """ Respond to device with system time. """ t = Time() t.data = rospy.Time.now() data_buffer = StringIO.StringIO() t.serialize(data_buffer) self.send(TopicInfo.ID_TIME, data_buffer.getvalue()) self.lastsync = rospy.Time.now()
def handleTimeRequest(self, data): """ Respond to device with system time. """ t = Time() t.data = rospy.Time.now() data_buffer = StringIO.StringIO() t.serialize(data_buffer) self.send( TopicInfo.ID_TIME, data_buffer.getvalue() ) self.lastsync = rospy.Time.now()
def runSim(self, maxTime, incrTime, superRealTimeFactor): rospy.loginfo('=== RUN SIM (%.4f, %.4f, %.4f) ===' % (maxTime, incrTime, superRealTimeFactor)) rospy.loginfo('Unpausing simulation to unlock /clock') self.unpause_sim_cln(EmptyRequest()) rospy.loginfo('Resetting robot pose') #self.reset_sim_cln(EmptyRequest()) self.reset_sim_world_cln(EmptyRequest()) rospy.loginfo('Resetting policy at start of runSim') self.reset_policy_cln(ResetPolicyRequest(self.theta)) rospy.loginfo('Pausing simulation') self.pause_sim_cln(EmptyRequest()) sim_start_time = rospy.Time.now() time_msg = Time() time_msg.data = sim_start_time rospy.loginfo('Announcing new episode start time') self.new_episode_pub.publish(time_msg) latest_real_time = time.time() for i in xrange(int(math.ceil(float(maxTime) / incrTime))): t = float(i * incrTime) desired_time = sim_start_time + rospy.Duration(t) step_policy_req = StepPolicyRequest(desired_time) rospy.loginfo('StepPolicy(%.4f)' % (step_policy_req.desired_time).to_sec()) self.step_policy_cln(step_policy_req) step_sim_req = RunSimulationUntilTimeRequest(desired_time) rospy.loginfo('StepSimulator(%.4f)' % (step_sim_req.desired_time).to_sec()) self.step_simulator_cln(step_sim_req) # For now we will sleep for some time before stepping next; in reality we would go as fast as plant lets us sleep_dur = incrTime / superRealTimeFactor now = time.time() real_time_diff = now - latest_real_time if real_time_diff < sleep_dur: try: time.sleep(sleep_dur - real_time_diff) except KeyboardInterrupt: break latest_real_time = time.time() rospy.loginfo('Resetting policy at end of runSim') self.reset_policy_cln(ResetPolicyRequest(self.theta)) rospy.loginfo('Unpausing simulation to unlock /clock') self.unpause_sim_cln(EmptyRequest()) rospy.loginfo('runSim done!')
def timer_callback(self, data): self.time -= self.unit if self.time < 0: self.time = 0 msg = Time() msg.data = rospy.Time(secs=self.time) self.pub.publish(msg)
def handleTimeRequest(self, data): """ Respond to device with system time. """ t = Time() t.deserialize(data) if t.data.secs == 0 and t.data.nsecs == 0: t = Time() t.data = rospy.Time.now() data_buffer = StringIO.StringIO() t.serialize(data_buffer) self.send( TopicInfo.ID_TIME, data_buffer.getvalue() ) self.lastsync = rospy.Time.now() else: # TODO sync the time somehow self.lastsync = rospy.Time.now() pass
def dji_gps_position_cb(self, gps_msg): if self.gps_record == True: rostime = Time() rostime.data = rospy.get_rostime() self.gpsA3_bag.write('rostime', rostime) self.gpsA3_bag.write('geoPosition', gps_msg) if self.telemetry_save: if not self.telemetry_file_head: self.telemetry_file_writer.writerow({'Time': 'Time', 'Latitude':'Latitude', 'Longitude':'Longitude', 'Altitude':'Altitude', \ 'Laser_altitude':'Laser_altitude', 'Quat.x':'Quat.x', \ 'Quat.y':'Quat.y', 'Quat.z':'Quat.z', 'Quat.w':'Quat.w'}) self.telemetry_file_head = True else: self.telemetry_file_writer.writerow({'Time':rospy.get_rostime(), 'Latitude':gps_msg.latitude, 'Longitude':gps_msg.longitude, 'Altitude':gps_msg.altitude, \ 'Laser_altitude':self.current_laser_altitude, 'Quat.x':self.current_attitude.quaternion.x, \ 'Quat.y':self.current_attitude.quaternion.y, 'Quat.z':self.current_attitude.quaternion.z, 'Quat.w':self.current_attitude.quaternion.w})
def callback_dis(msg): global pub, models, count, pixels, obj_R, obj_L count += 1 if count == 1: pixels = np.append(pixels, np.array([msg.vector.x, msg.vector.y])) pixels = pixels.reshape(1, -1) else: pixels = np.append(pixels, np.array([msg.vector.x, msg.vector.y]).reshape(1, -1), axis=0) if pixels.size == 8: # {3 pixels : 6, 4 pixels : 8, 5 pixels: 10, 6 pixels : 12} clf = pickle.load(open(models[count], 'rb')) prediction = clf.predict(np.array([[distance.euclidean([msg.vector.x, msg.vector.y], obj_R), distance.euclidean([msg.vector.x, msg.vector.y], obj_L)]])) pred_msg = Bool() pred_msg.data = True if prediction == 1 else False rospy.loginfo('Predicted {} '.format(prediction)) pub.publish(pred_msg) prediction_time = Time() prediction_time.data = rospy.Time.now() prediction_time_pub.publish(prediction_time)
def talker(filename, topicName='PoseStamped'): pub = rospy.Publisher('iiwa/poseFromFile/' + topicName, PoseStamped, queue_size=1) time_pub = rospy.Publisher('iiwa/poseFromFile/receiveTime', Time, queue_size=1) #rosTime_pub = rospy.Publisher('poseFromFile/receiveRosTime', Time, queue_size=1) rospy.init_node('posePublisher', anonymous=True) # Without delay the first few values could not be received by the subscriber despite it had been # already registered. Apparently it takes some time until the connection is established after the # publisher (node) is created. time.sleep(0.5) rate = rospy.Rate(SAMPLE_RATE) reader = csv.reader(open(filename)) for line in reader: if rospy.is_shutdown(): break values = [float(x) for x in line] #rospy.loginfo(values) receiveTime = Time() #receiveRosTime = Time() receiveTime.data = rospy.Time.from_sec(time.time()) #receiveRosTime.data = rospy.Time.now() pose = PoseStamped() pose.header.frame_id = "/operator" pose.header.stamp = rospy.Time.now() pose.pose.position.x = values[0] pose.pose.position.y = values[1] pose.pose.position.z = values[2] pose.pose.orientation.x = values[3] pose.pose.orientation.y = values[4] pose.pose.orientation.z = values[5] pose.pose.orientation.w = values[6] pub.publish(pose) time_pub.publish(receiveTime) #rosTime_pub.publish(receiveRosTime) rate.sleep()
def rgb_camera_capture_function(self): try: time_now = datetime.datetime.now() print('Capturing image') file_path = gp.check_result( gp.gp_camera_capture(self.rgb_camera, gp.GP_CAPTURE_IMAGE)) filename = 'image_{0}.jpg'.format( time_now.strftime("%Y-%m-%d %H:%M:%S")) target = os.path.join(self.rgb_images_dir, filename) print('Copying image to', target) camera_file = gp.check_result( gp.gp_camera_file_get(self.rgb_camera, file_path.folder, file_path.name, gp.GP_FILE_TYPE_NORMAL)) gp.check_result(gp.gp_file_save(camera_file, target)) rostime = Time() rostime.data = rospy.get_rostime() self.rgb_file_writer.writerow({ 'RGB Image': filename, 'Time': rospy.get_rostime() }) except: rospy.logerr("PAL: Error in connection with RGB camera")
def runLive(self, maxTime): time.sleep( 0.5 ) # not sure why need this delay, but otherwise /aqua_rl/new_episode doesn't publish properly rospy.loginfo('=== RUN LIVE (%.4f) ===' % maxTime) rospy.loginfo('Resetting robot pose') #self.reset_sim_cln(EmptyRequest()) self.reset_sim_world_cln(EmptyRequest()) rospy.loginfo('Resetting policy at start of runLive') self.reset_policy_cln(ResetPolicyRequest(self.theta)) time_msg = Time() time_msg.data = rospy.Time.now() rospy.loginfo('Announcing new episode start time') self.new_episode_pub.publish(time_msg) rospy.loginfo('Unpausing simulation') self.unpause_sim_cln(EmptyRequest()) rospy.loginfo('Publishing run_live') self.run_live_pub.publish(EmptyMsg()) rospy.loginfo( 'Sleeping for %.2f seconds before stopping live episode' % maxTime) try: time.sleep(maxTime) except KeyboardInterrupt: pass rospy.loginfo('Resetting policy at end of runLive') self.reset_policy_cln(ResetPolicyRequest(self.theta)) rospy.loginfo('runLive done!')
def run(self): """ Forward recieved messages to appropriate publisher. """ data = '' while not rospy.is_shutdown(): if (rospy.Time.now() - self.lastsync).to_sec() > (self.timeout * 3): rospy.logerr("Lost sync with device, restarting...") self.requestTopics() self.lastsync = rospy.Time.now() flag = [0,0] flag[0] = self.port.read(1) if (flag[0] != '\xff'): continue flag[1] = self.port.read(1) if ( flag[1] != '\xff'): rospy.loginfo("Failed Packet Flags ") continue # topic id (2 bytes) header = self.port.read(4) if (len(header) != 4): #self.port.flushInput() continue topic_id, msg_length = struct.unpack("<hh", header) msg = self.port.read(msg_length) if (len(msg) != msg_length): rospy.loginfo("Packet Failed : Failed to read msg data") #self.port.flushInput() continue chk = self.port.read(1) checksum = sum(map(ord,header) ) + sum(map(ord, msg)) + ord(chk) if checksum%256 == 255: if topic_id == TopicInfo.ID_PUBLISHER: try: m = TopicInfo() m.deserialize(msg) self.senders[m.topic_id] = Publisher(m.topic_name, m.message_type) rospy.loginfo("Setup Publisher on %s [%s]" % (m.topic_name, m.message_type) ) except Exception as e: rospy.logerr("Failed to parse publisher: %s", e) elif topic_id == TopicInfo.ID_SUBSCRIBER: try: m = TopicInfo() m.deserialize(msg) self.receivers[m.topic_name] = [m.topic_id, Subscriber(m.topic_name, m.message_type, self)] rospy.loginfo("Setup Subscriber on %s [%s]" % (m.topic_name, m.message_type)) except Exception as e: rospy.logerr("Failed to parse subscriber. %s"%e) elif topic_id == TopicInfo.ID_SERVICE_SERVER: try: m = TopicInfo() m.deserialize(msg) self.senders[m.topic_id]=ServiceServer(m.topic_name, m.message_type, self) rospy.loginfo("Setup ServiceServer on %s [%s]"%(m.topic_name, m.message_type) ) except: rospy.logerr("Failed to parse service server") elif topic_id == TopicInfo.ID_SERVICE_CLIENT: pass elif topic_id == TopicInfo.ID_PARAMETER_REQUEST: self.handleParameterRequest(msg) elif topic_id == TopicInfo.ID_LOG: self.handleLogging(msg) elif topic_id == TopicInfo.ID_TIME: t = Time() t.data = rospy.Time.now() data_buffer = StringIO.StringIO() t.serialize(data_buffer) self.send( TopicInfo.ID_TIME, data_buffer.getvalue() ) self.lastsync = rospy.Time.now() elif topic_id >= 100: # TOPIC try: self.senders[topic_id].handlePacket(msg) except KeyError: rospy.logerr("Tried to publish before configured, topic id %d" % topic_id) else: rospy.logerr("Unrecognized command topic!") rospy.sleep(0.001)
def yuv_callback(self, yuv_frame): # Use OpenCV to convert the yuv frame to RGB info = yuv_frame.info( ) # the VideoFrame.info() dictionary contains some useful information such as the video resolution rospy.logdebug_throttle(10, "yuv_frame.info = " + str(info)) cv2_cvt_color_flag = { olympe.PDRAW_YUV_FORMAT_I420: cv2.COLOR_YUV2BGR_I420, olympe.PDRAW_YUV_FORMAT_NV12: cv2.COLOR_YUV2BGR_NV12, }[info["yuv"]["format"]] # convert pdraw YUV flag to OpenCV YUV flag cv2frame = cv2.cvtColor(yuv_frame.as_ndarray(), cv2_cvt_color_flag) # Publish image msg_image = self.bridge.cv2_to_imgmsg(cv2frame, "bgr8") self.pub_image.publish(msg_image) # yuv_frame.vmeta() returns a dictionary that contains additional metadata from the drone (GPS coordinates, battery percentage, ...) metadata = yuv_frame.vmeta() rospy.logdebug_throttle(10, "yuv_frame.vmeta = " + str(metadata)) if metadata[1] != None: header = Header() header.stamp = rospy.Time.now() header.frame_id = '/body' frame_timestamp = metadata[1][ 'frame_timestamp'] # timestamp [millisec] msg_time = Time() msg_time.data = frame_timestamp # secs = int(frame_timestamp//1e6), nsecs = int(frame_timestamp%1e6*1e3) self.pub_time.publish(msg_time) drone_quat = metadata[1]['drone_quat'] # attitude msg_attitude = QuaternionStamped() msg_attitude.header = header msg_attitude.quaternion = Quaternion(drone_quat['x'], -drone_quat['y'], -drone_quat['z'], drone_quat['w']) self.pub_attitude.publish(msg_attitude) location = metadata[1][ 'location'] # GPS location [500.0=not available] (decimal deg) msg_location = PointStamped() if location != {}: msg_location.header = header msg_location.header.frame_id = '/world' msg_location.point.x = location['latitude'] msg_location.point.y = location['longitude'] msg_location.point.z = location['altitude'] self.pub_location.publish(msg_location) ground_distance = metadata[1]['ground_distance'] # barometer (m) self.pub_height.publish(ground_distance) speed = metadata[1]['speed'] # opticalflow speed (m/s) msg_speed = Vector3Stamped() msg_speed.header = header msg_speed.header.frame_id = '/world' msg_speed.vector.x = speed['north'] msg_speed.vector.y = -speed['east'] msg_speed.vector.z = -speed['down'] self.pub_speed.publish(msg_speed) air_speed = metadata[1][ 'air_speed'] # air speed [-1=no data, > 0] (m/s) self.pub_air_speed.publish(air_speed) link_goodput = metadata[1][ 'link_goodput'] # throughput of the connection (b/s) self.pub_link_goodput.publish(link_goodput) link_quality = metadata[1]['link_quality'] # [0=bad, 5=good] self.pub_link_quality.publish(link_quality) wifi_rssi = metadata[1][ 'wifi_rssi'] # signal strength [-100=bad, 0=good] (dBm) self.pub_wifi_rssi.publish(wifi_rssi) battery_percentage = metadata[1][ 'battery_percentage'] # [0=empty, 100=full] self.pub_battery.publish(battery_percentage) state = metadata[1][ 'state'] # ['LANDED', 'MOTOR_RAMPING', 'TAKINGOFF', 'HOWERING', 'FLYING', 'LANDING', 'EMERGENCY'] self.pub_state.publish(state) mode = metadata[1][ 'mode'] # ['MANUAL', 'RETURN_HOME', 'FLIGHT_PLAN', 'TRACKING', 'FOLLOW_ME', 'MOVE_TO'] self.pub_mode.publish(mode) msg_pose = PoseStamped() msg_pose.header = header msg_pose.pose.position = msg_location.point msg_pose.pose.position.z = ground_distance msg_pose.pose.orientation = msg_attitude.quaternion self.pub_pose.publish(msg_pose) Rot = R.from_quat([ drone_quat['x'], -drone_quat['y'], -drone_quat['z'], drone_quat['w'] ]) drone_rpy = Rot.as_euler('xyz') msg_odometry = Odometry() msg_odometry.header = header msg_odometry.child_frame_id = '/body' msg_odometry.pose.pose = msg_pose.pose msg_odometry.twist.twist.linear.x = math.cos( drone_rpy[2]) * msg_speed.vector.x + math.sin( drone_rpy[2]) * msg_speed.vector.y msg_odometry.twist.twist.linear.y = -math.sin( drone_rpy[2]) * msg_speed.vector.x + math.cos( drone_rpy[2]) * msg_speed.vector.y msg_odometry.twist.twist.linear.z = msg_speed.vector.z self.pub_odometry.publish(msg_odometry) # log battery percentage if battery_percentage >= 30: if battery_percentage % 10 == 0: rospy.loginfo_throttle( 100, "Battery level: " + str(battery_percentage) + "%") else: if battery_percentage >= 20: rospy.logwarn_throttle( 10, "Low battery: " + str(battery_percentage) + "%") else: if battery_percentage >= 10: rospy.logerr_throttle( 1, "Critical battery: " + str(battery_percentage) + "%") else: rospy.logfatal_throttle( 0.1, "Empty battery: " + str(battery_percentage) + "%") # log signal strength if wifi_rssi <= -60: if wifi_rssi >= -70: rospy.loginfo_throttle( 100, "Signal strength: " + str(wifi_rssi) + "dBm") else: if wifi_rssi >= -80: rospy.logwarn_throttle( 10, "Weak signal: " + str(wifi_rssi) + "dBm") else: if wifi_rssi >= -90: rospy.logerr_throttle( 1, "Unreliable signal:" + str(wifi_rssi) + "dBm") else: rospy.logfatal_throttle( 0.1, "Unusable signal: " + str(wifi_rssi) + "dBm") else: rospy.logwarn("Packet lost!")
time_elapsed = Time() rate = rospy.Rate(10) while not rospy.is_shutdown(): curr_time = rospy.Time.now() if curr_time.to_sec() - node.start_time.to_sec( ) > 1e-4 and not node.use_model and curr_time.to_sec( ) - node.start_time.to_sec() >= 30: # 30 seconds node.dyn_reconfig_client.update_configuration({"use_model": True}) node.use_model = True print("------- Using model -------") if curr_time.to_sec() - node.start_time.to_sec( ) > 1e-4 and curr_time.to_sec() - node.start_time.to_sec() >= 120: node.bag.close() os.system( "rosnode kill -a && sleep 5 && kill -2 $( ps -C roslaunch -o pid= ) && sleep 2 && kill -2 $( ps -C roscore -o pid= )" ) os.system("tmux kill-session") break rate.sleep() node.bag.write("/downward/vio/odometry", node.odom_msg) node.bag.write("/reference_vis", node.ref_msg) time_elapsed.data = curr_time - node.start_time node.bag.write("/time_elapsed", time_elapsed) print("Time elapsed", time_elapsed.data.to_sec())
def prepareMessages(N_messages): #print('Starting prepareMessages...') messages = [] temp = [] bridge = CvBridge() for N in range(N_messages): # Prepare messages: msg_facialRecognitionMessage = String() facialRecognitionMessage = str(N) + ' -- ' + str(time.time()) msg_facialRecognitionMessage.data = facialRecognitionMessage temp.append(msg_facialRecognitionMessage) msg_fails = Int32() fails = int(1000*np.random.random()) msg_fails.data = fails temp.append(msg_fails) msg_firstTime = Bool() firstTime = True msg_firstTime.data = firstTime temp.append(msg_firstTime) msg_session = Time() session = rospy.Time.now() msg_session.data = session temp.append(msg_session) msg_sessionSet = Bool() sessionSet = True msg_sessionSet.data = sessionSet temp.append(msg_sessionSet) msg_success = Int32() success = int(1000*np.random.random()) msg_success.data = success temp.append(msg_success) msg_userTag = String() userTag = 'Employee_FR' msg_userTag.data = userTag temp.append(msg_userTag) msg_username = String() username = '******' + str(N) msg_username.data = username temp.append(msg_username) msg_userPhoto = CompressedImage() userPhoto = cv2.imread('testImg.jpg') image_message = bridge.cv2_to_compressed_imgmsg(userPhoto, dst_format='jpg') msg_userPhoto = image_message temp.append(msg_userPhoto) msg_viewFlow = String() viewFlow = ''.join(random.choice(string.ascii_uppercase + string.digits) for _ in range(20)) msg_viewFlow.data = viewFlow temp.append(msg_viewFlow) msg_actualPoleHeightPercentage = Float32() actualPoleHeightPercentage = 100*np.random.random() msg_actualPoleHeightPercentage.data = actualPoleHeightPercentage temp.append(msg_actualPoleHeightPercentage) msg_actualkickStandState = Int32() actualkickStandState = int(4*np.random.random()) msg_actualkickStandState.data = actualkickStandState temp.append(msg_actualkickStandState) msg_batteryFullyCharged = Bool() batteryFullyCharged = False msg_batteryFullyCharged.data = batteryFullyCharged temp.append(msg_batteryFullyCharged) msg_batteryPercentage = Float32() batteryPercentage = 100*np.random.random() msg_batteryPercentage.data = batteryPercentage temp.append(msg_batteryPercentage) msg_firmwareVersion = String() firmwareVersion = ''.join(random.choice(string.ascii_uppercase + string.digits) for _ in range(10)) msg_firmwareVersion.data = firmwareVersion temp.append(msg_firmwareVersion) msg_location = NavSatFix() msg_location.latitude = 180*np.random.random() msg_location.longitude = 180*np.random.random() temp.append(msg_location) msg_motionDetection = Bool() motionDetection = False msg_motionDetection.data = motionDetection temp.append(msg_motionDetection) msg_serial = String() serial = ''.join(random.choice(string.ascii_uppercase + string.digits) for _ in range(15)) msg_serial.data = serial temp.append(msg_serial) msg_status = Bool() status = True msg_status.data = status temp.append(msg_status) msg_camera = CompressedImage() camera = cv2.imread('testImg.jpg') image_message = bridge.cv2_to_compressed_imgmsg(camera, dst_format='jpg') msg_camera = image_message temp.append(msg_camera) messages.append(temp) return messages
#!/usr/bin/env python from std_msgs.msg import Time import rospy pub = rospy.Publisher('/timer', Time, queue_size=1) rospy.init_node("time", anonymous=True) rate = rospy.Rate(5) #hz test = Time() while not rospy.is_shutdown(): test.data = rospy.Time.now() pub.publish(test) rate.sleep()
def run(self): """ Forward recieved messages to appropriate publisher. """ data = '' while not rospy.is_shutdown(): if (rospy.Time.now() - self.lastsync).to_sec() > (self.timeout * 3): rospy.logerr("Lost sync with device, restarting...") self.requestTopics() self.lastsync = rospy.Time.now() flag = [0,0] flag[0] = self.port.read(1) if (flag[0] != '\xff'): continue flag[1] = self.port.read(1) if ( flag[1] != '\xff'): rospy.loginfo("Failed Packet Flags ") continue # topic id (2 bytes) header = self.port.read(4) if (len(header) != 4): #self.port.flushInput() continue topic_id, msg_length = struct.unpack("<hh", header) msg = self.port.read(msg_length) if (len(msg) != msg_length): rospy.loginfo("Packet Failed : Failed to read msg data") #self.port.flushInput() continue chk = self.port.read(1) checksum = sum(map(ord,header) ) + sum(map(ord, msg)) + ord(chk) if checksum%256 == 255: if topic_id == TopicInfo.ID_PUBLISHER: try: m = TopicInfo() m.deserialize(msg) self.senders[m.topic_id] = Publisher(m.topic_name, m.message_type) rospy.loginfo("Setup Publisher on %s [%s]" % (m.topic_name, m.message_type) ) except Exception as e: rospy.logerr("Failed to parse publisher: %s", e) elif topic_id == TopicInfo.ID_SUBSCRIBER: try: m = TopicInfo() m.deserialize(msg) self.receivers[m.topic_name] = [m.topic_id, Subscriber(m.topic_name, m.message_type, self)] rospy.loginfo("Setup Subscriber on %s [%s]" % (m.topic_name, m.message_type)) except Exception as e: rospy.logerr("Failed to parse subscriber. %s"%e) elif topic_id == TopicInfo.ID_SERVICE_SERVER: try: m = TopicInfo() m.deserialize(msg) service = ServiceServer(m.topic_name, m.message_type, self) self.receivers[m.topic_name] = [m.topic_id, service] self.senders[m.topic_id] = service rospy.loginfo("Setup ServiceServer on %s [%s]"%(m.topic_name, m.message_type) ) except: rospy.logerr("Failed to parse service server") elif topic_id == TopicInfo.ID_SERVICE_CLIENT: pass elif topic_id == TopicInfo.ID_PARAMETER_REQUEST: self.handleParameterRequest(msg) elif topic_id == TopicInfo.ID_LOG: self.handleLogging(msg) elif topic_id == TopicInfo.ID_TIME: t = Time() t.data = rospy.Time.now() data_buffer = StringIO.StringIO() t.serialize(data_buffer) self.send( TopicInfo.ID_TIME, data_buffer.getvalue() ) self.lastsync = rospy.Time.now() elif topic_id >= 100: # TOPIC try: self.senders[topic_id].handlePacket(msg) except KeyError: rospy.logerr("Tried to publish before configured, topic id %d" % topic_id) else: rospy.logerr("Unrecognized command topic!") rospy.sleep(0.001)
import rospy from std_msgs.msg import Bool, Time import RPi.GPIO as GPIO if __name__ == "__main__": rospy.init_node('camera_trigger_node') trigger_pub = rospy.Publisher('trigger', Time, queue_size=1) trigger_pin = rospy.get_param('~trigger_pin', 26) trigger_rate = rospy.get_param('~trigger_rate', 2) # in Hz. Defaults at 2Hz GPIO.setmode(GPIO.BCM) GPIO.setup(trigger_pin, GPIO.OUT) r = rospy.Rate(trigger_rate) # hz pulse_width = 0.05 while not rospy.is_shutdown(): # Write output high GPIO.output(trigger_pin, GPIO.HIGH) # Sleep time.sleep(pulse_width) # Write low GPIO.output(trigger_pin, GPIO.LOW) tmsg = Time() tmsg.data = rospy.get_rostime() trigger_pub.publish(tmsg) r.sleep() GPIO.cleanup(trigger_pin)
def openpose_callback(msg): global pub, prediction_pixel_time, prediction_pixel_pub, time_pub, start_game, start, end, start_std, count, x_prev, y_prev, time_prev, start_time, print_time if start_game: try: x = msg.human_list[0].body_key_points_with_prob[4].x y = msg.human_list[0].body_key_points_with_prob[4].y except: return time = msg.header.stamp count += 1 if x != 0 and y != 0: if len(x_prev) == 0: x_prev.append(x) y_prev.append(y) time_prev.append(time) if start: x_motion.append(x) y_motion.append(y) time_motion.append(time) pixel = Vector3Stamped() pixel.vector.x = x pixel.vector.y = y pixel.header.stamp = msg.header.stamp pub.publish(pixel) else: if len(x_prev) == 10: start_std = True if abs(x - x_prev[-1]) < 30 and abs(y - y_prev[-1]) < 30: if start: if len(x_prev) == 5: del x_prev[0], y_prev[0], time_prev[0] elif len(x_prev) == 10: del x_prev[0], y_prev[0], time_prev[0] x_prev.append(x) y_prev.append(y) time_prev.append(time) if start_std: std_x = np.std(x_prev) std_y = np.std(y_prev) std_total = np.sqrt(std_x**2 + std_y**2) if (not start) and (std_total > 1.5): print("Motion started at sample {}".format(count)) start_time = rospy.Time.now().to_sec() x_prev, y_prev, time_prev = [], [], [] start = True if start: if std_total > 10: end = True if std_total <= 10 and end: if print_time: end_time = rospy.Time.now().to_sec() # rospy.loginfo("Human motion duration: {}".format((end_time - start_time)*1000)) # rospy.loginfo("Human motion duration from the prediction onwards: {}".format((end_time - prediction_pixel_time.data.to_sec())*1000)) print("Motion ended at sample {}".format( count)) human_time = Time() human_time.data = rospy.Time.now() time_pub.publish(human_time) print_time = False return x_motion.append(x) y_motion.append(y) if len(x_motion) == 4: prediction_pixel_time = Time() prediction_pixel_time.data = rospy.Time.now() prediction_pixel_pub.publish( prediction_pixel_time) time_motion.append(time) pixel = Vector3Stamped() pixel.vector.x = x pixel.vector.y = y pixel.header.stamp = msg.header.stamp pub.publish(pixel)
def usb_cam_cb(self, image_msg): if self.webcam_record == True: rostime = Time() rostime.data = rospy.get_rostime() self.webcam_bag.write('rostime', rostime) self.webcam_bag.write('webcamImage', image_msg)
def talker(filename, jointTopicName='JointPosition', poseTopicName='PoseStampedRelative'): pub = rospy.Publisher('jointAnglesFromFile/' + jointTopicName, JointPosition, queue_size=10) pose_pub = rospy.Publisher('poseFromFile/' + poseTopicName, PoseStamped, queue_size=10) time_pub = rospy.Publisher('iiwa/poseFromFile/receiveTime', Time, queue_size=1) rospy.init_node('jointPosPublisher', anonymous=True) time.sleep(0.5) rate = rospy.Rate(SAMPLE_RATE) reader = csv.reader(open(filename)) reader_list = list(reader) for i in range(0, NUM_OF_ITERATIONS): if i % 2 == 0: for line in reader_list: if rospy.is_shutdown(): break values = [float(x) for x in line] receiveTime = Time() receiveTime.data = rospy.Time.from_sec(time.time()) #rospy.loginfo(value) joints = JointPosition() joints.header.frame_id = "/base_link" joints.header.stamp = rospy.Time.now() joints.position.a1 = values[0] joints.position.a2 = values[1] joints.position.a3 = values[2] joints.position.a4 = values[3] joints.position.a5 = values[4] joints.position.a6 = values[5] joints.position.a7 = values[6] pose = PoseStamped() pose.header.frame_id = "/world" pose.header.stamp = rospy.Time.now() pose.pose.position.x = values[7] pose.pose.position.y = values[8] pose.pose.position.z = values[9] pose.pose.orientation.x = values[10] pose.pose.orientation.y = values[11] pose.pose.orientation.z = values[12] pose.pose.orientation.w = values[13] pub.publish(joints) pose_pub.publish(pose) time_pub.publish(receiveTime) rate.sleep() else: for line in reversed(reader_list): if rospy.is_shutdown(): break values = [float(x) for x in line] #rospy.loginfo(value) joints = JointPosition() joints.header.frame_id = "/base_link" joints.header.stamp = rospy.Time.now() joints.position.a1 = values[0] joints.position.a2 = values[1] joints.position.a3 = values[2] joints.position.a4 = values[3] joints.position.a5 = values[4] joints.position.a6 = values[5] joints.position.a7 = values[6] pose = PoseStamped() pose.header.frame_id = "/world" pose.header.stamp = rospy.Time.now() pose.pose.position.x = values[7] pose.pose.position.y = values[8] pose.pose.position.z = values[9] pose.pose.orientation.x = values[10] pose.pose.orientation.y = values[11] pose.pose.orientation.z = values[12] pose.pose.orientation.w = values[13] pub.publish(joints) pose_pub.publish(pose) rate.sleep() time.sleep(0.5)