def __init__(self, accFrame, gyroFrame, outFrame):
self.omega_out = np.array([0., 0., 0.])
self.omega_cov_out = np.zeros([3,3])
self.omega_dot = np.array([0., 0., 0.])
self.a_out = np.array([0., 0., 0.])
self.a_cov_out = np.zeros([3,3])
self.rot_out = np.array([0., 0., 0.])
self.tf_listener = tf.TransformListener()
self.out_to_in_trans = np.array([0., 0., 0.])
self.in_orientation = np.array([0.,0.,0.,1.])
self.out_to_in_rot = np.zeros([3,3]) # turns to eye(3) or proper format after receiving first tf frame.
self.out_to_in_rot_q = np.array([0.,0.,0.,1.])
self.in_to_gyro_rot = np.zeros([3,3]) # turns to eye(3) or proper format after receiving first tf frame.
self.in_to_gyro_rot_q = np.array([0.,0.,0.,1.])
self.omega_last = np.array([0., 0., 0.])
self.a_in = np.array([0., 0., 0.])
self.a_cov_in = np.zeros([3,3])
self.time_omega_old = Time()
self.time_acc_old = Time()
# in_frame should be the accelerometer frame. Gyro data shall be transformed to the accel frame accordingly.
self.gyroFrame = gyroFrame
self.inFrame = accFrame
self.outFrame = outFrame
self.pub_transformed = rospy.Publisher("imu_in_"+outFrame, Imu, queue_size=1)
def _parse_plan(self):
# TODO this parser only accepts a set of goto commands
"""A plan has been requested by neptus to start. Parse the plan here and send to controller."""
geopoints = []
speeds = []
for maneuver in self._current_plan.plan_spec.maneuvers:
geopoints.append(
GeoPoint(maneuver.maneuver.lat * 180.0 / np.pi,
maneuver.maneuver.lon * 180.0 / np.pi,
maneuver.maneuver.z))
speeds.append(maneuver.maneuver.speed)
if self._datum is not None:
geopoints.append(self._datum)
req = ConvertGeoPointsRequest()
req.geopoints = geopoints
points = self._geo_converter.call(req).utmpoints
wps = []
# If datum is available, then reference frame is ENU World
if self._datum is not None:
for point, speed in zip(points[:-1], speeds):
wp = Waypoint()
wp.point = Point(point.x - points[-1].x,
point.y - points[-1].y,
-point.z - points[-1].z)
wp.header.stamp = rospy.Time.now()
wp.header.frame_id = "world"
wp.radius_of_acceptance = 3.0
wp.max_forward_speed = speed
wp.use_fixed_heading = False
wp.heading_offset = 0.0
wps.append(wp)
# If datum is not given, then reference frame is ENU UTM TODO: LOOKUP TRANSFORM
else:
for point, speed in zip(points, speeds):
wp = Waypoint()
wp.point.x = point.x
wp.point.y = point.y
wp.point.z = -point.z
wp.header.stamp = rospy.Time.now()
wp.header.frame_id = "utm"
wp.radius_of_acceptance = 3.0
wp.max_forward_speed = speed
wp.use_fixed_heading = False
wp.heading_offset = 0.0
wps.append(wp)
self.STATE = PlanControlState.READY
req = InitWaypointSetRequest()
req.start_time = Time(rospy.Time.from_sec(0))
req.start_now = True
req.waypoints = wps
req.max_forward_speed = max(speeds)
req.interpolator.data = "lipb"
req.heading_offset = 0
self._waypoint_setter.wait_for_service()
if self._waypoint_setter(Time(rospy.Time.from_sec(0.0)), True, wps,
max(speeds), 0.0, String("lipb")):
self.STATE = PlanControlState.EXECUTING
else:
self.STATE = PlanControlState.FAILURE
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 timeTimerCallback(self, event):
with self.lock:
if self.last_send_time:
self.last_send_time_pub.publish(self.last_send_time)
else:
self.last_send_time_pub.publish(Time(data=rospy.Time(0)))
if self.last_input_received_time:
self.last_input_received_time_pub.publish(
self.last_input_received_time)
else:
self.last_input_received_time_pub.publish(
Time(data=rospy.Time(0)))
def update(self, pressed, timestamp):
if not pressed and self.last_state: # only when released
rospy.logdebug(
"MagicButtonRelay : stamped event for button '%s' published at '%s'"
% (self.button_id, self.publisher.name))
self.publisher.publish(Time(timestamp))
self.last_state = pressed
def iso8601_to_rostime(iso):
"""Converts ISO 8601 time to ROS Time.
Args:
iso: ISO 8601 encoded string.
Returns:
std_msgs/Time.
"""
# Convert to datetime in UTC.
t = dateutil.parser.parse(iso)
if not t.utcoffset():
t = t.replace(tzinfo=tzutc())
# Convert to time from epoch in UTC.
epoch = datetime.utcfromtimestamp(0)
epoch = epoch.replace(tzinfo=tzutc())
dt = t - epoch
# Create ROS message.
time = Time()
time.data.secs = int(dt.total_seconds())
time.data.nsecs = dt.microseconds * 1000
return time
def execute(self, userdata):
target_waypoint_msg = WaypointMsg()
target_waypoint_msg = userdata.target_waypoint.to_message()
print 'waiting for go to waypoint server'
rospy.wait_for_service('anahita/go_to')
go_to = rospy.ServiceProxy('anahita/go_to', GoTo)
start_time = Time()
start_time.data.secs = rospy.get_rostime().to_sec()
start_time.data.nsecs = rospy.get_rostime().to_nsec()
interpolator = String()
interpolator.data = 'cubic_interpolator'
print 'adding waypoints....'
resp = init_waypoint_set(max_forward_speed = 0.5,
interpolator = interpolator,
waypoint = target_waypoint_msg)
then = rospy.get_time()
while not rospy.is_shutdown():
now = rospy.get_time()
dt = now - then
dist = calc_dist(self._pose, target_waypoint_msg)
if dist < self._threshold:
return 'success'
if dt > self._timeout:
return 'timeout'
if dist > self._cutoff:
return 'failed'
def test_dictionary_with_time(self):
from std_msgs.msg import Time
now_time = rospy.Time.now()
expected_message = Time(data=now_time)
dictionary = {'data': {'secs': now_time.secs, 'nsecs': now_time.nsecs}}
message = message_converter.convert_dictionary_to_ros_message(
'std_msgs/Time', dictionary)
self.assertEqual(message, expected_message)
def transform_callback(self, data, msg_type):
""" ROS callback.
"""
a = time.time()
self.num_messages_received += 1
self.lastbeat = time.time()
# Get frame IDs of the objects to which the ROS messages are referred.
node_id0 = data.header.frame_id
if msg_type == "AprilTagDetection":
node_id1 = str(data.tag_id)
pose_error = float(data.pose_error) * 10**8 / 37.0
# self.pose_errors.append(pose_error)
# var = np.var(self.pose_errors)
# mean = np.mean(self.pose_errors)
# print("pose error : mean %f, var %f" % (mean, var))
elif msg_type == "TransformStamped":
node_id1 = data.child_frame_id
else:
raise Exception(
"Transform callback received unsupported msg_type %s" %
msg_type)
# Convert the frame IDs to the right format.
node_id0 = self.filter_name(node_id0)
node_id1 = self.filter_name(node_id1)
# Ignore messages from one watchtower to another watchtower (i.e.,
# odometry messages between watchtowers).
is_from_watchtower = False
if (node_id0.startswith("watchtower")):
is_from_watchtower = True
if (node_id1.startswith("watchtower")):
# print(data)
return 0
# Create translation vector.
header_time = Time(data.header.stamp)
time_stamp = header_time.data.secs + header_time.data.nsecs * 10**(-9)
if self.first_time_stamp == -1:
self.first_time_stamp = time_stamp
self.resampler.signal_reference_time_stamp(self.first_time_stamp)
if (node_id1 == node_id0):
# Same ID: odometry message, e.g. the same Duckiebot sending
# odometry information at different instances in time.
self.handle_odometry_message(node_id1, data, time_stamp)
elif (is_from_watchtower):
# Tag detected by a watchtower.
self.handle_watchtower_message(node_id0, node_id1, data,
time_stamp, pose_error)
else:
# Tag detected by a Duckiebot.
self.handle_duckiebot_message(node_id0, node_id1, data, time_stamp,
pose_error)
b = time.time()
self.callback_times.append(b - a)
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 test_ros_message_with_time(self):
from std_msgs.msg import Time
rospy.init_node('time_node')
now_time = rospy.Time.now()
expected_dictionary = {
'data': { 'secs': now_time.secs, 'nsecs': now_time.nsecs }
}
message = Time(data=now_time)
dictionary = message_converter.convert_ros_message_to_dictionary(message)
self.assertEqual(dictionary, expected_dictionary)
def transform_callback(self, data):
""" ROS callback.
"""
self.num_messages_received += 1
# Get frame IDs of the objects to which the ROS messages are referred.
id0 = data.header.frame_id
id1 = data.child_frame_id
# Convert the frame IDs to the right format.
id0 = self.filter_name(id0)
id1 = self.filter_name(id1)
# Ignore messages from one watchtower to another watchtower (i.e.,
# odometry messages between watchtowers). TODO: check if we can avoid
# sending these messages.
is_from_watchtower = False
if (id0.startswith("watchtower")):
is_from_watchtower = True
if (id1.startswith("watchtower")):
# print(data)
return 0
# Create translation vector.
t = [
data.transform.translation.x, data.transform.translation.y,
data.transform.translation.z
]
# Create rotation matrix. NOTE: in Pygeometry, quaternion is
# the (w, x, y, z) form.
q = [
data.transform.rotation.w, data.transform.rotation.x,
data.transform.rotation.y, data.transform.rotation.z
]
M = g.rotations.rotation_from_quaternion(np.array(q))
# Verify that the rotation is a proper rotation.
det = np.linalg.det(M)
if (det < 0):
print("det is %f" % det)
# Obtain complete transform and use it to add vertices in the graph.
transform = g2o.Isometry3d(M, t)
time = Time(data.header.stamp)
time_stamp = time.data.secs + time.data.nsecs * 10**(-9)
if (id1 == id0):
# Same ID: odometry message, e.g. the same Duckiebot sending
# odometry information at different instances in time.
self.handle_odometry_message(id1, transform, time_stamp)
elif (is_from_watchtower):
# Tag detected by a watchtower.
self.handle_watchtower_message(id0, id1, transform, time_stamp)
else:
# Tag detected by a Duckiebot.
self.handle_duckiebot_message(id0, id1, transform, time_stamp)
def parse_request(mis, utm_frame_id="utm", radius_of_acceptance=5.0):
with open(mis, "r") as f:
while not f.readline().startswith("START"):
pass
geopoints = []
headings = []
speeds = []
line = f.readline().rstrip()
while not line.startswith("END"):
values = line.split(";")
values = [value.strip() for value in values]
params = parse_iver_format(values[5])
zvar = -float(params[0][1:]) * 0.3048 if params[0][
0] == 'D' else float(params[0][1:]) * 0.3048
geopoints.append(GeoPoint(float(values[1]), float(values[2]),
zvar))
headings.append(float(values[4]))
speeds.append(float(params[-1][1:]) * 0.5144)
line = f.readline().rstrip()
# while not f.readline().startswith("START VEHICLE"):
# pass
# line = f.readline().rstrip()
# while not line.startswith("END VEHICLE"):
# if line.startswith("UVC=WPRadius"):
# roas.append(float(line.split("=")[-1]))
# break
req = ConvertGeoPointsRequest()
req.geopoints = geopoints
p = rospy.ServiceProxy("convert_points", ConvertGeoPoints)
p.wait_for_service()
res = p(req)
wps = []
for point, heading, speed in zip(res.utmpoints, headings, speeds):
wp = Waypoint()
wp.point.x = point.x
wp.point.y = point.y
wp.point.z = point.z
wp.header.stamp = rospy.Time.now()
wp.header.frame_id = utm_frame_id
wp.radius_of_acceptance = radius_of_acceptance
wp.max_forward_speed = speed
wp.use_fixed_heading = False
wp.heading_offset = 0.0
wps.append(wp)
req = InitWaypointSetRequest()
req.start_time = Time(rospy.Time.from_sec(0))
req.start_now = True
req.waypoints = wps
req.max_forward_speed = max(speeds)
req.interpolator = String("lipb")
req.heading_offset = 0
req.max_forward_speed = max(speeds)
req.waypoints = wps
return req
def test_ros_message_with_time(self):
from std_msgs.msg import Time
from time import time
now_time = rospy.Time(time())
expected_dictionary = {
'data': { 'secs': now_time.secs, 'nsecs': now_time.nsecs }
}
message = Time(data=now_time)
message = serialize_deserialize(message)
dictionary = message_converter.convert_ros_message_to_dictionary(message)
self.assertEqual(dictionary, expected_dictionary)
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 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 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 handleTimeCal(req):
global local_time
# there may be a case that user send time request even before "local_time" topic receive data
if local_time != None:
remote = Time()
# list of choise
if req.location == "Japan":
# time differece here is 11 hour
remote = local_time - rospy.Duration(hourToSec(11))
elif req.location == "Australia":
# time differece here is 10 hour
remote = local_time - rospy.Duration(hourToSec(10))
rospy.loginfo("There is a request coming from Kent at " + str(datetime.fromtimestamp(local_time.to_sec())))
#return result back to client
return timeserverResponse(remote)
def _send_goal(self, req):
res = InitWaypointSetResponse()
res.success = False
try:
self._mode_client.wait_for_service(rospy.Duration(5))
mode_req = SetControlModeRequest()
mode_req.mode.mode = mode_req.mode.LOSGUIDANCE
self._mode_client(mode_req)
self._action_client.wait_for_server(rospy.Duration(5))
goal = FollowWaypointsGoal()
goal.waypoints.header = Header(0, rospy.Time.now(), "utm")
goal.waypoints.start_time = Time(goal.waypoints.header.stamp)
goal.waypoints.waypoints = req.waypoints
self._marker_pub.publish(goal.waypoints)
self._action_client.send_goal(goal, self._handle_done,
self._handle_active,
self._handle_feedback)
res.success = True
return res
except Exception as e:
rospy.logerr("{} | {}".format(rospy.get_name(), e.message))
return res
def __init__(self):
self._pos_token_glob_x = []
self._pos_token_glob_y = []
self.blob_sub = rospy.Subscriber('block_data', PixyData,
self.blobb_callback)
self._blob_y = 0.0
self._blob_x = 0.0
self._last_stamp = Time()
# self._pose_pub_sub = rospy.Subscriber('pose', PoseStamped, self.pose_pub_callback)
self._pose_pub_sub = rospy.Subscriber('robot_pose', Pose,
self.pose_pub_callback)
self._current_pose_pub = None
self._current_x_pub = None
self._current_y_pub = None
self._current_orientation_pub = None
self._phi_pub = None
print 'wait pose'
# rospy.wait_for_message('pose', PoseStamped)
rospy.wait_for_message('robot_pose', Pose)
print 'got pose'
self._found_x = []
self._found_y = []
self._interrupt_pub = rospy.Subscriber('check_token', CorrectPosSrv,
self.interrupt_callback)
msg = rospy.wait_for_message("map", OccupancyGrid)
meta_data = msg.info
self._offset_x = meta_data.origin.position.x
self._offset_y = meta_data.origin.position.y
self._resolution = meta_data.resolution
self._tl = tf.TransformListener()
self._turtlebot_pub = rospy.Publisher('cmd_vel', Twist, queue_size=10)
self._move_base_client = actionlib.SimpleActionClient(
'move_base', MoveBaseAction)
self._move_base_client.wait_for_server()
self._current_goal_x = 0.0
self._current_goal_y = 0.0
print 'init finished'
def _parse_plan(self):
# TODO this parser only accepts a set of goto commands
"""A plan has been requested by neptus to start. Parse the plan here and send to controller."""
geopoints = []
speeds = []
is_depths = []
for maneuver in self._current_plan.plan_spec.maneuvers:
geopoints.append(
GeoPoint(maneuver.maneuver.lat * 180.0 / np.pi,
maneuver.maneuver.lon * 180.0 / np.pi,
maneuver.maneuver.z))
speeds.append(maneuver.maneuver.speed)
is_depths.append(maneuver.maneuver.z_units == 1)
req = ConvertGeoPointsRequest()
req.geopoints = geopoints
points = self._geo_converter.call(req).utmpoints
wps = []
# If datum is available, then reference frame is ENU World
for point, speed, is_depth in zip(points, speeds, is_depths):
wp = Waypoint()
wp.point.x = point.x
wp.point.y = point.y
wp.point.z = -abs(point.z) if is_depth else abs(point.z)
wp.header.stamp = rospy.Time.now()
wp.header.frame_id = "utm"
wp.radius_of_acceptance = 1.0
wp.max_forward_speed = speed
wp.use_fixed_heading = False
wp.heading_offset = 0.0
wps.append(wp)
req = InitWaypointSetRequest()
req.start_time = Time(rospy.Time.from_sec(0))
req.start_now = True
req.waypoints = wps
req.max_forward_speed = max(speeds)
req.interpolator.data = "lipb"
req.heading_offset = 0
self._send_goal(req)
def odom_cb(self, odom_msg):
if self.start_time == 0:
self.start_time = rospy.Time.now()
self.odom_msg = odom_msg
def ref_cb(self, odom_msg):
self.ref_msg = odom_msg
if __name__ == '__main__':
rospy.init_node('bag_data_node')
node = BagDataNode()
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()
if params['n_points'] <= 2:
raise rospy.ROSException('Number of points must be at least 2')
if params['max_forward_speed'] <= 0:
raise rospy.ROSException('Velocity limit must be positive')
try:
rospy.wait_for_service('start_helical_trajectory', timeout=2)
except rospy.ROSException:
raise rospy.ROSException('Service not available! Closing node...')
try:
traj_gen = rospy.ServiceProxy('start_helical_trajectory',
InitHelicalTrajectory)
except rospy.ServiceException, e:
raise rospy.ROSException('Service call failed, error=' + e)
print 'Generating trajectory that starts at t=%fs' % start_time
success = traj_gen(
Time(rospy.Time(start_time)), start_now, params['radius'],
Point(params['center'][0], params['center'][1],
params['center'][2]), False, 0.0, params['n_points'],
params['heading_offset'] * pi / 180, params['max_forward_speed'],
params['duration'], params['n_turns'], params['delta_z'])
if success:
print 'Trajectory successfully generated!'
else:
print 'Failed'
def __init__(self):
# ad-hoc solution for separate publishing of wheel data
self.xyyaw = Point()
self.odom_msg = Odometry()
self.fl_str = 0
self.fr_str = 0
self.rl_str = 0
self.rr_str = 0
self.avg_vel = 0
self.curv = 0
self.msg_state = [0, 0, 0, 0, 0, 0]
self.point_x = list()
self.point_y = list()
self.est_yaw = 0
self.yaw_first = 0
self.count = 30
# odometry info ---------------------
self.last_t = time.time()
self.last_t2 = time.time()
self.x_global = 0
self.y_global = 0
self.last_omega = 0
self.last_v = 0
self.yaw_global = 0
self.yaw_old = 0
self.angle_buffer = []
self.euler = []
self.time_stamp = Time()
self.init_flag = True
# self.sub_fl_str_ = rospy.Subscriber("/communication/dspace/flwheel_steering", Float32, self.cb1, queue_size=1)
# self.sub_fr_str_ = rospy.Subscriber("/communication/dspace/frwheel_steering", Float32, self.cb2, queue_size=1)
# self.sub_rl_str_ = rospy.Subscriber("/communication/dspace/rlwheel_steering", Float32, self.cb3, queue_size=1)
# self.sub_rr_str_ = rospy.Subscriber("/communication/dspace/rrwheel_steering", Float32, self.cb4, queue_size=1)
# self.sub_avg_vel_ = rospy.Subscriber("/communication/dspace/avg_speed", Float32, self.cb5, queue_size=1)
# self.sub_curv = rospy.Subscriber("/communication/dspace/curvature", Float32, self.cb6, queue_size=1)
self.sub_yaw = rospy.Subscriber("/communication/dspace/imu",
Imu,
self.cb6,
queue_size=1)
self.sub_est = rospy.Subscriber("/odometry/filtered",
Odometry,
self.call_back_est,
queue_size=1)
self.pub_xyyaw_test = rospy.Publisher("/xyyaw_test",
Point,
queue_size=1)
self.pub_odom_test = rospy.Publisher("/odom_test",
Odometry,
queue_size=1)
# -----------------------------------------------------
self.sub_wheel_data_all_ = rospy.Subscriber(
"/communication/dspace/wheel_data_all",
Float32MultiArray,
self.callback,
queue_size=1)
self.pub_wheel_odometry_ = rospy.Publisher('Wheel_Odometry',
Odometry,
queue_size=1)
self.wheel_data = list()
# for some visualization --------------------
self.sample_count = 10
# for parameter tuning
self.pose_cov = rospy.get_param("/wheel_pose_cov")
self.twist_cov = rospy.get_param("/wheel_twist_cov")
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)
pos = numpy.matrix(s['Pos']).transpose()
dir = numpy.matrix(s['Dir']).transpose()
feat_name = '%s/%s.%d' % (obj.name, ITasCFeature.NAMES[featureT],
counters[featureT])
f = ITasCFeature(featureT, feat_name, pos, dir)
obj.features.append(f)
print ' ITasCFeature: %s, %s, pos:%s, dir:%s' % (
feat_name, ITasCFeature.NAMES[featureT], str(pos), str(dir))
counters[featureT] += 1
# start ros node and broadcast all poses for each single frame
pub = rospy.Publisher('handtracking', Feature)
#rospy.init_node('handtracking')
for frame_idx, frame in enumerate(frames):
rot_mats = frame['mats']
timestamp = Time(time.time())
for i, (obj, rot_mat) in enumerate(zip(object_store, rot_mats)):
if len(obj.features) == 0: continue
#obj_pos = rot_mat[0:3,3]
#obj_dir = rot_mat[0:3,0:3]
for feature in obj.iterFeaturePoses(rot_mat):
msg_frame = 'ref_frame_name'
pos_vect = Vector3(float(feature.pos[0]),
float(feature.pos[1]),
float(feature.pos[2]))
dir_vect = Vector3(float(feature.dir[0]),
float(feature.dir[1]),
float(feature.dir[2]))
contactdir_vect = Vector3(0, 0, 0)
feat_msg = Feature(msg_frame, feature.type, feature.name,
pos_vect, dir_vect, contactdir_vect)
def requestSyncTime(self):
t = Time()
data_buffer = StringIO.StringIO()
t.serialize(data_buffer)
self.send(TopicInfo.ID_TIME, data_buffer.getvalue())
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!")