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)
