def updateDynamicConfiguration_SERIALPORT(self):
"""
Update dynamic configuration of ROS
:param:
:returns: none
"""
global dynamicConfig_SERIAL_PORT
# intialize dynamic configuration
dynamicConfigServer = Server(DWM1001_Tune_SerialConfig,
self.callbackDynamicConfig)
# set close port to true
dynamicConfig_CLOSE_PORT.update({"close_port": True})
# set the open port to false
dynamicConfig_OPEN_PORT.update({"open_port": False})
# update the server
dynamicConfigServer.update_configuration(dynamicConfig_OPEN_PORT)
dynamicConfigServer.update_configuration(dynamicConfig_CLOSE_PORT)
# update the server with opened port
dynamicConfig_CLOSE_PORT.update({"close_port": False})
# update the server with close port
dynamicConfig_OPEN_PORT.update({"open_port": True})
# update name of serial port in dynamic configuration
dynamicConfig_SERIAL_PORT = {
"serial_port": str(serialPortDWM1001.name)
}
# now update server configuration
dynamicConfigServer.update_configuration(dynamicConfig_OPEN_PORT)
dynamicConfigServer.update_configuration(dynamicConfig_OPEN_PORT)
dynamicConfigServer.update_configuration(dynamicConfig_CLOSE_PORT)
dynamicConfigServer.update_configuration(dynamicConfig_SERIAL_PORT)
def updateDynamicConfiguration_SERIALPORT():
global dynamicConfig_SERIAL_PORT
# intialize dynamic configuration
dynamicConfigServer = Server(DWM1001_Tune_SerialConfig, callbackDynamicConfig)
# set close port to true
dynamicConfig_CLOSE_PORT.update({"close_port": True})
# set the open port to false
dynamicConfig_OPEN_PORT.update({"open_port" : False})
# update the server
dynamicConfigServer.update_configuration(dynamicConfig_OPEN_PORT)
dynamicConfigServer.update_configuration(dynamicConfig_CLOSE_PORT)
# update the server with opened port
dynamicConfig_CLOSE_PORT.update({"close_port": False})
# update the server with close port
dynamicConfig_OPEN_PORT.update({"open_port": True})
# update name of serial port in dynamic configuration
dynamicConfig_SERIAL_PORT = {"serial_port": str(SERIAL_PORT_DETAILS.name)}
# now update server configuration
dynamicConfigServer.update_configuration(dynamicConfig_OPEN_PORT)
dynamicConfigServer.update_configuration(dynamicConfig_OPEN_PORT)
dynamicConfigServer.update_configuration(dynamicConfig_CLOSE_PORT)
dynamicConfigServer.update_configuration(dynamicConfig_SERIAL_PORT)
class OGridServer:
def __init__(self, frame_id='enu', map_size=500, resolution=0.3, rate=1):
self.frame_id = frame_id
self.ogrids = {}
self.odom = None
# Some default values
self.plow = True
self.plow_factor = 0
self.ogrid_min_value = -1
self.draw_bounds = False
self.resolution = resolution
self.ogrid_timeout = 2
self.enforce_bounds = False
self.enu_bounds = [[0, 0, 1], [0, 0, 1], [0, 0, 1], [0, 0, 1]]
# Default to centering the ogrid
position = np.array(
[-(map_size * resolution) / 2, -(map_size * resolution) / 2, 0])
quaternion = np.array([0, 0, 0, 1])
self.origin = navigator_tools.numpy_quat_pair_to_pose(
position, quaternion)
self.global_ogrid = self.create_grid((map_size, map_size))
set_odom = lambda msg: setattr(
self, 'odom', navigator_tools.pose_to_numpy(msg.pose.pose))
rospy.Subscriber('/odom', Odometry, set_odom)
self.publisher = rospy.Publisher('/ogrid_master',
OccupancyGrid,
queue_size=1)
self.ogrid_server = Server(OgridConfig, self.dynamic_cb)
dynam_client = Client("bounds_server", config_callback=self.bounds_cb)
self.ogrid_server.update_configuration({'width': 500})
rospy.Service("/center_ogrid", Trigger, self.center_ogrid)
rospy.Timer(rospy.Duration(1.0 / rate), self.publish)
def center_ogrid(self, srv):
fprint("CENTERING OGRID AT POSITION", msg_color='blue')
if self.odom is None:
return
dim = -(self.map_size[0] * self.resolution) / 2
new_org = self.odom[0] + np.array([dim, dim, 0])
config = {}
config['origin_x'] = float(new_org[0])
config['origin_y'] = float(new_org[1])
config['set_origin'] = True
self.ogrid_server.update_configuration(config)
def bounds_cb(self, config):
fprint("BOUNDS DYNAMIC CONFIG UPDATE!", msg_color='blue')
if hasattr(config, "enu_1_lat"):
self.enu_bounds = [[config['enu_1_lat'], config['enu_1_long'], 1],
[config['enu_2_lat'], config['enu_2_long'], 1],
[config['enu_3_lat'], config['enu_3_long'], 1],
[config['enu_4_lat'], config['enu_4_long'], 1],
[config['enu_1_lat'], config['enu_1_long'], 1]]
self.enforce_bounds = config['enforce']
def dynamic_cb(self, config, level):
fprint("OGRID DYNAMIC CONFIG UPDATE!", msg_color='blue')
topics = config['topics'].replace(' ', '').split(',')
replace_topics = config['replace_topics'].replace(' ', '').split(',')
new_grids = {}
for topic in topics:
new_grids[topic] = OGrid(
topic) if topic not in self.ogrids else self.ogrids[topic]
for topic in replace_topics:
new_grids[topic] = OGrid(
topic, replace=True
) if topic not in self.ogrids else self.ogrids[topic]
self.ogrids = new_grids
map_size = map(int, (config['height'], config['width']))
self.map_size = map_size
self.plow = config['plow']
self.plow_factor = config['plow_factor']
self.ogrid_min_value = config['ogrid_min_value']
self.draw_bounds = config['draw_bounds']
self.resolution = config['resolution']
self.ogrid_timeout = config['ogrid_timeout']
if config['set_origin']:
fprint("Setting origin!")
position = np.array([config['origin_x'], config['origin_y'], 0])
quaternion = np.array([0, 0, 0, 1])
self.origin = navigator_tools.numpy_quat_pair_to_pose(
position, quaternion)
else:
position = np.array([
-(map_size[1] * self.resolution) / 2,
-(map_size[0] * self.resolution) / 2, 0
])
quaternion = np.array([0, 0, 0, 1])
self.origin = navigator_tools.numpy_quat_pair_to_pose(
position, quaternion)
self.global_ogrid = self.create_grid(map_size)
return config
def create_grid(self, map_size):
"""
Creates blank ogrids for everyone for the low low price of $9.95!
`map_size` should be in the form of (h, w)
"""
ogrid = OccupancyGrid()
ogrid.header.stamp = rospy.Time.now()
ogrid.header.frame_id = self.frame_id
ogrid.info.resolution = self.resolution
ogrid.info.width = map_size[1]
ogrid.info.height = map_size[0]
ogrid.info.origin = self.origin
# TODO: Make sure this produces the correct sized ogrid
ogrid.data = np.full((map_size[1], map_size[0]), -1).flatten()
# fprint('Created Occupancy Map', msg_color='blue')
return ogrid
def publish(self, *args):
# fprint("Merging Maps", newline=2)
global_ogrid = deepcopy(self.global_ogrid)
np_grid = numpyify(global_ogrid)
# Global ogrid (only compute once)
corners = get_enu_corners(global_ogrid)
index_limits = transform_enu_to_ogrid(corners, global_ogrid)[:, :2]
g_x_min = index_limits[0][0]
g_x_max = index_limits[1][0]
g_y_min = index_limits[0][1]
g_y_max = index_limits[1][1]
to_add = [o for o in self.ogrids.itervalues() if not o.replace]
to_replace = [o for o in self.ogrids.itervalues() if o.replace]
for ogrids in [to_add, to_replace]:
for ogrid in ogrids:
# Hard coded 5 second timeout - probably no need to reconfig this.
if ogrid.nav_ogrid is None or ogrid.callback_delta > self.ogrid_timeout:
#fprint("Ogrid too old!")
continue
# Proactively checking for errors.
# This should be temporary but probably wont be.
l_h, l_w = ogrid.nav_ogrid.info.height, ogrid.nav_ogrid.info.width
g_h, g_w = global_ogrid.info.height, global_ogrid.info.width
if l_h > g_h or l_w > g_w:
fprint("Proactively preventing errors in ogrid size.",
msg_color="red")
new_size = max(l_w, g_w, l_h, g_h)
self.global_ogrid = self.create_grid([new_size, new_size])
# Local Ogrid (get everything in global frame though)
corners = get_enu_corners(ogrid.nav_ogrid)
index_limits = transform_enu_to_ogrid(corners, ogrid.nav_ogrid)
index_limits = transform_between_ogrids(
index_limits, ogrid.nav_ogrid, global_ogrid)[:, :2]
l_x_min = index_limits[0][0]
l_x_max = index_limits[1][0]
l_y_min = index_limits[0][1]
l_y_max = index_limits[1][1]
xs = np.sort([g_x_max, g_x_min, l_x_max, l_x_min])
ys = np.sort([g_y_max, g_y_min, l_y_max, l_y_min])
# These are indicies in cell units
start_x, end_x = np.round(xs[1:3]) # Grabbing indices 1 and 2
start_y, end_y = np.round(ys[1:3])
# Should be indicies
l_ogrid_start = transform_between_ogrids([start_x, start_y, 1],
global_ogrid,
ogrid.nav_ogrid)
# fprint("ROI {},{} {},{}".format(start_x, start_y, end_x, end_y))
index_width = l_ogrid_start[
0] + end_x - start_x # I suspect rounding will be a source of error
index_height = l_ogrid_start[1] + end_y - start_y
# fprint("width: {}, height: {}".format(index_width, index_height))
# fprint("Ogrid size: {}, {}".format(ogrid.nav_ogrid.info.height, ogrid.nav_ogrid.info.width))
to_add = ogrid.np_map[l_ogrid_start[1]:index_height,
l_ogrid_start[0]:index_width]
# fprint("to_add shape: {}".format(to_add.shape))
# Make sure the slicing doesn't produce an error
end_x = start_x + to_add.shape[1]
end_y = start_y + to_add.shape[0]
try:
# fprint("np_grid shape: {}".format(np_grid[start_y:end_y, start_x:end_x].shape))
fprint("{}, {}".format(ogrid.topic, ogrid.replace))
if ogrid.replace:
np_grid[start_y:end_y, start_x:end_x] = to_add
else:
np_grid[start_y:end_y, start_x:end_x] += to_add
except Exception as e:
fprint("Exception caught, probably a dimension mismatch:",
msg_color='red')
print e
fprint("w: {}, h: {}".format(global_ogrid.info.width,
global_ogrid.info.height),
msg_color='red')
if self.draw_bounds and self.enforce_bounds:
ogrid_bounds = transform_enu_to_ogrid(
self.enu_bounds, global_ogrid).astype(np.int32)
for i, point in enumerate(ogrid_bounds[:, :2]):
if i == 0:
last_point = point
continue
cv2.line(np_grid, tuple(point), tuple(last_point), 100, 3)
last_point = point
if self.plow:
self.plow_snow(np_grid, global_ogrid)
# Clip and flatten grid
np_grid = np.clip(np_grid, self.ogrid_min_value, 100)
global_ogrid.data = np_grid.flatten().astype(np.int8)
self.publisher.publish(global_ogrid)
def plow_snow(self, np_grid, ogrid):
"""Remove region around the boat so we never touch an occupied cell (making lqrrt not break
if something touches us).
"""
if self.odom is None:
return np_grid
p, q = self.odom
yaw_rot = trns.euler_from_quaternion(q)[2] # rads
boat_width = params.boat_length + params.boat_buffer + self.plow_factor # m
boat_height = params.boat_width + params.boat_buffer + self.plow_factor # m
x, y, _ = transform_enu_to_ogrid([p[0], p[1], 1], ogrid)
theta = yaw_rot
w = boat_width / ogrid.info.resolution
h = boat_height / ogrid.info.resolution
box = cv2.cv.BoxPoints(((x, y), (w, h), np.degrees(theta)))
box = np.int0(box)
cv2.drawContours(np_grid, [box], 0, 0, -1)
# Draw a "boat" in the ogrid
boat_width = params.boat_length + params.boat_buffer
boat_height = params.boat_width + params.boat_buffer
x, y, _ = transform_enu_to_ogrid([p[0], p[1], 1], ogrid)
w = boat_width / ogrid.info.resolution
h = boat_height / ogrid.info.resolution
box = cv2.cv.BoxPoints(((x, y), (w, h), np.degrees(theta)))
box = np.int0(box)
cv2.drawContours(np_grid, [box], 0, 40, -1)
# fprint("Plowed snow!")
return np_grid
class JoyController:
def __init__(self,options=None):
self.options = options
self.warntime = time.time()
self.USB = False
self.joy_scale = [-1,1,-1,1,1] #RPYTY
self.trim_roll = 0
self.trim_pitch = 0
self.max_angle = 30
self.max_yawangle = 200
self.max_thrust = 80.
self.min_thrust = 25.
self.max_thrust_raw = percentageToThrust(self.max_thrust)
self.min_thrust_raw = percentageToThrust(self.min_thrust)
self.old_thurst_raw = 0
self.slew_limit = 45
self.slew_rate = 30
self.slew_limit_raw = percentageToThrust(self.slew_limit)
self.slew_rate_raw = percentageToThrust(self.slew_rate)
self.dynserver = None
self.prev_cmd = None
self.curr_cmd = None
self.yaw_joy = True
self.sub_joy = rospy.Subscriber("/joy", JoyMSG, self.new_joydata)
self.sub_tf = tf.TransformListener()
self.pub_tf = tf.TransformBroadcaster()
self.pub_cfJoy = rospy.Publisher("/cfjoy", CFJoyMSG, queue_size=2)
self.pub_PID = rospy.Publisher("/pid", PidMSG,queue_size=50)
# PIDs for R,P,Y,THROTTLE
self.PID = (PID(), PID(), PID(), PID()) # XY, Throttle, Yaw
self.PIDActive = (True, True, True, True) # XY, YAw, Throttle on/off
self.PIDDelay = 0.0
self.PIDSource = Source.Qualisys
self.PIDControl = True
self.goal = [0.0, 0.0, 0.0, 0.0] # X Y Z YAW
self.prev_msg = None
self.PIDSetCurrentAuto = True
self.thrust = (40., 90., 66.) # Min, Max, Base thrust
self.distFunc = fLIN
self.wandControl = False
# Dynserver
self.dynserver = DynamicReconfigureServer(CFJoyCFG, self.reconfigure)
def setXYRGoal(self, msg):
new_settings = {}
new_settings["x"] = msg.pose.position.x
new_settings["y"] = msg.pose.position.y
new_settings["rz"] = degrees(tf.transformations.euler_from_quaternion(
[msg.pose.orientation.x,
msg.pose.orientation.y,
msg.pose.orientation.z,
msg.pose.orientation.w])[2])
os.system("beep -f 6000 -l 35&")
print new_settings
self.dynserver.update_configuration(new_settings)
def released(self, id):
return self.prev_cmd.buttons[id] and not self.curr_cmd.buttons[id]
def pressed(self, id):
return not self.prev_cmd.buttons[id] and self.curr_cmd.buttons[id]
def held(self, id):
return self.prev_cmd.buttons[id] and self.curr_cmd.buttons[id]
#Needed so the class can change the dynserver values with button presses
def set_dynserver(self, server):
self.dynserver = server
def thurstToRaw(self, joy_thrust):
# Deadman button or invalid thrust
if not self.curr_cmd.buttons[Button.L1] or joy_thrust>1:
return 0
raw_thrust = 0
if joy_thrust > 0.016:
raw_thrust = self.min_thrust_raw + joy_thrust*(self.max_thrust_raw-self.min_thrust_raw)
if self.slew_rate_raw>0 and self.slew_limit_raw > raw_thrust:
if self.old_thurst_raw > self.slew_limit_raw:
self.old_thurst_raw = self.slew_limit_raw
if raw_thrust < (self.old_thurst_raw - (self.slew_rate_raw/100)):
raw_thrust = self.old_thurst_raw - self.slew_rate_raw/100
if joy_thrust < 0 or raw_thrust < self.min_thrust_raw:
raw_thrust = 0
self.old_thurst_raw = raw_thrust
return raw_thrust
def new_joydata(self, joymsg):
global Button
global Axes
#Should run at 100hz. Use launch files roslaunch crazyflie_ros joy.launch
if self.prev_cmd == None:
self.prev_cmd = joymsg
print len(joymsg.axes)
#USB mode
if len(joymsg.axes) == 27:
rospy.logwarn("Experimental: Using USB mode. Axes/Buttons may be different")
Button = enum(L1=10,R1=11,Select=0,Start=3,L2=8,R2=9,Up=4,Right=5,Down=6,Left=7,Square=15,Cross=14,Triangle=12,Circle=13)
Axes = enum(SLL=0,SLU=1,SRL=2,SRU=3,Up=4+4,Right=5+4,Down=6+4,Left=7+4,L2=4+8,R2=4+9,L1=4+10,R1=4+11,Triangle=4+12,Circle=4+13,Cross=4+14,Square=4+15,AccL=16,AccF=17,AccU=18,GyroY=19)
self.USB = True
return
# bp = [i for i in range(len(joymsg.buttons)) if joymsg.buttons[i]]
# ap = [(i,round(joymsg.axes[i],2)) for i in range(len(joymsg.axes)) if abs(joymsg.axes[i])>0.51 ]
# if len(bp)>0:
# print "Buttons:", bp
# if len(ap)>0:
# print "Axes :", ap
if self.USB:
# USB buttons go from 1 to -1, Bluetooth go from 0 to -1, so normalise
joymsg.axes =np.array(joymsg.axes, dtype=np.float32)
joymsg.axes[8:19] -=1
joymsg.axes[8:19] /=2
# Strange bug: left button as axis doesnt work. So use button instead
joymsg.axes[11] = -joymsg.buttons[Button.Left]/2.
self.curr_cmd = joymsg
hover = False
x = 0
y = 0
z = 0
r = 0
r2 = 0
# Get stick positions [-1 1]
if self.curr_cmd.buttons[Button.L1]: # Deadman pressed (=allow flight)
x = self.joy_scale[0] * self.curr_cmd.axes[Axes.SLL] # Roll
y = self.joy_scale[1] * self.curr_cmd.axes[Axes.SLU] # Pitch
r = self.joy_scale[2] * self.curr_cmd.axes[Axes.SRL] # Yaw
z = self.joy_scale[3] * self.curr_cmd.axes[Axes.SRU] # Thrust
r2 = self.joy_scale[4] * (self.curr_cmd.axes[Axes.L2] - self.curr_cmd.axes[Axes.R2])
hover = self.curr_cmd.axes[Axes.L1]<-0.75
roll = x * self.max_angle
pitch = y * self.max_angle
yaw = 0
#TODO use dyn reconf to decide which to use
if r2!=0:
yaw = r2 * self.max_yawangle
else:
if self.yaw_joy:
# Deadzone
if r < -0.2 or r > 0.2:
if r < 0:
yaw = (r + 0.2) * self.max_yawangle * 1.25
else:
yaw = (r - 0.2) * self.max_yawangle * 1.25
if hover:
thrust = int(round(deadband(z,0.2)*32767 + 32767)) #Convert to uint16
else:
thrust = self.thurstToRaw(z)
trimmed_roll = roll + self.trim_roll
trimmed_pitch = pitch + self.trim_pitch
# Control trim manually
new_settings = {}
if joymsg.header.seq%10 == 0:
if self.curr_cmd.buttons[Button.Left]:
new_settings["trim_roll"] = max(self.trim_roll + self.curr_cmd.axes[Axes.Left], -20)
if self.curr_cmd.buttons[Button.Right]:
new_settings["trim_roll"] = min(self.trim_roll - self.curr_cmd.axes[Axes.Right], 20)
if self.curr_cmd.buttons[Button.Down]:
new_settings["trim_pitch"] = max(self.trim_pitch + self.curr_cmd.axes[Axes.Down], -20)
if self.curr_cmd.buttons[Button.Up]:
new_settings["trim_pitch"] = min(self.trim_pitch - self.curr_cmd.axes[Axes.Up], 20)
msg = CFJoyMSG()
msg.header.stamp = rospy.Time.now()
msg.roll = trimmed_roll
msg.pitch = trimmed_pitch
msg.yaw = yaw
msg.thrust = thrust
msg.hover = hover
#msg.calib = self.pressed(Button.Triangle)
# TODO: rotation is wrong! Cannot simply decompose x,y into r,p.
# TODO: roll, pitch should be dependent from the distance and the angle.
if self.PIDControl: # and hover:
try:
rtime = rospy.Time.now()
# Update goal position
if hover and joymsg.header.seq%10 == 0:
if self.PIDActive[0]:
if abs(x)>0.016:
new_settings["x"] = self.goal[0]-roll/70.
if abs(y)>0.016:
new_settings["y"] = self.goal[1]-pitch/70.
if self.PIDActive[1]:
if abs(z)>0.016:
new_settings["z"] = self.goal[2]+z/5
if self.PIDActive[2]:
if abs(yaw)>0:
new_settings["rz"] = self.goal[3]-yaw/self.max_yawangle*20
if new_settings.has_key("x") or new_settings.has_key("y") or new_settings.has_key("z") or new_settings.has_key("rz"):
os.system("beep -f 6000 -l 15&")
# Get error from current position to goal if possible
[px,py,alt,rz] = self.getErrorToGoal()
# Starting control mode
if hover and not self.prev_msg.hover:
# Starting Goal should be from current position
if self.PIDSetCurrentAuto:
new_settings["SetCurrent"] = True
# Reset PID controllers
for p in range(4):
self.PID[p].reset(rtime)
# Set yaw difference as I part
#self.PID[3].error_sum = 0
if hover:
msg.hover = False #TODO should be an option
pidmsg = PidMSG()
pidmsg.header.stamp = rtime
# Get PID control
#XY control depends on the asin of the distance
#cr, crp, cri, crd = self.PID[0].get_command(-py,rtime,f=lambda d: degrees(asin(d))/90.*self.max_angle)
#cp, cpp, cpi, cpd = self.PID[1].get_command(px,rtime,f=lambda d: degrees(asin(d))/90.*self.max_angle)
#cr, crp, cri, crd = self.PID[0].get_command(-py,rtime)
#cp, cpp, cpi, cpd = self.PID[1].get_command(px,rtime)
# Distance to angle (X direction = pitch, Y direction = roll)
cr, crp, cri, crd = self.PID[0].get_command(-py,rtime,f=self.distFunc )
cp, cpp, cpi, cpd = self.PID[1].get_command(px,rtime,f=self.distFunc )
# THRUST
ct, ctp, cti, ctd = self.PID[2].get_command(alt,rtime)
# YAW VEL
cy, cyp, cyi, cyd = self.PID[3].get_command(-rz,rtime)#,d=rz,i=copysign(1,-rz)) #TODO: normalise rotation
# ROll/Pitch
if self.PIDActive[0]:
msg.roll = cr + self.trim_roll
msg.pitch = cp + self.trim_pitch
pidmsg.ypid = [cr, crp, cri, -crd, -py]
pidmsg.xpid = [cp, cpp, cpi, -cpd, px]
# THRUST
if self.PIDActive[1]:
# Add base thrust
ct += self.thrust[2]
# Add roll compensation
# TODO: done on the flie itself now @ 250hz
# Bound
ct = max(self.thrust[0],ct)
ct = min(self.thrust[1],ct)
# ct in thrust percent
msg.thrust = percentageToThrust(ct)
pidmsg.zpid= [ct, ctp, cti, ctd, alt]
# YAW
if self.PIDActive[2]:
msg.yaw = cy
pidmsg.rzpid= [cy, cyp, cyi, cyd, -rz]
self.pub_PID.publish(pidmsg)
if self.pressed(Button.Cross):
new_settings["SetCurrent"] = True
q = tf.transformations.quaternion_from_euler(radians(msg.roll-self.trim_roll), radians(msg.pitch - self.trim_pitch), radians(-msg.yaw))
self.pub_tf.sendTransform((0,0,0), q, rospy.Time.now(), "/cmd", "/cf_gt2d")
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
if time.time() - self.warntime>2:
rospy.logwarn('Could not look up cf_gt2d -> goal')
self.warntime = time.time()
global tid
if self.pressed(Button.Circle):
tid = (tid +1)%len(trajectory)
new_settings["x"], new_settings["y"], new_settings["z"], new_settings["rz"] = trajectory[tid]
# previous
if self.pressed(Button.Triangle):
tid = (tid -1)%len(trajectory)
new_settings["x"], new_settings["y"], new_settings["z"], new_settings["rz"] = trajectory[tid]
# Set trim to current input
if self.released(Button.R1):
new_settings["trim_roll"] = min(20, max(msg.roll, -20))
new_settings["trim_pitch"] = min(20, max(msg.pitch, -20))
rospy.loginfo("Trim updated Roll/Pitch: %r/%r", round(new_settings["trim_roll"],2), round(new_settings["trim_pitch"],2))
# Reset Trim
if self.released(Button.Square):
new_settings["trim_roll"] = 0.0
new_settings["trim_pitch"] = 0.0
rospy.loginfo("Pitch reset to 0/0")
if new_settings != {} and self.dynserver is not None:
if self.USB:
new_settings = numpy2python(new_settings)
self.dynserver.update_configuration(new_settings)
# Cache prev joy command
self.pub_cfJoy.publish(msg)
self.prev_cmd = self.curr_cmd
self.prev_msg = msg
self.prev_msg.hover = hover
def lookupTransformInWorld(self, frame='/cf_gt', forceRealtime = False, poseNoiseMeters=0.0 ):
now = rospy.Time(0)
if self.PIDDelay > 0.00001 and not forceRealtime:
#rospy.logwarn('Delay in TF Pose: %5.3s', self.PIDDelay)
now = rospy.Time.now()-rospy.Duration(self.PIDDelay)
(trans,rot) = self.sub_tf.lookupTransform('/world', frame, now)
if poseNoiseMeters>0:
trans = [trans[0]+random.normalvariate(0,poseNoiseMeters), trans[1]+random.normalvariate(0,poseNoiseMeters), trans[2]+random.normalvariate(0,poseNoiseMeters)]
return (trans,rot)
def getErrorToGoal(self):
# Look up flie position, convert to 2d, publish, return error
if self.wandControl:
#update goal using wand
try:
(t,r) = self.lookupTransformInWorld(frame='/wand', forceRealtime=True)
e = tf.transformations.euler_from_quaternion(r)
self.goal= [t[0], t[1], t[2], degrees(e[2]-e[0])]
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
rospy.loginfo('Could not find wand')
# Publish GOAL from world frame
q = tf.transformations.quaternion_from_euler(0, 0, radians(self.goal[3]))
self.pub_tf.sendTransform((self.goal[0],self.goal[1],self.goal[2]), q, rospy.Time.now(), "/goal", "/world")
# Look up 6D flie pose, publish 3d+yaw pose
(trans,rot) = self.lookupTransformInWorld(frame='/cf_gt', poseNoiseMeters=0.0)
euler = tf.transformations.euler_from_quaternion(rot)
q = tf.transformations.quaternion_from_euler(0, 0, euler[2])
self.pub_tf.sendTransform(trans, q, rospy.Time.now(), "/cf_gt2d", "/world")
# Look up error goal->cf
(trans,rot) = self.sub_tf.lookupTransform('/cf_gt2d', '/goal', rospy.Time(0))
#self.pub_tf.sendTransform(trans, rot, rospy.Time.now(), "/goalCF", "/cf_gt2d")
euler = tf.transformations.euler_from_quaternion(rot)
return trans[0], trans[1], trans[2], degrees(euler[2])
def setGoalFromCurrent(self,config={}):
try:
(trans,rot) = self.lookupTransformInWorld(frame='/cf_gt', forceRealtime=True)
euler = tf.transformations.euler_from_quaternion(rot)
config['x'],config['y'],config['z'],config['rz']= trans[0], trans[1], trans[2], degrees(euler[2])
rospy.loginfo('Updated goal state: [%.2f %.2f %.2f %.1f]', trans[0], trans[1], trans[2], degrees(euler[2]))
self.goal = [config['x'],config['y'],config['z'],config['rz']]
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
rospy.logwarn('Could not look up transform world -> cf_gt')
return config
def reconfigure(self, config, level):
# Manual control stuff
self.trim_roll = config["trim_roll"]
self.trim_pitch = config["trim_pitch"]
self.max_angle = config["max_angle"]
self.max_yawangle = config["max_yawangle"]
self.max_thrust = config["max_thrust"]
self.min_thrust = config["min_thrust"]
self.slew_limit = config["slew_limit"]
self.slew_rate = config["slew_rate"]
self.max_thrust_raw = percentageToThrust(self.max_thrust)
self.min_thrust_raw = percentageToThrust(self.min_thrust)
self.slew_limit_raw = percentageToThrust(self.slew_limit)
self.slew_rate_raw = percentageToThrust(self.slew_rate)
self.yaw_joy = config["yaw_joy"]
#PID control stuff
if config["PIDPreset"]>0:
if config["PIDPreset"]==1:
# Aggressive
config["DistFunc"] = 1 # ATAN
config["Pxy"] = 1.0
config["Ixy"] = 0.0
config["Dxy"] = 0.2
config["Pyaw"] = 8
config["Iyaw"] = 0.0
config["Dyaw"] = 0.0
config["Pz"] = 20
config["Iz"] = 5
config["Dz"] = 8
config["RP_maxAngle"] = 35
config["Y_maxVel"] = 250
config["z_minThrust"] = 50
config["z_maxThrust"] = 100
config["z_baseThrust"] = 73
config["Response"] = 0.3
if config["PIDPreset"]==2:
# Passive
config["DistFunc"] = 2 # ASIN
config["Pxy"] = 2.0
config["Ixy"] = 0.0
config["Dxy"] = 0.4
config["Pyaw"] = 2.0
config["Iyaw"] = 0.0
config["Dyaw"] = 0.0
config["Pz"] = 20
config["Iz"] = 5
config["Dz"] = 8
config["RP_maxAngle"] = 7
config["Y_maxVel"] = 80
config["z_minThrust"] = 50
config["z_maxThrust"] = 90
config["z_baseThrust"] = 73
config["Response"] = 0.175
if config["PIDPreset"]==3:
# Linear
config["DistFunc"] = 0
config["Pxy"] = 17
config["Ixy"] = 0.0
config["Dxy"] = 5
config["Pyaw"] = 3.3
config["Iyaw"] = 0.0
config["Dyaw"] = 0.0
config["Pz"] = 20
config["Iz"] = 5
config["Dz"] = 8
config["RP_maxAngle"] = 25
config["Y_maxVel"] = 300
config["z_minThrust"] = 50
config["z_maxThrust"] = 90
config["z_baseThrust"] = 73
config["Response"] = 1
rospy.loginfo("Preset Loaded")
config["PIDPreset"] = 0
# Settings
self.PIDDelay = config["Delay"]
self.PIDSource = config["Source"]
self.PIDControl = config["Control"]
self.max_anglePID = config['RP_maxAngle']
# Gains
self.PID[0].set_gains(config["Pxy"], config["Ixy"], config["Dxy"] )
self.PID[1].set_gains(config["Pxy"], config["Ixy"], config["Dxy"] )
self.PID[2].set_gains(config["Pz"], config["Iz"], config["Dz"] )
self.PID[3].set_gains(config["Pyaw"], config["Iyaw"], config["Dyaw"] )
# Goal
self.wandControl = config['WandControl']
if config['SetCurrent']:
config['SetCurrent'] = False
config = self.setGoalFromCurrent(config)
if config['Set'] or config['LiveUpdate']:
config['Set'] = False
if not self.wandControl:
self.goal = [config['x'],config['y'],config['z'],config['rz']]
# Limits
#self.limits = (config['RP_maxAngle'], config['Y_maxVel'], config['z_maxAcc'])
self.PID[0].set_limits(config['RP_maxAngle'])
self.PID[1].set_limits(config['RP_maxAngle'])
self.PID[2].set_limits(100)
self.PID[3].set_limits(config['Y_maxVel'])
self.thrust = (config['z_minThrust'],config['z_maxThrust'], config['z_baseThrust'] )
# On/OFF
self.PIDActive = (config['xyControl'],config['thrustControl'],config['yawControl'])
self.PIDSetCurrentAuto = config['SetCurrentAuto']
self.distFunc = [lambda x: fLIN(x,config["Response"]),
lambda x: fATAN(x,config["Response"]),
lambda x: fASIN(x,config["Response"])][config['DistFunc']]
return config
class Driver:
def __init__(self, options):
self.options = options
self.crazyflie = Crazyflie()
self.battery_monitor = BatteryMonitor()
cflib.crtp.init_drivers()
self.zspeed = zSpeed()
# Keep track of this to know if the user changed imu mode to/from imu6/9
self.preMagcalib = None
#self.csvwriter = csv.writer(open('baro.csv', 'wb'), delimiter=',',quotechar='"', quoting=csv.QUOTE_MINIMAL)
#self.baro_start_time = None
# Some shortcuts
self.HZ100 = 10
self.HZ10 = 100
self.HZ1 = 1000
self.HZ500 = 2
self.HZ250 = 4
self.HZ125 = 8
self.HZ50 = 20
# Callbacks the CF driver calls
self.crazyflie.connectSetupFinished.add_callback(
self.connectSetupFinishedCB)
self.crazyflie.connectionFailed.add_callback(self.connectionFailedCB)
self.crazyflie.connectionInitiated.add_callback(
self.connectionInitiatedCB)
self.crazyflie.disconnected.add_callback(self.disconnectedCB)
self.crazyflie.connectionLost.add_callback(self.connectionLostCB)
self.crazyflie.linkQuality.add_callback(self.linkQualityCB)
# Advertise publishers
self.pub_acc = rospy.Publisher("/cf/acc", accMSG)
self.pub_mag = rospy.Publisher("/cf/mag", magMSG)
self.pub_gyro = rospy.Publisher("/cf/gyro", gyroMSG)
self.pub_baro = rospy.Publisher("/cf/baro", baroMSG)
self.pub_motor = rospy.Publisher("/cf/motor", motorMSG)
self.pub_hover = rospy.Publisher("/cf/hover", hoverMSG)
self.pub_attitude = rospy.Publisher("/cf/attitude", attitudeMSG)
self.pub_bat = rospy.Publisher("/cf/bat", batMSG)
self.pub_zpid = rospy.Publisher("/cf/zpid", zpidMSG)
self.pub_tf = tf.TransformBroadcaster()
# Subscribers
self.sub_tf = tf.TransformListener()
self.sub_joy = rospy.Subscriber("/cfjoy", joyMSG, self.new_joydata)
self.sub_magCalib = rospy.Subscriber("/magCalib", magCalibMSG,
self.new_magCalib)
# Keep track of link quality to send out with battery status
self.link = 0
# Loggers
self.logMotor = None
self.logBaro = None
self.logMag = None
self.logGyro = None
self.logAttitude = None
self.logAcc = None
self.logBat = None
self.logHover = None
self.logZPid = None
self.logGravOffset = None #only here till it works
# keep tracks if loggers are running
self.acc_monitor = False
self.gyro_monitor = False
self.baro_monitor = False
self.mag_monitor = False
self.bat_monitor = False
self.motor_monitor = False
self.attitude_monitor = False
self.hover_monitor = False
self.zpid_monitor = False
# CF params we wanna be able to change
self.cf_param_groups = ["hover", "sensorfusion6", "magCalib"]
self.cf_params_cache = {}
# Dynserver
self.dynserver = DynamicReconfigureServer(driverCFG, self.reconfigure)
# Finished set up. Connect to flie
self.connect(self.options)
def new_magCalib(self, msg):
""" Receive magnetometer calibration results. Send to flie via dyn conf"""
new_settings = {}
#TODO
new_settings["magCalib_off_x"] = msg.offset[0]
new_settings["magCalib_off_y"] = msg.offset[1]
new_settings["magCalib_off_z"] = msg.offset[2]
new_settings["magCalib_thrust_x"] = msg.thrust_comp[0]
new_settings["magCalib_thrust_y"] = msg.thrust_comp[1]
new_settings["magCalib_thrust_z"] = msg.thrust_comp[2]
new_settings["magCalib_scale_x"] = msg.scale[0]
new_settings["magCalib_scale_y"] = msg.scale[1]
new_settings["magCalib_scale_z"] = msg.scale[2]
self.dynserver.update_configuration(new_settings)
rospy.loginfo("Updated magnetometer calibration data")
def connect(self, options):
""" Look for crazyflie every 2s and connect to the specified one if found"""
rospy.loginfo("Waiting for crazyflie...")
while not rospy.is_shutdown():
interfaces = cflib.crtp.scan_interfaces()
if len(interfaces) > 1:
radio = None
rospy.loginfo("Found: ")
for i in interfaces:
rospy.loginfo("--> %s [%s]", i[0], i[1])
if i[0].startswith(options.uri):
radio = i[0]
if radio != None:
self.crazyflie.open_link(radio)
break
else:
rospy.sleep(2)
def new_joydata(self, joymsg):
""" Joydata arrived. Should happen at 100hz."""
# Parse joydata and extract commands
roll = joymsg.roll
pitch = joymsg.pitch
yawrate = joymsg.yaw
thrust = joymsg.thrust
hover = joymsg.hover
set_hover = joymsg.hover_set
hover_change = joymsg.hover_change
# Send to flie
self.send_control(
(roll, pitch, yawrate, thrust, hover, set_hover, hover_change))
def reconfigure(self, config, level):
""" Fill in local variables with values received from dynamic reconfigure clients (typically the GUI)."""
config = self.zspeed.reconfigure(config)
self.deadband = config["deadband"]
# On / off logging
if self.zpid_monitor != config["read_zpid"]:
self.zpid_monitor = config["read_zpid"]
if self.logZPid != None:
if self.zpid_monitor:
self.logZPid.start()
rospy.loginfo("zPID Logging started")
else:
self.logZPid.stop()
rospy.loginfo("zPID Logging stopped")
if self.gyro_monitor != config["read_gyro"]:
self.gyro_monitor = config["read_gyro"]
if self.logGyro != None:
if self.gyro_monitor:
self.logGyro.start()
rospy.loginfo("Gyro Logging started")
else:
self.logGyro.stop()
rospy.loginfo("Gyro Logging stopped")
if self.hover_monitor != config["read_hover"]:
self.hover_monitor = config["read_hover"]
if self.logHover != None:
if self.hover_monitor:
self.logHover.start()
rospy.loginfo("Hover Logging started")
else:
self.logHover.stop()
rospy.loginfo("Hover Logging stopped")
if self.acc_monitor != config["read_acc"]:
self.acc_monitor = config["read_acc"]
if self.logGyro != None:
if self.acc_monitor:
self.logAcc.start()
rospy.loginfo("Acc Logging started")
else:
self.logAcc.stop()
rospy.loginfo("Acc Logging stopped")
if self.baro_monitor != config["read_baro"]:
self.baro_monitor = config["read_baro"]
if self.logBaro != None:
if self.baro_monitor:
self.logBaro.start()
rospy.loginfo("Baro Logging started")
else:
self.logBaro.stop()
rospy.loginfo("Baro Logging stopped")
if self.mag_monitor != config["read_mag"]:
self.mag_monitor = config["read_mag"]
if self.logMag != None:
if self.mag_monitor:
self.logMag.start()
rospy.loginfo("Mag Logging started")
else:
self.logMag.stop()
rospy.loginfo("Mag Logging stopped")
if self.bat_monitor != config["read_bat"]:
self.bat_monitor = config["read_bat"]
if self.logBat != None:
if self.bat_monitor:
self.logBat.start()
rospy.loginfo("Battery/Link Logging started")
else:
self.logBat.stop()
rospy.loginfo("Battery/Link Logging stopped")
if self.attitude_monitor != config["read_attitude"]:
self.attitude_monitor = config["read_attitude"]
if self.logAttitude != None:
if self.attitude_monitor:
self.logAttitude.start()
rospy.loginfo("Attitude Logging started")
else:
self.logAttitude.stop()
rospy.loginfo("Attitude Logging stopped")
if self.motor_monitor != config["read_motor"]:
self.motor_monitor = config["read_motor"]
if self.logMotor != None:
if self.motor_monitor:
self.logMotor.start()
rospy.loginfo("Motor Logging started")
else:
self.logMotor.stop()
rospy.loginfo("Motor Logging stopped")
# SET CRAZYFLIE PARAMS
for key, value in config.iteritems():
# Skip this key (whats its purpose??)
if key == "groups":
continue
# Continue if variable exists and is correct
if self.cf_params_cache.has_key(key):
if config[key] == self.cf_params_cache[key]:
# Nothing changed
if not key.startswith("magCalib") or (
self.preMagcalib
or not config["sensorfusion6_magImu"]):
continue
# Doesnt exist or has changed
# Split into group and name
s = key.split("_", 1)
# Make sure it is a group name pair
if len(s) == 2:
# make sure group is valid
if s[0] in self.cf_param_groups:
# update cache, send variable to flie
self.cf_params_cache[key] = value
self.send_param(s[0] + "." + s[1], value)
print "updated: ", s[0] + "." + s[1], "->", value
rospy.sleep(0.1)
self.preMagcalib = config["sensorfusion6_magImu"]
return config
def send_param(self, key, value):
self.crazyflie.param.set_value(key, str(value))
def restart(self):
rospy.loginfo("Restarting Driver")
rospy.sleep(1)
self.connect(self.options)
def logErrorCB(self, errmsg):
rospy.logerr("Log error: %s", errmsg)
def send_control(self, cmd):
""" Roll, pitch in deg, yaw in deg/s, thrust in 10000-60000, hover bool, set_hover bool, hover chance float -1 to +1 """
roll, pitch, yawrate, thrust, hover, set_hover, hover_change = cmd
if self.crazyflie.state == CFState.CONNECTED:
self.crazyflie.commander.send_setpoint(roll, pitch, yawrate,
thrust, hover, set_hover,
hover_change)
def setup_log(self):
""" Console callbacks """
self.crazyflie.console.receivedChar.add_callback(self.consoleCB)
rospy.sleep(0.25)
""" ATTITUDE LOGGING @ 100hz """
logconf = LogConfig("attitude", self.HZ100 * 2) #ms
logconf.addVariable(LogVariable("attitude.q0", "float"))
logconf.addVariable(LogVariable("attitude.q1", "float"))
logconf.addVariable(LogVariable("attitude.q2", "float"))
logconf.addVariable(LogVariable("attitude.q3", "float"))
self.logAttitude = self.crazyflie.log.create_log_packet(logconf)
if (self.logAttitude is not None):
self.logAttitude.dataReceived.add_callback(
self.logCallbackAttitude)
self.logAttitude.error.add_callback(self.logErrorCB)
if self.attitude_monitor:
self.logAttitude.start()
rospy.loginfo("Attitude Logging started")
else:
rospy.logwarn("Could not setup Attitude logging!")
rospy.sleep(0.25)
""" ZPID LOGGING @ 100hz """
logconf = LogConfig("zpid", self.HZ100) #ms
logconf.addVariable(LogVariable("zpid.p", "float"))
logconf.addVariable(LogVariable("zpid.i", "float"))
logconf.addVariable(LogVariable("zpid.d", "float"))
logconf.addVariable(LogVariable("zpid.pid", "float"))
self.logZPid = self.crazyflie.log.create_log_packet(logconf)
if (self.logZPid is not None):
self.logZPid.dataReceived.add_callback(self.logCallbackZPid)
self.logZPid.error.add_callback(self.logErrorCB)
if self.zpid_monitor:
self.logZPid.start()
rospy.loginfo("ZPID Logging started")
else:
rospy.logwarn("Could not setup ZPID logging!")
rospy.sleep(0.25)
""" HOVER LOGGING @ 100hz """
logconf = LogConfig("hover", self.HZ100) #ms
logconf.addVariable(LogVariable("hover.err", "float"))
logconf.addVariable(LogVariable("hover.target", "float"))
logconf.addVariable(LogVariable("hover.zSpeed", "float"))
logconf.addVariable(LogVariable("hover.zBias", "float"))
logconf.addVariable(LogVariable("hover.acc_vspeed", "float"))
logconf.addVariable(LogVariable("hover.asl_vspeed", "float"))
self.logHover = self.crazyflie.log.create_log_packet(logconf)
if (self.logHover is not None):
self.logHover.dataReceived.add_callback(self.logCallbackHover)
self.logHover.error.add_callback(self.logErrorCB)
if self.hover_monitor:
self.logHover.start()
rospy.loginfo("Hover Logging started")
else:
rospy.logwarn("Could not setup Hover logging!")
rospy.sleep(0.25)
""" GYROMETER LOGGING @ 100hz """
logconf = LogConfig("LoggingGyro", self.HZ100) #ms
logconf.addVariable(LogVariable("gyro.x", "float"))
logconf.addVariable(LogVariable("gyro.y", "float"))
logconf.addVariable(LogVariable("gyro.z", "float"))
self.logGyro = self.crazyflie.log.create_log_packet(logconf)
if (self.logGyro is not None):
self.logGyro.dataReceived.add_callback(self.logCallbackGyro)
self.logGyro.error.add_callback(self.logErrorCB)
if self.gyro_monitor:
self.logGyro.start()
rospy.loginfo("Gyro Logging started")
else:
rospy.logwarn("Could not setup Gyro logging!")
rospy.sleep(0.25)
""" ACCELEROMETER LOGGING @ 100hz """
logconf = LogConfig("LoggingAcc", self.HZ100) #ms
logconf.addVariable(LogVariable("acc.x", "float"))
logconf.addVariable(LogVariable("acc.y", "float"))
logconf.addVariable(LogVariable("acc.z", "float"))
logconf.addVariable(LogVariable("acc.xw", "float"))
logconf.addVariable(LogVariable("acc.yw", "float"))
logconf.addVariable(LogVariable("acc.zw", "float"))
self.logAcc = self.crazyflie.log.create_log_packet(logconf)
if (self.logAcc is not None):
self.logAcc.dataReceived.add_callback(self.logCallbackAcc)
self.logAcc.error.add_callback(self.logErrorCB)
if self.acc_monitor:
self.logAcc.start()
rospy.loginfo("Acc Logging started")
else:
rospy.logwarn("Could not setup Acc logging!")
rospy.sleep(0.25)
""" MAGNETOMETER LOGGING @ 100hz """
logconf = LogConfig("LoggingMag", self.HZ100) #ms
logconf.addVariable(LogVariable("mag.x", "float"))
logconf.addVariable(LogVariable("mag.y", "float"))
logconf.addVariable(LogVariable("mag.z", "float"))
logconf.addVariable(LogVariable("mag.x_raw", "float"))
logconf.addVariable(LogVariable("mag.y_raw", "float"))
logconf.addVariable(LogVariable("mag.z_raw", "float"))
self.logMag = self.crazyflie.log.create_log_packet(logconf)
if (self.logMag is not None):
self.logMag.dataReceived.add_callback(self.logCallbackMag)
self.logMag.error.add_callback(self.logErrorCB)
if self.mag_monitor:
self.logMag.start()
rospy.loginfo("Mag Logging started")
else:
rospy.logwarn("Could not setup Mag logging!")
rospy.sleep(0.25)
""" BAROMETER LOGGING @ 100hz """
logconf = LogConfig("LoggingBaro", self.HZ100) #ms
logconf.addVariable(LogVariable("baro.aslRaw", "float"))
logconf.addVariable(LogVariable("baro.asl", "float"))
logconf.addVariable(LogVariable("baro.temp", "float"))
logconf.addVariable(LogVariable("baro.pressure", "float"))
logconf.addVariable(LogVariable("baro.aslLong", "float"))
self.logBaro = self.crazyflie.log.create_log_packet(logconf)
if (self.logBaro is not None):
self.logBaro.dataReceived.add_callback(self.logCallbackBaro)
self.logBaro.error.add_callback(self.logErrorCB)
if self.baro_monitor:
self.logBaro.start()
rospy.loginfo("Baro Logging started")
else:
rospy.logwarn("Could not setup Baro logging!")
rospy.sleep(0.25)
""" BATTERY/LINK LOGGING @ 10hz """
logconf = LogConfig("LoggingBat", self.HZ10) #ms
logconf.addVariable(LogVariable("pm.vbat", "float"))
logconf.addVariable(LogVariable("pm.state", "uint8_t"))
logconf.addVariable(LogVariable("pm.state_charge", "uint8_t"))
self.logBat = self.crazyflie.log.create_log_packet(logconf)
if (self.logBat is not None):
self.logBat.dataReceived.add_callback(self.logCallbackBat)
self.logBat.error.add_callback(self.logErrorCB)
if self.bat_monitor:
self.logBat.start()
rospy.loginfo("Bat Logging started")
else:
rospy.logwarn("Could not setup Battery/Link logging!")
rospy.sleep(0.25)
#TODO motor logging not working
""" MOTOR LOGGING @ 100hz"""
logconf = LogConfig("LoggingMotor", self.HZ100) #ms
logconf.addVariable(LogVariable("motor.m1", "uint32_t"))
logconf.addVariable(LogVariable("motor.m2", "uint32_t"))
logconf.addVariable(LogVariable("motor.m3", "uint32_t"))
logconf.addVariable(LogVariable("motor.m4", "uint32_t"))
logconf.addVariable(LogVariable("motor.thrust", "uint16_t"))
logconf.addVariable(LogVariable("motor.vLong", "float")) #TODO move
self.logMotor = self.crazyflie.log.create_log_packet(logconf)
if (self.logMotor is not None):
self.logMotor.dataReceived.add_callback(self.logCallbackMotor)
self.logMotor.error.add_callback(self.logErrorCB)
if self.motor_monitor:
self.logMotor.start()
rospy.loginfo("Motor Logging started")
else:
rospy.logwarn("Could not setup Motor logging!")
def logCallbackAcc(self, data):
"""Accelerometer axis data in mG --> G"""
msg = accMSG()
msg.header.stamp = rospy.Time.now()
msg.acc[0] = data["acc.x"]
msg.acc[1] = data["acc.y"]
msg.acc[2] = data["acc.z"]
msg.acc_w[0] = data["acc.xw"]
msg.acc_w[1] = data["acc.yw"]
msg.acc_w[2] = data["acc.zw"]
self.zspeed.add(msg.acc_w[2])
self.pub_acc.publish(msg)
self.pub_tf.sendTransform(msg.acc_w, (0, 0, 0, 1), rospy.Time.now(),
"/acc_w", "/cf_q")
self.pub_tf.sendTransform(msg.acc, (0, 0, 0, 1), rospy.Time.now(),
"/acc", "/cf_q")
def logCallbackMag(self, data):
"""TODO UNITS"""
msg = magMSG()
msg.header.stamp = rospy.Time.now()
msg.mag[0] = data["mag.x"]
msg.mag[1] = data["mag.y"]
msg.mag[2] = data["mag.z"]
msg.magRaw[0] = data["mag.x_raw"]
msg.magRaw[1] = data["mag.y_raw"]
msg.magRaw[2] = data["mag.z_raw"]
self.pub_mag.publish(msg)
if np.linalg.norm(msg.magRaw) > 1:
self.pub_tf.sendTransform(msg.mag / np.linalg.norm(msg.magRaw),
(0, 0, 0, 1), rospy.Time.now(),
"/mag_raw", "/cf_q")
if np.linalg.norm(msg.mag) > 1:
self.pub_tf.sendTransform(msg.mag / np.linalg.norm(msg.mag),
(0, 0, 0, 1), rospy.Time.now(),
"/mag_calib", "/cf_q")
def logCallbackGyro(self, data):
"""Gyro axis data in deg/s -> rad/s"""
msg = gyroMSG()
msg.header.stamp = rospy.Time.now()
msg.gyro[0] = radians(data["gyro.x"])
msg.gyro[1] = radians(data["gyro.y"])
msg.gyro[2] = radians(data["gyro.z"])
self.pub_gyro.publish(msg)
def logCallbackHover(self, data):
"""Output data regarding hovering"""
msg = hoverMSG()
msg.header.stamp = rospy.Time.now()
msg.err = deadband(data["hover.err"], self.deadband)
msg.zSpeed = data["hover.zSpeed"]
msg.acc_vspeed = data[
"hover.acc_vspeed"] # * self.cf_params_cache["hover_acc_vspeedFac"]
msg.asl_vspeed = data[
"hover.asl_vspeed"] # * self.cf_params_cache["hover_asl_vspeedFac"]
msg.target = data["hover.target"]
msg.zBias = data["hover.zBias"]
self.pub_hover.publish(msg)
def logCallbackZPid(self, data):
msg = zpidMSG()
msg.header.stamp = rospy.Time.now()
msg.p = data["zpid.p"]
msg.i = data["zpid.i"]
msg.d = data["zpid.d"]
msg.pid = data["zpid.pid"]
self.pub_zpid.publish(msg)
def logCallbackAttitude(self, data):
msg = attitudeMSG()
msg.header.stamp = rospy.Time.now()
q0 = data["attitude.q0"]
q1 = data["attitude.q1"]
q2 = data["attitude.q2"]
q3 = data["attitude.q3"]
msg.q = np.array((q0, q1, q2, q3))
self.pub_attitude.publish(msg)
self.pub_tf.sendTransform((0, 0, 0), (q1, q2, q3, q0),
rospy.Time.now(), "/cf_q", "/world")
def logCallbackGravOffset(self, data):
pass
def logCallbackBaro(self, data):
msg = baroMSG()
msg.header.stamp = rospy.Time.now()
msg.temp = data["baro.temp"]
msg.pressure = data["baro.pressure"]
msg.asl = data["baro.asl"]
msg.aslRaw = data["baro.aslRaw"]
msg.aslLong = data["baro.aslLong"]
self.pub_baro.publish(msg)
#if self.baro_start_time == None:
# self.baro_start_time = rospy.Time.now()
#self.csvwriter.writerow([(msg.header.stamp-self.baro_start_time).to_sec(),msg.asl, msg.asl_raw,msg.temperature,msg.pressure])
def logCallbackBat(self, data):
msg = batMSG()
msg.header.stamp = rospy.Time.now()
msg.link = self.link
msg.bat_v = data["pm.vbat"]
msg.bat_p = (data["pm.vbat"] - 3.0) / 1.15 * 100.
msg.charge_state = data["pm.state_charge"]
msg.state = data["pm.state"]
self.pub_bat.publish(msg)
self.battery_monitor.update(msg.state, msg.charge_state, msg.bat_v)
def logCallbackMotor(self, data):
msg = motorMSG()
msg.header.stamp = rospy.Time.now()
msg.pwm[0] = thrustToPercentage(data["motor.m1"])
msg.pwm[1] = thrustToPercentage(data["motor.m2"])
msg.pwm[2] = thrustToPercentage(data["motor.m3"])
msg.pwm[3] = thrustToPercentage(data["motor.m4"])
msg.thrust = thrustToPercentage(data["motor.thrust"]) / 100.
msg.thrust_raw = data["motor.thrust"]
msg.vLong = data["motor.vLong"]
self.pub_motor.publish(msg)
def connectSetupFinishedCB(self, uri=None):
rospy.loginfo("...Connected")
self.setup_log()
def connectionFailedCB(self, msg=None, errmsg=None):
rospy.logerr("Connection failed: %s", errmsg)
exit()
#self.restart()
def connectionInitiatedCB(self, msg=None):
rospy.loginfo("Connecting to: %s...", msg)
def disconnectedCB(self, msg=None):
self.battery_monitor.done()
if msg != None:
rospy.loginfo("Disconnected from: %s", msg)
def connectionLostCB(self, msg=None, errmsg=None):
rospy.logerr("Connection lost: %s", errmsg)
self.restart()
def linkQualityCB(self, msg=None):
self.link = msg
def consoleCB(self, msg):
rospy.loginfo(msg)
def shutdown(self):
self.crazyflie.close_link()
def main():
rospy.init_node('ur_driver', disable_signals=True)
if rospy.get_param("use_sim_time", False):
rospy.logwarn("use_sim_time is set!!!")
global prevent_programming
reconfigure_srv = Server(URDriverConfig, reconfigure_callback)
prefix = rospy.get_param("~prefix", "")
print "Setting prefix to %s" % prefix
global joint_names
joint_names = [prefix + name for name in JOINT_NAMES]
# Parses command line arguments
parser = optparse.OptionParser(
usage="usage: %prog robot_hostname [reverse_port]")
(options, args) = parser.parse_args(rospy.myargv()[1:])
if len(args) < 1:
parser.error("You must specify the robot hostname")
elif len(args) == 1:
robot_hostname = args[0]
reverse_port = DEFAULT_REVERSE_PORT
elif len(args) == 2:
robot_hostname = args[0]
reverse_port = int(args[1])
if not (0 <= reverse_port <= 65535):
parser.error("You entered an invalid port number")
else:
parser.error("Wrong number of parameters")
# Reads the calibrated joint offsets from the URDF
global joint_offsets
joint_offsets = load_joint_offsets(joint_names)
if len(joint_offsets) > 0:
rospy.loginfo("Loaded calibration offsets from urdf: %s" %
joint_offsets)
else:
rospy.loginfo("No calibration offsets loaded from urdf")
# Reads the maximum velocity
# The max_velocity parameter is only used for debugging in the ur_driver. It's not related to actual velocity limits
global max_velocity
max_velocity = rospy.get_param("~max_velocity", MAX_VELOCITY) # [rad/s]
rospy.loginfo("Max velocity accepted by ur_driver: %s [rad/s]" %
max_velocity)
# Reads the minimum payload
global min_payload
min_payload = rospy.get_param("~min_payload", MIN_PAYLOAD)
# Reads the maximum payload
global max_payload
max_payload = rospy.get_param("~max_payload", MAX_PAYLOAD)
rospy.loginfo("Bounds for Payload: [%s, %s]" % (min_payload, max_payload))
# Sets up the server for the robot to connect to
server = TCPServer(("", reverse_port), CommanderTCPHandler)
thread_commander = threading.Thread(name="CommanderHandler",
target=server.serve_forever)
thread_commander.daemon = True
thread_commander.start()
with open(roslib.packages.get_pkg_dir('ur_driver') + '/prog') as fin:
program = fin.read() % {
"driver_hostname": get_my_ip(robot_hostname, PORT),
"driver_reverseport": reverse_port
}
connection = URConnection(robot_hostname, PORT, program)
connection.connect()
connection.send_reset_program()
connectionRT = URConnectionRT(robot_hostname, RT_PORT)
connectionRT.connect()
set_io_server()
service_provider = None
action_server = None
try:
while not rospy.is_shutdown():
# Checks for disconnect
if getConnectedRobot(wait=False):
time.sleep(0.2)
try:
prevent_programming = rospy.get_param(
"~prevent_programming")
update = {'prevent_programming': prevent_programming}
reconfigure_srv.update_configuration(update)
except KeyError, ex:
print "Parameter 'prevent_programming' not set. Value: " + str(
prevent_programming)
pass
if prevent_programming:
print "Programming now prevented"
connection.send_reset_program()
else:
print "Disconnected. Reconnecting"
if action_server:
action_server.set_robot(None)
rospy.loginfo("Programming the robot")
while True:
# Sends the program to the robot
while not connection.ready_to_program():
print "Waiting to program"
time.sleep(1.0)
try:
prevent_programming = rospy.get_param(
"~prevent_programming")
update = {'prevent_programming': prevent_programming}
reconfigure_srv.update_configuration(update)
except KeyError, ex:
print "Parameter 'prevent_programming' not set. Value: " + str(
prevent_programming)
pass
connection.send_program()
r = getConnectedRobot(wait=True, timeout=1.0)
if r:
break
rospy.loginfo("Robot connected")
#provider for service calls
if service_provider:
service_provider.set_robot(r)
else:
service_provider = URServiceProvider(r, connection)
if action_server:
action_server.set_robot(r)
else:
action_server = URTrajectoryFollower(
r, rospy.Duration(1.0))
action_server.start()
# init dynamic reconfigure server
cfg_srv = Server(PoseControllerConfig, cfg_callback)
set_server_value(cfg_srv)
# Get ros args
if rospy.has_param('~vel_topic'):
vel_topic = rospy.get_param('~vel_topic', vel_topic)
if rospy.has_param('~cmd_vel'):
cmd_vel_topic = rospy.get_param('~cmd_vel', cmd_vel_topic)
if rospy.has_param('~goal_topic'):
goal_topic = rospy.get_param('~goal_topic', goal_topic)
if rospy.has_param('~pose_topic'):
pose_topic = rospy.get_param('~pose_topic', pose_topic)
if rospy.has_param('~max_vel'):
max_vel = rospy.get_param('~max_vel', max_vel)
cfg_srv.update_configuration({"max_vel": max_vel})
if rospy.has_param('~min_vel'):
min_vel = rospy.get_param('~min_vel', min_vel)
cfg_srv.update_configuration({"min_vel": min_vel})
if rospy.has_param('~max_angle'):
max_angle = rospy.get_param('~max_angle', max_angle)
cfg_srv.update_configuration({"max_angle": max_angle})
## PID params
if rospy.has_param(
'~kP_pose'
): #pose means vel in case of repeatativenes but just now
kP_pose = rospy.get_param('~kP_pose', kP_pose)
cfg_srv.update_configuration({"kP_pose": kP_pose})
if rospy.has_param('~kI_pose'):
kI_pose = rospy.get_param('~kI_pose', kI_pose)
cfg_srv.update_configuration({"kI_pose": kI_pose})
class img_interface_node: ## This is the LISTENER for the layer ABOVE
"""The 'img_interface_node' is the class which communicates to the layer above
according to this structure. It provides services and dynamic reconfigure server
and also publishes topics."""
###############################################
## ***** Static Data Members *****
###############################################
# **** Services provided to the above layer
#==================================================
listOf_services = {}
###############################################
## ***** Constructor Method *****
###############################################
def __init__(self, translator, driverMgr):
"""img_interface_node constructor is meant to launch the dynamic reconfigure server
until it is called to launch services for listening. It doesn't receive or return
anything."""
self.translator = translator
self.driverMgr = driverMgr
self.avoidRemoteReconf = 0
try:
## Dynamic Reconfigure
self.avoidRemoteReconf = self.avoidRemoteReconf + 1
# Launch self DynamicReconfigure server
self.dynServer = DynamicReconfigureServer(PropertiesConfig, self.dynServerCallback)
# Launch request listener services
self.listenToRequests()
rospy.loginfo("Interface for requests created.")
except:
rospy.logerr("Error while trying to initialise dynamic parameter server.")
raise
return
###################################################
## ***** General Purpose Methods *****
###################################################
# **** Launch services for receiving requests
#==================================================
def listenToRequests(self):
"""Main services creator method in order to remain listening for requests.
It receives nothing and returns true just for future needs."""
self.listOf_services['get/StringProperty'] = rospy.Service('get/StringProperty', stringValue, self.getStringProperty)
self.listOf_services['get/IntProperty'] = rospy.Service('get/IntProperty', intValue, self.getIntProperty)
self.listOf_services['get/FloatProperty'] = rospy.Service('get/FloatProperty', floatValue, self.getFloatProperty)
self.listOf_services['get/BoolProperty'] = rospy.Service('get/BoolProperty', booleanValue, self.getBoolProperty)
##MICHI: 14Feb2012
self.listOf_services['get/DispImage'] = rospy.Service('get/DispImage', disparityImage, self.getDispImage)
self.listOf_services['get/Image'] = rospy.Service('get/Image', normalImage, self.getImage)
##MICHI: 13Mar2012
self.listOf_services['publishDispImages'] = rospy.Service('publishDispImages', requestTopic, self.publishDispImages)
self.listOf_services['publishImages'] = rospy.Service('publishImages', requestTopic, self.publishImages)
##MICHI: 16Apr2012
self.listOf_services['get/TopicLocation'] = rospy.Service('get/TopicLocation', stringValue, self.getStringProperty)
self.listOf_services['set/TopicLocation'] = rospy.Service('set/TopicLocation', setString, self.setTopicLocation)
self.listOf_services['set/StrProperty'] = rospy.Service('set/StrProperty', setString, self.setStrProperty)
self.listOf_services['set/IntProperty'] = rospy.Service('set/IntProperty', setInteger, self.setIntProperty)
self.listOf_services['set/FloatProperty'] = rospy.Service('set/FloatProperty', setFloat, self.setFloatProperty)
self.listOf_services['set/BoolProperty'] = rospy.Service('set/BoolProperty', setBoolean, self.setBoolProperty)
rospy.loginfo("Ready to answer service requests.")
return True
###################################################
# **** Dynamic Reconfigure related methods
#==================================================
def dynServerCallback(self, dynConfiguration, levelCode):
"""Handler for the changes in the Dynamic Reconfigure server
Must return a configuration in the same format as received."""
if self.avoidRemoteReconf > 0:
rospy.loginfo("LOCAL config was changed by self (levelCode = %s)"%levelCode)
self.avoidRemoteReconf = self.avoidRemoteReconf - 1
elif levelCode != 0:
rospy.loginfo("LOCAL configuration changed.".rjust(80, '_'))
for elem in dynConfiguration:
if self.translator.canSet(elem):
success = self.setAnyProperty(elem, dynConfiguration[elem])
if success == False or success == None:
rospy.logerr('|__> ...error... while setting property "%s"'%elem)
rospy.logerr("Changing %s failed with %s..."%(elem,dynConfiguration[elem]))
dynConfiguration[elem] = self.getAnyProperty(elem)
rospy.logdebug("...%s used"%(dynConfiguration[elem]))
return dynConfiguration
def updateSelfParameters(self, dynConfiguration, avoidPropagation = True):
"""This method is responsible for changing the node's own dynamic reconfigure parameters
from its own code ***but avoiding a chain of uncontrolled callbacks!!.
It returns True if everything went as expected."""
newConfig = {}
for elem in dynConfiguration:
paramName = self.translator.reverseInterpret(elem)
if paramName != "":
newValue = dynConfiguration[elem]
newConfig[paramName] = newValue
else:
rospy.logerr("Error while retrieving reverse translation.")
if avoidPropagation == True:
self.avoidRemoteReconf = self.avoidRemoteReconf + 1
requestResult = self.dynServer.update_configuration(newConfig)
if requestResult != None:
return requestResult
return False
def setTopicLocation(self, setStrMsg):
rospy.loginfo (str("Received call to set/TopicLocation " + setStrMsg.topicName + " " + setStrMsg.newValue))
translation = self.translator.interpret(setStrMsg.topicName)
if translation != None and (translation[PPTY_KIND]=="publishedTopic" or translation[PPTY_KIND]=="topic"):
oldAddress = translation[PPTY_REF]
elif manager3D.is_published(setStrMsg.topicName):
oldAddress = setStrMsg.topicName
else:
print str("The " + setStrMsg.topicName + " topic wasn't found neither as topic or as a name:")
return str("Exception while relocating. Topic not found.")
try:
rospy.loginfo (str("...relocating " + oldAddress + "..."))
self.driverMgr.relocateTopic(oldAddress, setStrMsg.newValue)
## If it was a name; the value needs to be updated in the translator
if oldAddress == setStrMsg.topicName:
self.translator.updateValue(setStrMsg.topicName, setStrMsg.newValue)
except Exception as e:
rospy.logerr("Exception while relocating: %s"%(e))
return str("Exception while relocating: %s"%(e))
return "Success!"
###################################################
# **** Generic handlers for getting and setting parameters
#==================================================
def setAnyProperty(self, propertyName, newValue):
"""Generic setter in order to redirect to the type-specific setter method."""
propertyData = self.translator.interpret(propertyName)
if propertyData == None:
return None
#else:
if (propertyData[PPTY_TYPE].lower()).find("string") >= 0:
# if type(newValue) != str:
# print "Error: value '%s' seems to be %s instead of '%s' which is needed"%(newValue, type(newValue), propertyData[PPTY_TYPE])
# return None
valueType = str
elif (propertyData[PPTY_TYPE].lower()).find("int") >= 0:
valueType = int
elif (propertyData[PPTY_TYPE].lower()).find("double") >= 0 or (propertyData[PPTY_TYPE].lower()).find("float") >= 0:
valueType = float
elif (propertyData[PPTY_TYPE].lower()).find("bool") >= 0:
valueType = bool
else:
rospy.logerr("Error: non registered value type.")
return None
return self.setFixedTypeProperty(propertyName, newValue, valueType)
####################################
# Specific fixed type property setters
#===================================
## The next methods are the callbacks for the setter services
##all of them are supposed to return the resulting values to
##inform in case the set event failed.
## In case there's an error; the error message is sent no
##matter the returning type is
def setStrProperty(self, setterMessage):
return self.setFixedTypeProperty(setterMessage.topicName, setterMessage.newValue, str)
def setIntProperty(self, setterMessage):
return self.setFixedTypeProperty(setterMessage.topicName, setterMessage.newValue, int)
def setFloatProperty(self, setterMessage):
return self.setFixedTypeProperty(setterMessage.topicName, setterMessage.newValue, float)
def setBoolProperty(self, setterMessage):
return self.setFixedTypeProperty(setterMessage.topicName, setterMessage.newValue, bool)
def setFixedTypeProperty(self, propertyName, newValue, valueType):
"""Main property setter receiving the property name, the value to assign and its type
and returning the response from the "setParameter" method from the low level driver."""
propertyData = self.translator.interpret(propertyName)
#TODO: Any type checking here?
if propertyData == None:
rospy.logerr("Error: trying to set not found property.")
return None
# else:
# print "Obtained %s = %s."%(propertyName, propertyData[PPTY_REF])
resp1 = True
if propertyData[PPTY_KIND] == "dynParam":
paramName = propertyTranslator.getBasename(propertyData[PPTY_REF])
resp1 = self.driverMgr.setParameter(paramName, newValue, self.translator.getDynServerPath(paramName), self)
elif propertyData[PPTY_KIND] == "topic":
self.driverMgr.sendByTopic(rospy.get_namespace() + propertyData[PPTY_REF], newValue, remoteType[valueType])
else:
resp1 = False
rospy.logerr("Error: unable to set property %s of kind: '%s'"%(propertyName, propertyData[PPTY_KIND]))
return valueType(resp1)
# Getter and getter handlers
# Generic setter in order to redirect to the appropriate method
def getAnyProperty(self, propertyName):
"""Generic getter in order to redirect to the type-specific getter method."""
propertyData = self.translator.interpret(propertyName)
if propertyData == None:
return None
#else:
if propertyData[PPTY_TYPE].find("string") >= 0:
valueType = str
elif (propertyData[PPTY_TYPE].lower()).find("int") >= 0:
valueType = int
elif (propertyData[PPTY_TYPE].lower()).find("double") >= 0 or (propertyData[PPTY_TYPE].lower()).find("float") >= 0:
valueType = float
elif (propertyData[PPTY_TYPE].lower()).find("bool") >= 0:
valueType = bool
else:
rospy.logerr("Error: non registered value type")
return None
return self.getFixedTypeProperty(propertyName, valueType)
# Generic getter in order to redirect to the appropriate method
#Specific fixed type property getters
def getStringProperty(self, srvMsg):
return self.getFixedTypeProperty(srvMsg.topicName, str)
def getIntProperty(self, srvMsg):
return self.getFixedTypeProperty(srvMsg.topicName, int)
def getFloatProperty(self, srvMsg):
return self.getFixedTypeProperty(srvMsg.topicName, float)
def getBoolProperty(self, srvMsg):
return self.getFixedTypeProperty(srvMsg.topicName, bool)
def getDispImage(self, srvMsg):
respImages = []
while len(respImages) < srvMsg.nImages:
respImages.append(self.getFixedTypeProperty(srvMsg.topicName, None))
for image in respImages:
self.driverMgr.sendByTopic("testImages", image, DisparityImage)
return respImages
def getImage(self, srvMsg):
respImages = []
while len(respImages) < srvMsg.nImages:
respImages.append(self.getFixedTypeProperty(srvMsg.topicName, None))
return respImages
##AMPLIATION: I could add normalImage.timestamp (time[] timestamp) if needed (including the changes in the "normalImage.srv"
## in that case I would need a normalImage variable and set both fields: images and timestamp
def getFixedTypeProperty(self, propertyName, valueType):
"""Main property getter receiving the property name and the value type
and returning the current value from the low level driver."""
propertyData = self.translator.interpret(propertyName)
if propertyData == None:
return None
#else:
if propertyData[PPTY_KIND] == "dynParam":
paramName = propertyTranslator.getBasename(propertyData[PPTY_REF])
resp1 = self.driverMgr.getParameter(paramName, self.translator.getDynServerPath(propertyName))
elif propertyData[PPTY_KIND] == "publishedTopic":
resp1 = self.driverMgr.getTopic(propertyData[PPTY_REF], valueType)
elif propertyData[PPTY_KIND] == "readOnlyParam":
paramName = propertyTranslator.getBasename(propertyData[PPTY_REF])
location = rospy.search_param(param_name)
resp1 = rospy.get_param(location)
elif propertyData[PPTY_KIND] == "topic":
##MICHI: 14Feb2012
if valueType == str:
resp1 = str(propertyData[PPTY_REF])
else: ## Special case for reading disparity images
valueType2 = DisparityImage
if propertyData[PPTY_TYPE].find("Disparity") < 0:
valueType2 = Image
resp1 = self.driverMgr.getTopic(propertyData[PPTY_REF], valueType2)
else:
rospy.logerr("Error: unable to get property %s of kind %s"%(propertyName, propertyData[PPTY_TYPE]))
resp1 = None
return resp1
# Method to request the complete list of properties
def get_property_list(self):
"""Auxiliary method for receiving the list of properties known by the translator."""
return self.translator.get_property_list()
# Methods related to requesting topics to publish
def publishDispImages(self, srvMsg):
"""Specific purpose method meant to retransmit a disparity image topic on a specific topic
path receiving the original path, the new path and the amount of messages. It returns a
message telling if it worked."""
topicPath = self.translator.get_topic_path(srvMsg.sourceTopic)
self.driverMgr.retransmitTopic(srvMsg.nImages, topicPath, srvMsg.responseTopic, DisparityImage)
"%s images sent from %s topic to %s."%(srvMsg.nImages, srvMsg.sourceTopic, srvMsg.responseTopic)
return "%s images sent from %s topic to %s."%(srvMsg.nImages, srvMsg.sourceTopic, srvMsg.responseTopic)
def publishImages(self, srvMsg):
"""Specific purpose method meant to retransmit an image topic on a specific topic
path receiving the original path, the new path and the amount of messages. It returns
a message telling if it worked."""
topicPath = self.translator.get_topic_path(srvMsg.sourceTopic)
self.driverMgr.retransmitTopic(srvMsg.nImages, topicPath, srvMsg.responseTopic, Image)
"%s images sent from %s topic to %s."%(srvMsg.nImages, srvMsg.sourceTopic, srvMsg.responseTopic)
return "%s images sent from %s topic to %s."%(srvMsg.nImages, srvMsg.sourceTopic, srvMsg.responseTopic)
def talker():
global counter_x, counter_y, counter_z, dummy_tag, angle
global cars
# initialize node
rospy.init_node('DWM1001_API', anonymous=False)
# initialize our topics
pub_Network = rospy.Publisher('DWM1001_Network', String, queue_size=10)
pub_Anchor_0 = rospy.Publisher('DWM1001_Network_Anchor_0',
String,
queue_size=10)
pub_Anchor_1 = rospy.Publisher('DWM1001_Network_Anchor_1',
String,
queue_size=10)
pub_Anchor_2 = rospy.Publisher('DWM1001_Network_Anchor_2',
String,
queue_size=10)
pub_Anchor_3 = rospy.Publisher('DWM1001_Network_Anchor_3',
String,
queue_size=10)
pub_Tag = rospy.Publisher('DWM1001_Network_Tag', String, queue_size=10)
srv = Server(GUIConfig, callback)
rate = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
coordinates_tag = dummy_tag.split()
coordinates_anchor_0 = dummy_anchor_0.split()
inc_x = int(float(coordinates_tag[0]) - float(coordinates_anchor_0[0]))
inc_y = int(float(coordinates_tag[1]) - float(coordinates_anchor_0[1]))
angle_to_goal = math.atan2(inc_y, inc_x)
#radius = 15
#theta = math.radians(angle)
#counter_y = radius * math.cos(theta) + 0
#counter_x = radius * math.sin(theta) + 0
#angle = angle + 1
# if angle > 360:
# angle = 0
dummy_tag = str(counter_x) + " " + str(counter_y) + " " + str(
counter_z)
rospy.loginfo("sending dummy values: tag = " + str(dummy_tag))
pub_Anchor_0.publish(dummy_anchor_0)
pub_Anchor_1.publish(dummy_anchor_1)
pub_Anchor_2.publish(dummy_anchor_2)
pub_Anchor_3.publish(dummy_anchor_3)
pub_Tag.publish(dummy_tag)
srv.update_configuration(cars)
#if counter_y > 26:
# counter_x = counter_x + 0.3
#if counter_y > 30:
# counter_y = 30
#if counter_x > 26:
# counter_y = counter_y - 0.3
#if counter_y < 26:
# counter_y = counter_y + 0.3
#hi
counter_y = counter_y + 0.01
#counter_2 = counter_2 + 0.001
#counter_3 = counter_3 + 0.00001
rate.sleep()
class Driver:
def __init__(self, options):
self.options = options
self.crazyflie = Crazyflie()
self.battery_monitor = BatteryMonitor()
cflib.crtp.init_drivers()
self.zspeed = zSpeed()
# Keep track of this to know if the user changed imu mode to/from imu6/9
self.preMagcalib = None
#self.csvwriter = csv.writer(open('baro.csv', 'wb'), delimiter=',',quotechar='"', quoting=csv.QUOTE_MINIMAL)
#self.baro_start_time = None
# Some shortcuts
self.HZ100 = 10
self.HZ10 = 100
self.HZ1 = 1000
self.HZ500 = 2
self.HZ250 = 4
self.HZ125 = 8
self.HZ50 = 20
# Callbacks the CF driver calls
self.crazyflie.connectSetupFinished.add_callback(self.connectSetupFinishedCB)
self.crazyflie.connectionFailed.add_callback(self.connectionFailedCB)
self.crazyflie.connectionInitiated.add_callback(self.connectionInitiatedCB)
self.crazyflie.disconnected.add_callback(self.disconnectedCB)
self.crazyflie.connectionLost.add_callback(self.connectionLostCB)
self.crazyflie.linkQuality.add_callback(self.linkQualityCB)
# Advertise publishers
self.pub_acc = rospy.Publisher("/cf/acc", accMSG)
self.pub_mag = rospy.Publisher("/cf/mag", magMSG)
self.pub_gyro = rospy.Publisher("/cf/gyro", gyroMSG)
self.pub_baro = rospy.Publisher("/cf/baro", baroMSG)
self.pub_motor = rospy.Publisher("/cf/motor", motorMSG)
self.pub_hover = rospy.Publisher("/cf/hover", hoverMSG)
self.pub_attitude = rospy.Publisher("/cf/attitude", attitudeMSG)
self.pub_bat = rospy.Publisher("/cf/bat", batMSG)
self.pub_zpid = rospy.Publisher("/cf/zpid", zpidMSG)
self.pub_tf = tf.TransformBroadcaster()
# Subscribers
self.sub_tf = tf.TransformListener()
self.sub_joy = rospy.Subscriber("/cfjoy", joyMSG, self.new_joydata)
self.sub_magCalib = rospy.Subscriber("/magCalib", magCalibMSG, self.new_magCalib)
# Keep track of link quality to send out with battery status
self.link = 0
# Loggers
self.logMotor = None
self.logBaro = None
self.logMag = None
self.logGyro = None
self.logAttitude = None
self.logAcc = None
self.logBat = None
self.logHover = None
self.logZPid = None
self.logGravOffset = None #only here till it works
# keep tracks if loggers are running
self.acc_monitor = False
self.gyro_monitor = False
self.baro_monitor = False
self.mag_monitor = False
self.bat_monitor = False
self.motor_monitor = False
self.attitude_monitor = False
self.hover_monitor = False
self.zpid_monitor = False
# CF params we wanna be able to change
self.cf_param_groups = ["hover", "sensorfusion6", "magCalib"]
self.cf_params_cache = {}
# Dynserver
self.dynserver = DynamicReconfigureServer(driverCFG, self.reconfigure)
# Finished set up. Connect to flie
self.connect(self.options)
def new_magCalib(self, msg):
""" Receive magnetometer calibration results. Send to flie via dyn conf"""
new_settings = {}
#TODO
new_settings["magCalib_off_x"] = msg.offset[0]
new_settings["magCalib_off_y"] = msg.offset[1]
new_settings["magCalib_off_z"] = msg.offset[2]
new_settings["magCalib_thrust_x"] = msg.thrust_comp[0]
new_settings["magCalib_thrust_y"] = msg.thrust_comp[1]
new_settings["magCalib_thrust_z"] = msg.thrust_comp[2]
new_settings["magCalib_scale_x"] = msg.scale[0]
new_settings["magCalib_scale_y"] = msg.scale[1]
new_settings["magCalib_scale_z"] = msg.scale[2]
self.dynserver.update_configuration(new_settings)
rospy.loginfo("Updated magnetometer calibration data")
def connect(self, options):
""" Look for crazyflie every 2s and connect to the specified one if found"""
rospy.loginfo("Waiting for crazyflie...")
while not rospy.is_shutdown():
interfaces = cflib.crtp.scan_interfaces()
if len(interfaces)>1:
radio = None
rospy.loginfo("Found: ")
for i in interfaces:
rospy.loginfo("--> %s [%s]", i[0],i[1])
if i[0].startswith(options.uri):
radio = i[0]
if radio!=None:
self.crazyflie.open_link(radio)
break
else:
rospy.sleep(2)
def new_joydata(self, joymsg):
""" Joydata arrived. Should happen at 100hz."""
# Parse joydata and extract commands
roll = joymsg.roll
pitch = joymsg.pitch
yawrate = joymsg.yaw
thrust = joymsg.thrust
hover = joymsg.hover
set_hover = joymsg.hover_set
hover_change = joymsg.hover_change
# Send to flie
self.send_control((roll, pitch, yawrate, thrust, hover, set_hover, hover_change))
def reconfigure(self, config, level):
""" Fill in local variables with values received from dynamic reconfigure clients (typically the GUI)."""
config = self.zspeed.reconfigure(config)
self.deadband = config["deadband"]
# On / off logging
if self.zpid_monitor != config["read_zpid"]:
self.zpid_monitor = config["read_zpid"]
if self.logZPid != None:
if self.zpid_monitor:
self.logZPid.start()
rospy.loginfo("zPID Logging started")
else:
self.logZPid.stop()
rospy.loginfo("zPID Logging stopped")
if self.gyro_monitor != config["read_gyro"]:
self.gyro_monitor = config["read_gyro"]
if self.logGyro != None:
if self.gyro_monitor:
self.logGyro.start()
rospy.loginfo("Gyro Logging started")
else:
self.logGyro.stop()
rospy.loginfo("Gyro Logging stopped")
if self.hover_monitor != config["read_hover"]:
self.hover_monitor = config["read_hover"]
if self.logHover != None:
if self.hover_monitor:
self.logHover.start()
rospy.loginfo("Hover Logging started")
else:
self.logHover.stop()
rospy.loginfo("Hover Logging stopped")
if self.acc_monitor != config["read_acc"]:
self.acc_monitor = config["read_acc"]
if self.logGyro != None:
if self.acc_monitor:
self.logAcc.start()
rospy.loginfo("Acc Logging started")
else:
self.logAcc.stop()
rospy.loginfo("Acc Logging stopped")
if self.baro_monitor != config["read_baro"]:
self.baro_monitor = config["read_baro"]
if self.logBaro != None:
if self.baro_monitor:
self.logBaro.start()
rospy.loginfo("Baro Logging started")
else:
self.logBaro.stop()
rospy.loginfo("Baro Logging stopped")
if self.mag_monitor != config["read_mag"]:
self.mag_monitor = config["read_mag"]
if self.logMag != None:
if self.mag_monitor:
self.logMag.start()
rospy.loginfo("Mag Logging started")
else:
self.logMag.stop()
rospy.loginfo("Mag Logging stopped")
if self.bat_monitor != config["read_bat"]:
self.bat_monitor = config["read_bat"]
if self.logBat != None:
if self.bat_monitor:
self.logBat.start()
rospy.loginfo("Battery/Link Logging started")
else:
self.logBat.stop()
rospy.loginfo("Battery/Link Logging stopped")
if self.attitude_monitor != config["read_attitude"]:
self.attitude_monitor = config["read_attitude"]
if self.logAttitude != None:
if self.attitude_monitor:
self.logAttitude.start()
rospy.loginfo("Attitude Logging started")
else:
self.logAttitude.stop()
rospy.loginfo("Attitude Logging stopped")
if self.motor_monitor != config["read_motor"]:
self.motor_monitor = config["read_motor"]
if self.logMotor != None:
if self.motor_monitor:
self.logMotor.start()
rospy.loginfo("Motor Logging started")
else:
self.logMotor.stop()
rospy.loginfo("Motor Logging stopped")
# SET CRAZYFLIE PARAMS
for key, value in config.iteritems():
# Skip this key (whats its purpose??)
if key == "groups":
continue
# Continue if variable exists and is correct
if self.cf_params_cache.has_key(key):
if config[key] == self.cf_params_cache[key]:
# Nothing changed
if not key.startswith("magCalib") or ( self.preMagcalib or not config["sensorfusion6_magImu"]):
continue
# Doesnt exist or has changed
# Split into group and name
s = key.split("_",1)
# Make sure it is a group name pair
if len(s) == 2:
# make sure group is valid
if s[0] in self.cf_param_groups:
# update cache, send variable to flie
self.cf_params_cache[key] = value
self.send_param(s[0]+"."+s[1], value)
print "updated: ",s[0]+"."+s[1],"->",value
rospy.sleep(0.1)
self.preMagcalib = config["sensorfusion6_magImu"]
return config
def send_param(self, key, value):
self.crazyflie.param.set_value(key, str(value))
def restart(self):
rospy.loginfo("Restarting Driver")
rospy.sleep(1)
self.connect(self.options)
def logErrorCB(self, errmsg):
rospy.logerr("Log error: %s", errmsg)
def send_control(self,cmd):
""" Roll, pitch in deg, yaw in deg/s, thrust in 10000-60000, hover bool, set_hover bool, hover chance float -1 to +1 """
roll, pitch, yawrate, thrust, hover, set_hover, hover_change = cmd
if self.crazyflie.state == CFState.CONNECTED:
self.crazyflie.commander.send_setpoint(roll, pitch, yawrate, thrust, hover, set_hover, hover_change)
def setup_log(self):
""" Console callbacks """
self.crazyflie.console.receivedChar.add_callback(self.consoleCB)
rospy.sleep(0.25)
""" ATTITUDE LOGGING @ 100hz """
logconf = LogConfig("attitude", self.HZ100 * 2) #ms
logconf.addVariable(LogVariable("attitude.q0", "float"))
logconf.addVariable(LogVariable("attitude.q1", "float"))
logconf.addVariable(LogVariable("attitude.q2", "float"))
logconf.addVariable(LogVariable("attitude.q3", "float"))
self.logAttitude = self.crazyflie.log.create_log_packet(logconf)
if (self.logAttitude is not None):
self.logAttitude.dataReceived.add_callback(self.logCallbackAttitude)
self.logAttitude.error.add_callback(self.logErrorCB)
if self.attitude_monitor:
self.logAttitude.start()
rospy.loginfo("Attitude Logging started")
else:
rospy.logwarn("Could not setup Attitude logging!")
rospy.sleep(0.25)
""" ZPID LOGGING @ 100hz """
logconf = LogConfig("zpid", self.HZ100) #ms
logconf.addVariable(LogVariable("zpid.p", "float"))
logconf.addVariable(LogVariable("zpid.i", "float"))
logconf.addVariable(LogVariable("zpid.d", "float"))
logconf.addVariable(LogVariable("zpid.pid", "float"))
self.logZPid = self.crazyflie.log.create_log_packet(logconf)
if (self.logZPid is not None):
self.logZPid.dataReceived.add_callback(self.logCallbackZPid)
self.logZPid.error.add_callback(self.logErrorCB)
if self.zpid_monitor:
self.logZPid.start()
rospy.loginfo("ZPID Logging started")
else:
rospy.logwarn("Could not setup ZPID logging!")
rospy.sleep(0.25)
""" HOVER LOGGING @ 100hz """
logconf = LogConfig("hover", self.HZ100) #ms
logconf.addVariable(LogVariable("hover.err", "float"))
logconf.addVariable(LogVariable("hover.target", "float"))
logconf.addVariable(LogVariable("hover.zSpeed", "float"))
logconf.addVariable(LogVariable("hover.zBias", "float"))
logconf.addVariable(LogVariable("hover.acc_vspeed", "float"))
logconf.addVariable(LogVariable("hover.asl_vspeed", "float"))
self.logHover = self.crazyflie.log.create_log_packet(logconf)
if (self.logHover is not None):
self.logHover.dataReceived.add_callback(self.logCallbackHover)
self.logHover.error.add_callback(self.logErrorCB)
if self.hover_monitor:
self.logHover.start()
rospy.loginfo("Hover Logging started")
else:
rospy.logwarn("Could not setup Hover logging!")
rospy.sleep(0.25)
""" GYROMETER LOGGING @ 100hz """
logconf = LogConfig("LoggingGyro", self.HZ100) #ms
logconf.addVariable(LogVariable("gyro.x", "float"))
logconf.addVariable(LogVariable("gyro.y", "float"))
logconf.addVariable(LogVariable("gyro.z", "float"))
self.logGyro = self.crazyflie.log.create_log_packet(logconf)
if (self.logGyro is not None):
self.logGyro.dataReceived.add_callback(self.logCallbackGyro)
self.logGyro.error.add_callback(self.logErrorCB)
if self.gyro_monitor:
self.logGyro.start()
rospy.loginfo("Gyro Logging started")
else:
rospy.logwarn("Could not setup Gyro logging!")
rospy.sleep(0.25)
""" ACCELEROMETER LOGGING @ 100hz """
logconf = LogConfig("LoggingAcc", self.HZ100) #ms
logconf.addVariable(LogVariable("acc.x", "float"))
logconf.addVariable(LogVariable("acc.y", "float"))
logconf.addVariable(LogVariable("acc.z", "float"))
logconf.addVariable(LogVariable("acc.xw", "float"))
logconf.addVariable(LogVariable("acc.yw", "float"))
logconf.addVariable(LogVariable("acc.zw", "float"))
self.logAcc = self.crazyflie.log.create_log_packet(logconf)
if (self.logAcc is not None):
self.logAcc.dataReceived.add_callback(self.logCallbackAcc)
self.logAcc.error.add_callback(self.logErrorCB)
if self.acc_monitor:
self.logAcc.start()
rospy.loginfo("Acc Logging started")
else:
rospy.logwarn("Could not setup Acc logging!")
rospy.sleep(0.25)
""" MAGNETOMETER LOGGING @ 100hz """
logconf = LogConfig("LoggingMag", self.HZ100) #ms
logconf.addVariable(LogVariable("mag.x", "float"))
logconf.addVariable(LogVariable("mag.y", "float"))
logconf.addVariable(LogVariable("mag.z", "float"))
logconf.addVariable(LogVariable("mag.x_raw", "float"))
logconf.addVariable(LogVariable("mag.y_raw", "float"))
logconf.addVariable(LogVariable("mag.z_raw", "float"))
self.logMag = self.crazyflie.log.create_log_packet(logconf)
if (self.logMag is not None):
self.logMag.dataReceived.add_callback(self.logCallbackMag)
self.logMag.error.add_callback(self.logErrorCB)
if self.mag_monitor:
self.logMag.start()
rospy.loginfo("Mag Logging started")
else:
rospy.logwarn("Could not setup Mag logging!")
rospy.sleep(0.25)
""" BAROMETER LOGGING @ 100hz """
logconf = LogConfig("LoggingBaro", self.HZ100) #ms
logconf.addVariable(LogVariable("baro.aslRaw", "float"))
logconf.addVariable(LogVariable("baro.asl", "float"))
logconf.addVariable(LogVariable("baro.temp", "float"))
logconf.addVariable(LogVariable("baro.pressure", "float"))
logconf.addVariable(LogVariable("baro.aslLong", "float"))
self.logBaro = self.crazyflie.log.create_log_packet(logconf)
if (self.logBaro is not None):
self.logBaro.dataReceived.add_callback(self.logCallbackBaro)
self.logBaro.error.add_callback(self.logErrorCB)
if self.baro_monitor:
self.logBaro.start()
rospy.loginfo("Baro Logging started")
else:
rospy.logwarn("Could not setup Baro logging!")
rospy.sleep(0.25)
""" BATTERY/LINK LOGGING @ 10hz """
logconf = LogConfig("LoggingBat", self.HZ10) #ms
logconf.addVariable(LogVariable("pm.vbat", "float"))
logconf.addVariable(LogVariable("pm.state", "uint8_t"))
logconf.addVariable(LogVariable("pm.state_charge", "uint8_t"))
self.logBat = self.crazyflie.log.create_log_packet(logconf)
if (self.logBat is not None):
self.logBat.dataReceived.add_callback(self.logCallbackBat)
self.logBat.error.add_callback(self.logErrorCB)
if self.bat_monitor:
self.logBat.start()
rospy.loginfo("Bat Logging started")
else:
rospy.logwarn("Could not setup Battery/Link logging!")
rospy.sleep(0.25)
#TODO motor logging not working
""" MOTOR LOGGING @ 100hz"""
logconf = LogConfig("LoggingMotor", self.HZ100) #ms
logconf.addVariable(LogVariable("motor.m1", "uint32_t"))
logconf.addVariable(LogVariable("motor.m2", "uint32_t"))
logconf.addVariable(LogVariable("motor.m3", "uint32_t"))
logconf.addVariable(LogVariable("motor.m4", "uint32_t"))
logconf.addVariable(LogVariable("motor.thrust", "uint16_t"))
logconf.addVariable(LogVariable("motor.vLong", "float")) #TODO move
self.logMotor = self.crazyflie.log.create_log_packet(logconf)
if (self.logMotor is not None):
self.logMotor.dataReceived.add_callback(self.logCallbackMotor)
self.logMotor.error.add_callback(self.logErrorCB)
if self.motor_monitor:
self.logMotor.start()
rospy.loginfo("Motor Logging started")
else:
rospy.logwarn("Could not setup Motor logging!")
def logCallbackAcc(self, data):
"""Accelerometer axis data in mG --> G"""
msg = accMSG()
msg.header.stamp = rospy.Time.now()
msg.acc[0] = data["acc.x"]
msg.acc[1] = data["acc.y"]
msg.acc[2] = data["acc.z"]
msg.acc_w[0] = data["acc.xw"]
msg.acc_w[1] = data["acc.yw"]
msg.acc_w[2] = data["acc.zw"]
self.zspeed.add(msg.acc_w[2])
self.pub_acc.publish(msg)
self.pub_tf.sendTransform(msg.acc_w,(0,0,0,1),
rospy.Time.now(),
"/acc_w","/cf_q")
self.pub_tf.sendTransform(msg.acc,(0,0,0,1),
rospy.Time.now(),
"/acc", "/cf_q")
def logCallbackMag(self, data):
"""TODO UNITS"""
msg = magMSG()
msg.header.stamp = rospy.Time.now()
msg.mag[0] = data["mag.x"]
msg.mag[1] = data["mag.y"]
msg.mag[2] = data["mag.z"]
msg.magRaw[0] = data["mag.x_raw"]
msg.magRaw[1] = data["mag.y_raw"]
msg.magRaw[2] = data["mag.z_raw"]
self.pub_mag.publish(msg)
if np.linalg.norm(msg.magRaw)>1:
self.pub_tf.sendTransform(msg.mag/np.linalg.norm(msg.magRaw),(0,0,0,1),
rospy.Time.now(),
"/mag_raw",
"/cf_q")
if np.linalg.norm(msg.mag)>1:
self.pub_tf.sendTransform(msg.mag/np.linalg.norm(msg.mag),(0,0,0,1),
rospy.Time.now(),
"/mag_calib",
"/cf_q")
def logCallbackGyro(self, data):
"""Gyro axis data in deg/s -> rad/s"""
msg = gyroMSG()
msg.header.stamp = rospy.Time.now()
msg.gyro[0] = radians(data["gyro.x"])
msg.gyro[1] = radians(data["gyro.y"])
msg.gyro[2] = radians(data["gyro.z"])
self.pub_gyro.publish(msg)
def logCallbackHover(self, data):
"""Output data regarding hovering"""
msg = hoverMSG()
msg.header.stamp = rospy.Time.now()
msg.err = deadband(data["hover.err"], self.deadband)
msg.zSpeed = data["hover.zSpeed"]
msg.acc_vspeed = data["hover.acc_vspeed"]# * self.cf_params_cache["hover_acc_vspeedFac"]
msg.asl_vspeed = data["hover.asl_vspeed"]# * self.cf_params_cache["hover_asl_vspeedFac"]
msg.target = data["hover.target"]
msg.zBias = data["hover.zBias"]
self.pub_hover.publish(msg)
def logCallbackZPid(self, data):
msg = zpidMSG()
msg.header.stamp = rospy.Time.now()
msg.p = data["zpid.p"]
msg.i = data["zpid.i"]
msg.d = data["zpid.d"]
msg.pid = data["zpid.pid"]
self.pub_zpid.publish(msg)
def logCallbackAttitude(self, data):
msg = attitudeMSG()
msg.header.stamp = rospy.Time.now()
q0 = data["attitude.q0"]
q1 = data["attitude.q1"]
q2 = data["attitude.q2"]
q3 = data["attitude.q3"]
msg.q = np.array((q0,q1,q2,q3))
self.pub_attitude.publish(msg)
self.pub_tf.sendTransform((0, 0, 0),(q1,q2,q3,q0),
rospy.Time.now(),
"/cf_q",
"/world")
def logCallbackGravOffset(self, data):
pass
def logCallbackBaro(self, data):
msg = baroMSG()
msg.header.stamp = rospy.Time.now()
msg.temp = data["baro.temp"]
msg.pressure = data["baro.pressure"]
msg.asl = data["baro.asl"]
msg.aslRaw = data["baro.aslRaw"]
msg.aslLong = data["baro.aslLong"]
self.pub_baro.publish(msg)
#if self.baro_start_time == None:
# self.baro_start_time = rospy.Time.now()
#self.csvwriter.writerow([(msg.header.stamp-self.baro_start_time).to_sec(),msg.asl, msg.asl_raw,msg.temperature,msg.pressure])
def logCallbackBat(self, data):
msg = batMSG()
msg.header.stamp = rospy.Time.now()
msg.link = self.link
msg.bat_v = data["pm.vbat"]
msg.bat_p = (data["pm.vbat"]-3.0)/1.15*100.
msg.charge_state = data["pm.state_charge"]
msg.state = data["pm.state"]
self.pub_bat.publish(msg)
self.battery_monitor.update(msg.state, msg.charge_state, msg.bat_v)
def logCallbackMotor(self, data):
msg = motorMSG()
msg.header.stamp = rospy.Time.now()
msg.pwm[0] = thrustToPercentage(data["motor.m1"])
msg.pwm[1] = thrustToPercentage(data["motor.m2"])
msg.pwm[2] = thrustToPercentage(data["motor.m3"])
msg.pwm[3] = thrustToPercentage(data["motor.m4"])
msg.thrust = thrustToPercentage(data["motor.thrust"])/100.
msg.thrust_raw = data["motor.thrust"]
msg.vLong = data["motor.vLong"]
self.pub_motor.publish(msg)
def connectSetupFinishedCB(self, uri=None):
rospy.loginfo("...Connected")
self.setup_log()
def connectionFailedCB(self, msg=None, errmsg=None):
rospy.logerr("Connection failed: %s", errmsg)
exit()
#self.restart()
def connectionInitiatedCB(self, msg=None):
rospy.loginfo("Connecting to: %s...", msg)
def disconnectedCB(self, msg=None):
self.battery_monitor.done()
if msg!=None:
rospy.loginfo("Disconnected from: %s", msg)
def connectionLostCB(self, msg=None, errmsg=None):
rospy.logerr("Connection lost: %s", errmsg)
self.restart()
def linkQualityCB(self, msg=None):
self.link = msg
def consoleCB(self, msg):
rospy.loginfo(msg)
def shutdown(self):
self.crazyflie.close_link()
class Behavior:
def InitSaliencyCounter(self):
self.saliency_counter = random.randint(int(self.saliency_time_min * self.synthesizer_rate),int(self.saliency_time_max * self.synthesizer_rate))
def InitFacesCounter(self):
self.faces_counter = random.randint(int(self.faces_time_min * self.synthesizer_rate),int(self.faces_time_max * self.synthesizer_rate))
def InitEyesCounter(self):
self.eyes_counter = random.randint(int(self.eyes_time_min * self.synthesizer_rate),int(self.eyes_time_max * self.synthesizer_rate))
def InitAudienceCounter(self):
self.audience_counter = random.randint(int(self.audience_time_min * self.synthesizer_rate),int(self.audience_time_max * self.synthesizer_rate))
def InitGestureCounter(self):
self.gesture_counter = random.randint(int(self.gesture_time_min * self.synthesizer_rate),int(self.gesture_time_max * self.synthesizer_rate))
def InitExpressionCounter(self):
self.expression_counter = random.randint(int(self.expression_time_min * self.synthesizer_rate),int(self.expression_time_max * self.synthesizer_rate))
def InitAllFacesStartCounter(self):
self.all_faces_start_counter = random.randint(int(self.all_faces_start_time_min * self.synthesizer_rate),int(self.all_faces_start_time_max * self.synthesizer_rate))
def InitAllFacesDurationCounter(self):
self.all_faces_duration_counter = random.randint(int(self.all_faces_duration_min * self.synthesizer_rate),int(self.all_faces_duration_max * self.synthesizer_rate))
def __init__(self):
# create lock
self.lock = threading.Lock()
self.robot_name = rospy.get_param("/robot_name")
self.config_dir = os.path.join(rospy.get_param("/robots_config_dir"), 'heads', self.robot_name)
# setup face, hand and saliency structures
self.faces = {} # index = cface_id, which should be relatively steady from vision_pipeline
self.current_face_id = 0 # cface_id of current face
self.last_face_id = 0 # most recent cface_id of added face
self.last_talk_ts = 0 # ts of last seen face or talking
self.hand = None # current hand
self.last_hand_ts = 0 # ts of last seen hand
self.saliencies = {} # index = ts, and old saliency vectors will be removed after time
self.current_saliency_ts = 0 # ts of current saliency vector
self.current_eye = 0 # current eye (0 = left, 1 = right, 2 = mouth)
self.gaze_delay_counter = 0 # delay counter after with gaze or head follows head or gaze
self.gaze_pos = None # current gaze position
# animations
self.animations = None
self.current_gestures_name = None
self.current_expressions_name = None
self.tf_listener = tf.TransformListener(False, rospy.Duration(1))
# setup dynamic reconfigure parameters
self.enable_flag = True
self.synthesizer_rate = 10.0
self.keep_time = 1.0
self.saliency_time_min = 0.1
self.saliency_time_max = 3.0
self.faces_time_min = 0.1
self.faces_time_max = 3.0
self.eyes_time_min = 0.1
self.eyes_time_max = 3.0
self.audience_time_min = 0.1
self.audience_time_max = 3.0
self.gesture_time_min = 0.1
self.gesture_time_max = 3.0
self.expression_time_min = 0.1
self.expression_time_max = 3.0
self.InitSaliencyCounter()
self.InitFacesCounter()
self.InitEyesCounter()
self.InitAudienceCounter()
self.InitGestureCounter()
self.InitExpressionCounter()
self.hand_state_decay = 2.0
self.face_state_decay = 2.0
self.gaze_delay = 1.0
self.gaze_speed = 0.5
self.all_faces_start_time_min = 4.0
self.all_faces_start_time_max = 6.0
self.all_faces_duration_min = 2.0
self.all_faces_duration_max = 4.0
self.InitAllFacesStartCounter()
self.InitAllFacesDurationCounter()
self.eyecontact = EyeContact.IDLE
self.lookat = LookAt.IDLE
self.mirroring = Mirroring.IDLE
self.gaze = Gaze.GAZE_ONLY
self.state = State.SLEEPING
# take candidate streams exactly like RealSense Tracker until fusion is better defined and we can rely on combined camera stuff
rospy.Subscriber('/{}/perception/realsense/cface'.format(self.robot_name), CandidateFace, self.HandleFace)
rospy.Subscriber('/{}/perception/realsense/chand'.format(self.robot_name), CandidateHand, self.HandleHand)
rospy.Subscriber('/{}/perception/wideangle/csaliency'.format(self.robot_name), CandidateSaliency, self.HandleSaliency)
rospy.Subscriber('/{}/perception/acousticmagic/raw_audiodir'.format(self.robot_name), AudioDirection, self.HandleAudioDirection)
rospy.Subscriber('/{}/perception/motion/raw_motion'.format(self.robot_name), MotionVector, self.HandleMotion)
rospy.Subscriber('/{}/chat_events'.format(self.robot_name), String, self.HandleChatEvents)
rospy.Subscriber('/{}/speech_events'.format(self.robot_name), String, self.HandleSpeechEvents)
self.head_focus_pub = rospy.Publisher('/blender_api/set_face_target', Target, queue_size=1)
self.gaze_focus_pub = rospy.Publisher('/blender_api/set_gaze_target', Target, queue_size=1)
self.expressions_pub = rospy.Publisher('/blender_api/set_emotion_state', EmotionState, queue_size=1)
self.gestures_pub = rospy.Publisher('/blender_api/set_gesture', SetGesture, queue_size=1)
self.animationmode_pub = rospy.Publisher('/blender_api/set_animation_mode', UInt8, queue_size=1)
self.setpau_pub = rospy.Publisher('/blender_api/set_pau', pau, queue_size=1)
self.tts_pub = rospy.Publisher('/{}/tts'.format(self.robot_name), TTS, queue_size=1) # for debug messages
self.hand_events_pub = rospy.Publisher('/hand_events', String, queue_size=1)
# dynamic reconfigure client to the vision pipelines
self.lefteye_config = dynamic_reconfigure.client.Client("/{}/perception/lefteye/vision_pipeline".format(self.robot_name),timeout=30,config_callback=self.HandleLeftEyeConfig)
self.righteye_config = dynamic_reconfigure.client.Client("/{}/perception/righteye/vision_pipeline".format(self.robot_name),timeout=30,config_callback=self.HandleRightEyeConfig)
self.wideangle_config = dynamic_reconfigure.client.Client("/{}/perception/wideangle/vision_pipeline".format(self.robot_name),timeout=30,config_callback=self.HandleWideAngleConfig)
self.realsense_config = dynamic_reconfigure.client.Client("/{}/perception/realsense/vision_pipeline".format(self.robot_name),timeout=30,config_callback=self.HandleRealSenseConfig)
# TEMP: set all pipelines to 1Hz
self.lefteye_config.update_configuration({"pipeline_rate":1.0,"detect_rate":1.0})
self.righteye_config.update_configuration({"pipeline_rate":1.0,"detect_rate":1.0})
self.wideangle_config.update_configuration({"pipeline_rate":1.0,"detect_rate":1.0})
self.realsense_config.update_configuration({"pipeline_rate":1.0,"detect_rate":1.0})
# start timer
self.config_server = FakeConfigServer() # this is a workaround because self.HandleTimer could be triggered before the config_server actually exists
self.timer = rospy.Timer(rospy.Duration(1.0 / self.synthesizer_rate),self.HandleTimer)
# start dynamic reconfigure server
self.config_server = Server(BehaviorConfig, self.HandleConfig)
def UpdateStateDisplay(self):
self.config_server.update_configuration({
"eyecontact_state":self.eyecontact,
"lookat_state":self.lookat,
"mirroring_state":self.mirroring,
"gaze_state":self.gaze,
"eyecontact_state":self.eyecontact
})
def HandleConfig(self, config, level):
# Load gestures and expressions from configs first time loaded
if config.reload_animations or (self.animations == None):
try:
self.animations = YamlConfig.load(self.config_dir, 'r2_behavior_anim.yaml')
except IOError:
self.animations = YamlConfig.load(os.path.join(os.path.dirname(os.path.dirname(__file__)),'cfg'),
'r2_behavior_anim.default.yaml')
config.reload_animations = False
if self.current_gestures_name == None:
self.current_gestures_name = "idle_gestures"
if self.current_expressions_name == None:
self.current_expressions_name = "idle_expressions"
if self.enable_flag != config.enable_flag:
self.enable_flag = config.enable_flag
# TODO: enable or disable the behaviors
if self.synthesizer_rate != config.synthesizer_rate:
self.synthesizer_rate = config.synthesizer_rate
self.timer.shutdown()
self.timer = rospy.Timer(rospy.Duration(1.0 / self.synthesizer_rate),self.HandleTimer)
self.InitSaliencyCounter()
self.InitFacesCounter()
self.InitEyesCounter()
self.InitAudienceCounter()
self.InitGestureCounter()
self.InitExpressionCounter()
self.InitAllFacesStartCounter()
self.InitAllFacesDurationCounter()
# keep time
self.keep_time = config.keep_time
# update the counter ranges (and counters if the ranges changed)
if config.saliency_time_max < config.saliency_time_min:
config.saliency_time_max = config.saliency_time_min
if config.saliency_time_min != self.saliency_time_min or config.saliency_time_max != self.saliency_time_max:
self.saliency_time_min = config.saliency_time_min
self.saliency_time_max = config.saliency_time_max
self.InitSaliencyCounter()
if config.faces_time_max < config.faces_time_min:
config.faces_time_max = config.faces_time_min
if config.faces_time_min != self.faces_time_min or config.faces_time_max != self.faces_time_max:
self.faces_time_min = config.faces_time_min
self.faces_time_max = config.faces_time_max
self.InitFacesCounter()
if config.eyes_time_max < config.eyes_time_min:
config.eyes_time_max = config.eyes_time_min
if config.eyes_time_min != self.eyes_time_min or config.eyes_time_max != self.eyes_time_max:
self.eyes_time_min = config.eyes_time_min
self.eyes_time_max = config.eyes_time_max
self.InitEyesCounter()
if config.audience_time_max < config.audience_time_min:
config.audience_time_max = config.audience_time_min
if config.audience_time_min != self.audience_time_min or config.audience_time_max != self.audience_time_max:
self.audience_time_min = config.audience_time_min
self.audience_time_max = config.audience_time_max
self.InitAudienceCounter()
if config.gesture_time_max < config.gesture_time_min:
config.gesture_time_max = config.gesture_time_min
if config.gesture_time_min != self.gesture_time_min or config.gesture_time_max != self.gesture_time_max:
self.gesture_time_min = config.gesture_time_min
self.gesture_time_max = config.gesture_time_max
self.InitGestureCounter()
if config.expression_time_max < config.expression_time_min:
config.expression_time_max = config.expression_time_min
if config.expression_time_min != self.expression_time_min or config.expression_time_max != self.expression_time_max:
self.expression_time_min = config.expression_time_min
self.expression_time_max = config.expression_time_max
self.InitExpressionCounter()
self.hand_state_decay = config.hand_state_decay
self.face_state_decay = config.face_state_decay
self.gaze_delay = config.gaze_delay
self.gaze_speed = config.gaze_speed
if config.all_faces_start_time_max < config.all_faces_start_time_min:
config.all_faces_start_time_max = config.all_faces_start_time_min
if config.all_faces_start_time_min != self.all_faces_start_time_min or config.all_faces_start_time_max != self.all_faces_start_time_max:
self.all_faces_start_time_min = config.all_faces_start_time_min
self.all_faces_start_time_max = config.all_faces_start_time_max
self.InitAllFacesStartCounter()
if config.all_faces_duration_max < config.all_faces_duration_min:
config.all_faces_duration_max = config.all_faces_duration_min
if config.all_faces_duration_min != self.all_faces_duration_min or config.all_faces_duration_max != self.all_faces_duration_max:
self.all_faces_duration_min = config.all_faces_duration_min
self.all_faces_duration_max = config.all_faces_duration_max
self.InitAllFacesDurationCounter()
# and set the states for each state machine
self.SetEyeContact(config.eyecontact_state)
self.SetLookAt(config.lookat_state)
self.SetMirroring(config.mirroring_state)
self.SetGaze(config.gaze_state)
# and finally the overall state
self.SetState(config.state)
return config
def HandleLeftEyeConfig(self,config):
return config
def HandleRightEyeConfig(self,config):
return config
def HandleWideAngleConfig(self,config):
return config
def HandleRealSenseConfig(self,config):
return config
def Say(self,text):
# publish TTS message
msg = TTS()
msg.text = text
msg.lang = 'en-US'
self.tts_pub.publish(msg)
def SetGazeFocus(self,pos,speed):
msg = Target()
msg.x = pos.x
msg.y = pos.y
msg.z = pos.z
msg.speed = speed
self.gaze_focus_pub.publish(msg)
def SetHeadFocus(self,pos,speed):
msg = Target()
msg.x = pos.x
msg.y = pos.y
msg.z = pos.z
msg.speed = speed
self.head_focus_pub.publish(msg)
def UpdateGaze(self,pos):
self.gaze_pos = pos
if self.gaze == Gaze.GAZE_ONLY:
self.SetGazeFocus(pos,5.0)
elif self.gaze == Gaze.HEAD_ONLY:
self.SetHeadFocus(pos,3.0)
elif self.gaze == Gaze.GAZE_AND_HEAD:
self.SetGazeFocus(pos,5.0)
self.SetHeadFocus(pos,3.0)
elif self.gaze == Gaze.GAZE_LEADS_HEAD:
self.SetGazeFocus(pos,5.0)
elif self.gaze == Gaze.HEAD_LEADS_GAZE:
self.SetHeadFocus(pos,3.0)
def SelectNextFace(self):
# switch to the next (or first) face
if len(self.faces) == 0:
# there are no faces, so select none
self.current_face_id = 0
return
if self.current_face_id == 0:
self.current_face_id = self.faces.keys()[0]
else:
if self.current_face_id in self.faces:
next = self.faces.keys().index(self.current_face_id) + 1
if next >= len(self.faces.keys()):
next = 0
else:
next = 0
self.current_face_id = self.faces.keys()[next]
def SelectNextSaliency(self):
# switch to the next (or first) saliency vector
if len(self.saliencies) == 0:
# there are no saliency vectors, so select none
self.current_saliency_ts = 0
return
if self.current_saliency_ts == 0:
self.current_saliency_ts = self.saliencies.keys()[0]
else:
if self.current_saliency_ts in self.saliencies:
next = self.saliencies.keys().index(self.current_saliency_ts) + 1
if next >= len(self.saliencies):
next = 0
else:
next = 0
self.current_saliency_ts = self.saliencies.keys()[next]
def SelectNextAudience(self):
# TODO: switch to next audience (according to audience ROI)
()
def HandleTimer(self,data):
# this is the heart of the synthesizer, here the lookat and eyecontact state machines take care of where the robot is looking, and random expressions and gestures are triggered to look more alive (like RealSense Tracker)
ts = data.current_expected
# ==== handle lookat
if self.lookat == LookAt.IDLE:
# no specific target, let Blender do it's soma cycle thing
()
elif self.lookat == LookAt.AVOID:
# TODO: find out where there is no saliency, hand or face
# TODO: head_focus_pub
()
elif self.lookat == LookAt.SALIENCY:
self.saliency_counter -= 1
if self.saliency_counter == 0:
self.InitSaliencyCounter()
self.SelectNextSaliency()
if self.current_saliency_ts != 0:
cursaliency = self.saliencies[self.current_saliency_ts]
self.UpdateGaze(cursaliency.direction)
elif self.lookat == LookAt.HAND:
# stare at hand
if self.hand != None:
self.UpdateGaze(self.hand.position)
elif self.lookat == LookAt.AUDIENCE:
self.audience_counter -= 1
if self.audience_counter == 0:
self.InitAudienceCounter()
self.SelectNextAudience()
# TODO: self.UpdateGaze()
elif self.lookat == LookAt.SPEAKER:
()
# TODO: look at the speaker, according to speaker ROI
else:
if self.lookat == LookAt.ALL_FACES:
self.faces_counter -= 1
if self.faces_counter == 0:
self.InitFacesCounter()
self.SelectNextFace()
# take the current face
if self.current_face_id != 0:
curface = self.faces[self.current_face_id]
face_pos = curface.position
# ==== handle eyecontact (only for LookAt.ONE_FACE and LookAt.ALL_FACES)
# calculate where left eye, right eye and mouth are on the current face
left_eye_pos = Float32XYZ()
right_eye_pos = Float32XYZ()
mouth_pos = Float32XYZ()
# all are 5cm in front of the center of the face
left_eye_pos.x = face_pos.x - 0.05
right_eye_pos.x = face_pos.x - 0.05
mouth_pos.x = face_pos.x - 0.05
left_eye_pos.y = face_pos.y + 0.03 # left eye is 3cm to the left of the center
right_eye_pos.y = face_pos.y - 0.03 # right eye is 3cm to the right of the center
mouth_pos.y = face_pos.y # mouth is dead center
left_eye_pos.z = face_pos.z + 0.06 # left eye is 6cm above the center
right_eye_pos.z = face_pos.z + 0.06 # right eye is 6cm above the center
mouth_pos.z = face_pos.z - 0.04 # mouth is 4cm below the center
if self.eyecontact == EyeContact.IDLE:
# look at center of the head
self.UpdateGaze(face_pos)
elif self.eyecontact == EyeContact.LEFT_EYE:
# look at left eye
self.UpdateGaze(left_eye_pos)
elif self.eyecontact == EyeContact.RIGHT_EYE:
# look at right eye
self.UpdateGaze(right_eye_pos)
elif self.eyecontact == EyeContact.BOTH_EYES:
# switch between eyes back and forth
self.eyes_counter -= 1
if self.eyes_counter == 0:
self.InitEyesCounter()
if self.current_eye == 1:
self.current_eye = 0
else:
self.current_eye = 1
# look at that eye
if self.current_eye == 0:
cur_eye_pos = left_eye_pos
else:
cur_eye_pos = right_eye_pos
self.UpdateGaze(cur_eye_pos)
elif self.eyecontact == EyeContact.TRIANGLE:
# cycle between eyes and mouth
self.eyes_counter -= 1
if self.eyes_counter == 0:
self.InitEyesCounter()
if self.current_eye == 2:
self.current_eye = 0
else:
self.current_eye += 1
# look at that eye
if self.current_eye == 0:
cur_eye_pos = left_eye_pos
elif self.current_eye == 1:
cur_eye_pos = right_eye_pos
elif self.current_eye == 2:
cur_eye_pos = mouth_pos
self.UpdateGaze(cur_eye_pos)
# mirroring
msg = pau()
msg.m_coeffs = [ ]
msg.m_shapekeys = [ ]
if self.mirroring == Mirroring.EYEBROWS or self.mirroring == Mirroring.EYES or self.mirroring == Mirroring.MOUTH_EYEBROWS or self.mirroring == Mirroring.ALL:
# mirror eyebrows
left_brow = curface.left_brow
right_brow = curface.right_brow
msg.m_coeffs.append("brow_outer_UP.L")
msg.m_shapekeys.append(left_brow)
msg.m_coeffs.append("brow_inner_UP.L")
msg.m_shapekeys.append(left_brow * 0.8)
msg.m_coeffs.append("brow_outer_DN.L")
msg.m_shapekeys.append(1.0 - left_brow)
msg.m_coeffs.append("brow_outer_up.R")
msg.m_shapekeys.append(right_brow)
msg.m_coeffs.append("brow_inner_UP.R")
msg.m_shapekeys.append(right_brow * 0.8)
msg.m_coeffs.append("brow_outer_DN.R")
msg.m_shapekeys.append(1.0 - right_brow)
if self.mirroring == Mirroring.EYELIDS or self.mirroring == Mirroring.EYES or self.mirroring == Mirroring.MOUTH_EYELIDS or self.mirroring == Mirroring.ALL:
# mirror eyelids
eyes_closed = ((1.0 - curface.left_eyelid) + (1.0 - curface.right_eyelid)) / 2.0
msg.m_coeffs.append("eye-blink.UP.R")
msg.m_shapekeys.append(eyes_closed)
msg.m_coeffs.append("eye-blink.UP.L")
msg.m_shapekeys.append(eyes_closed)
msg.m_coeffs.append("eye-blink.LO.R")
msg.m_shapekeys.append(eyes_closed)
msg.m_coeffs.append("eye-blink.LO.L")
msg.m_shapekeys.append(eyes_closed)
if self.mirroring == Mirroring.MOUTH or self.mirroring == Mirroring.MOUTH_EYEBROWS or self.mirroring == Mirroring.MOUTH_EYELIDS:
# mirror mouth
mouth_open = curface.mouth_open
msg.m_coeffs.append("lip-JAW.DN")
msg.m_shapekeys.append(mouth_open)
if self.mirroring != Mirroring.IDLE:
self.StartPauMode()
self.setpau_pub.publish(msg)
# start random gestures
self.gesture_counter -= 1
if self.gesture_counter == 0:
self.InitGestureCounter()
if self.animations != None:
# list all gestures that would fire right now according to probability
firing = []
for g in self.animations[self.current_gestures_name]:
if random.uniform(0.0,1.0) <= g["probability"]:
firing.append(g)
# start randomly from that list
if len(firing) > 0:
g = firing[random.randint(0,len(firing) - 1)]
msg = SetGesture()
msg.name = g["name"]
msg.repeat = False
msg.speed = random.uniform(g["speed_min"],g["speed_max"])
msg.magnitude = random.uniform(g["magnitude_min"],g["magnitude_max"])
self.gestures_pub.publish(msg)
# start random expressions
self.expression_counter -= 1
if self.expression_counter == 0:
self.InitExpressionCounter()
if self.animations != None:
# list all expressions that would fire right now according to probability
firing = []
for g in self.animations[self.current_expressions_name]:
if random.uniform(0.0,1.0) <= g["probability"]:
firing.append(g)
# start randomly from that list
if len(firing) > 0:
g = firing[random.randint(0,len(firing) - 1)]
msg = EmotionState()
msg.name = g["name"]
msg.magnitude = random.uniform(g["magnitude_min"],g["magnitude_max"])
msg.duration = rospy.Duration(random.uniform(g["duration_min"],g["duration_max"]))
self.expressions_pub.publish(msg)
prune_before_time = ts - rospy.Duration.from_sec(self.keep_time)
# flush faces dictionary, update current face accordingly
to_be_removed = []
for face in self.faces.values():
if face.ts < prune_before_time:
to_be_removed.append(face.cface_id)
# remove the elements
for key in to_be_removed:
del self.faces[key]
# make sure the selected face is always valid
if self.current_face_id == key:
self.SelectNextFace()
# remove hand if it is too old
if self.hand != None:
if self.hand.ts < prune_before_time:
self.hand = None
# flush saliency dictionary
to_be_removed = []
for key in self.saliencies.keys():
if key < prune_before_time:
to_be_removed.append(key)
# remove the elements
for key in to_be_removed:
del self.saliencies[key]
# make sure the selected saliency is always valid
if self.current_saliency_ts == key:
self.SelectNextSaliency()
# decay from FOCUSED to IDLE if hand was not seen for a while
if self.state == State.FOCUSED and self.last_hand_ts < ts - rospy.Duration.from_sec(self.hand_state_decay):
self.SetState(State.IDLE)
self.UpdateStateDisplay()
# decay from SPEAKING or LISTENING to IDLE
if ((self.state == State.SPEAKING) or (self.state == State.LISTENING)) and self.last_talk_ts < ts - rospy.Duration.from_sec(self.face_state_decay):
self.SetState(State.IDLE)
self.UpdateStateDisplay()
# have gaze or head follow head or gaze after a while
if self.gaze_delay_counter > 0 and self.gaze_pos != None:
self.gaze_delay_counter -= 1
if self.gaze_delay_counter == 0:
if self.gaze == Gaze.GAZE_LEADS_HEAD:
self.SetHeadFocus(self.gaze_pos,self.gaze_speed)
self.gaze_delay_counter = int(self.gaze_delay * self.synthesizer_rate)
elif self.gaze == Gaze.HEAD_LEADS_GAZE:
self.SetGazeFocus(self.gaze_pos,self.gaze_speed)
self.gaze_delay_counter = int(self.gaze_delay * self.synthesizer_rate)
# when speaking, sometimes look at all faces
if self.state == State.SPEAKING:
if self.lookat == LookAt.AVOID:
self.all_faces_start_counter -= 1
if self.all_faces_start_counter == 0:
self.InitAllFacesStartCounter()
self.SetLookAt(LookAt.ALL_FACES)
self.UpdateStateDisplay()
elif self.lookat == LookAt.ALL_FACES:
self.all_faces_duration_counter -= 1
if self.all_faces_duration_counter == 0:
self.InitAllFacesDurationCounter()
self.SetLookAt(LookAt.AVOID)
self.UpdateStateDisplay()
def SetEyeContact(self, neweyecontact):
if neweyecontact == self.eyecontact:
return
self.eyecontact = neweyecontact
if self.eyecontact == EyeContact.BOTH_EYES or self.eyecontact == EyeContact.TRIANGLE:
self.InitEyesCounter()
def SetLookAt(self, newlookat):
if newlookat == self.lookat:
return
self.lookat = newlookat
if self.lookat == LookAt.SALIENCY:
self.InitSaliencyCounter()
elif self.lookat == LookAt.ONE_FACE:
self.InitEyesCounter()
elif self.lookat == LookAt.ALL_FACES:
self.InitFacesCounter()
self.InitEyesCounter()
elif self.lookat == LookAt.AUDIENCE:
self.InitAudienceCounter()
def StartPauMode(self):
mode = UInt8()
mode.data = 148
self.animationmode_pub.publish(mode)
def StopPauMode(self):
mode = UInt8()
mode.data = 0
self.animationmode_pub.publish(mode)
def SetMirroring(self, newmirroring):
if newmirroring == self.mirroring:
return
self.mirroring = newmirroring
if self.mirroring == Mirroring.IDLE:
self.StopPauMode()
else:
self.StartPauMode()
def SetGaze(self, newgaze):
if newgaze == self.gaze:
return
self.gaze = newgaze
if self.gaze == Gaze.GAZE_LEADS_HEAD or self.gaze == Gaze.HEAD_LEADS_GAZE:
self.gaze_delay_counter = int(self.gaze_delay * self.synthesizer_rate)
# ==== MAIN STATE MACHINE
def SetState(self, newstate):
# this is where the new main state is initialized, it sets up lookat and eyecontact states appropriately, manage perception system refresh rates and load random gesture and expression probabilities to be processed by HandleTimer
if newstate == self.state:
return
self.state = newstate
# initialize new state
if self.state == State.SLEEPING:
# the robot sleeps
print("State.SLEEPING")
self.current_gestures_name = "sleeping_gestures"
self.current_expressions_name = "sleeping_expressions"
#self.lefteye_config.update_configuration({"pipeline_rate":1.0,"detect_rate":1.0})
#self.righteye_config.update_configuration({"pipeline_rate":1.0,"detect_rate":1.0})
#self.wideangle_config.update_configuration({"pipeline_rate":1.0,"detect_rate":1.0})
#self.realsense_config.update_configuration({"pipeline_rate":1.0,"detect_rate":1.0})
#self.SetEyeContact(EyeContact.IDLE)
#self.SetLookAt(LookAt.IDLE)
#self.SetMirroring(Mirroring.IDLE)
#self.SetGaze(Gaze.GAZE_ONLY)
# IDEA: at SLEEPING, wakeup by ROS message and transition to IDLE
# IDEA: at SLEEPING, wakeup by loud noise
elif self.state == State.IDLE:
# the robot is idle
print("State.IDLE")
self.current_gestures_name = "idle_gestures"
self.current_expressions_name = "idle_expressions"
self.lefteye_config.update_configuration({"pipeline_rate":1.0,"detect_rate":1.0})
self.righteye_config.update_configuration({"pipeline_rate":1.0,"detect_rate":1.0})
self.wideangle_config.update_configuration({"pipeline_rate":10.0,"detect_rate":10.0})
self.realsense_config.update_configuration({"pipeline_rate":10.0,"detect_rate":20.0})
self.SetEyeContact(EyeContact.IDLE)
self.SetLookAt(LookAt.IDLE)
self.SetMirroring(Mirroring.IDLE)
self.SetGaze(Gaze.GAZE_ONLY)
elif self.state == State.INTERESTED:
# the robot is actively idle
print("State.INTERESTED")
self.current_gestures_name = "interested_gestures"
self.current_expressions_name = "interested_expressions"
self.lefteye_config.update_configuration({"pipeline_rate":1.0,"detect_rate":1.0})
self.righteye_config.update_configuration({"pipeline_rate":1.0,"detect_rate":1.0})
self.wideangle_config.update_configuration({"pipeline_rate":20.0,"detect_rate":10.0})
self.realsense_config.update_configuration({"pipeline_rate":20.0,"detect_rate":20.0})
self.SetEyeContact(EyeContact.IDLE)
self.SetLookAt(LookAt.SALIENCY)
self.SetMirroring(Mirroring.IDLE)
self.SetGaze(Gaze.GAZE_ONLY)
elif self.state == State.FOCUSED:
# the robot is very interested at something specific
print("State.FOCUSED")
self.current_gestures_name = "focused_gestures"
self.current_expressions_name = "focused_expressions"
self.lefteye_config.update_configuration({"pipeline_rate":1.0,"detect_rate":1.0})
self.righteye_config.update_configuration({"pipeline_rate":1.0,"detect_rate":1.0})
self.wideangle_config.update_configuration({"pipeline_rate":20.0,"detect_rate":20.0})
self.realsense_config.update_configuration({"pipeline_rate":20.0,"detect_rate":20.0})
self.SetEyeContact(EyeContact.IDLE)
self.SetLookAt(LookAt.HAND)
self.SetMirroring(Mirroring.IDLE)
self.SetGaze(Gaze.GAZE_AND_HEAD)
elif self.state == State.SPEAKING:
# the robot is speaking (directly/intimately) to people
print("State.SPEAKING")
self.current_gestures_name = "speaking_gestures"
self.current_expressions_name = "speaking_expressions"
self.lefteye_config.update_configuration({"pipeline_rate":1.0,"detect_rate":1.0})
self.righteye_config.update_configuration({"pipeline_rate":1.0,"detect_rate":1.0})
self.wideangle_config.update_configuration({"pipeline_rate":20.0,"detect_rate":10.0})
self.realsense_config.update_configuration({"pipeline_rate":20.0,"detect_rate":20.0})
self.SetEyeContact(EyeContact.IDLE)
self.SetLookAt(LookAt.AVOID)
self.SetMirroring(Mirroring.IDLE)
self.SetGaze(Gaze.GAZE_LEADS_HEAD)
self.last_talk_ts = rospy.get_rostime()
elif self.state == State.LISTENING:
# the robot is listening to whoever is speaking
print("State.LISTENING")
self.current_gestures_name = "listening_gestures"
self.current_expressions_name = "listening_expressions"
self.lefteye_config.update_configuration({"pipeline_rate":1.0,"detect_rate":1.0})
self.righteye_config.update_configuration({"pipeline_rate":1.0,"detect_rate":1.0})
self.wideangle_config.update_configuration({"pipeline_rate":20.0,"detect_rate":20.0})
self.realsense_config.update_configuration({"pipeline_rate":20.0,"detect_rate":20.0})
self.SetEyeContact(EyeContact.BOTH_EYES)
self.SetLookAt(LookAt.ONE_FACE)
self.SetMirroring(Mirroring.IDLE)
self.SetGaze(Gaze.HEAD_LEADS_GAZE)
self.last_talk_ts = rospy.get_rostime()
elif self.state == State.PRESENTING:
# the robot is presenting to the audience
print("State.PRESENTING")
self.current_gestures_name = "presenting_gestures"
self.current_expressions_name = "presenting_expressions"
self.lefteye_config.update_configuration({"pipeline_rate":1.0,"detect_rate":1.0})
self.righteye_config.update_configuration({"pipeline_rate":1.0,"detect_rate":1.0})
self.wideangle_config.update_configuration({"pipeline_rate":20.0,"detect_rate":10.0})
self.realsense_config.update_configuration({"pipeline_rate":20.0,"detect_rate":20.0})
self.SetEyeContact(EyeContact.IDLE)
self.SetLookAt(LookAt.AUDIENCE)
self.SetMirroring(Mirroring.IDLE)
self.SetGaze(Gaze.GAZE_AND_HEAD)
def HandleFace(self, msg):
self.faces[msg.cface_id] = msg
self.last_face = msg.cface_id
self.last_talk_ts = msg.ts
# TEMP: if there is no current face, make this the current face
if self.current_face_id == 0:
self.current_face_id = msg.cface_id
def HandleHand(self, msg):
self.hand = msg
self.last_hand_ts = msg.ts
# transition from IDLE or INTERESTED to FOCUSED
if self.state == State.IDLE or self.state == State.INTERESTED:
self.SetState(State.FOCUSED)
self.UpdateStateDisplay()
def HandleSaliency(self, msg):
self.saliencies[msg.ts] = msg
# TEMP: if there is no current saliency vector, make this the current saliency vector
if self.current_saliency_ts == 0:
self.saliency_counter = 1
self.current_saliency_ts = msg.ts
# transition from IDLE to INTERESTED
if self.state == State.IDLE:
self.SetState(State.INTERESTED)
self.UpdateStateDisplay()
def HandleChatEvents(self, msg):
# triggered when someone starts talking to the robot
self.last_talk_ts = rospy.get_rostime()
# transition from IDLE, INTERESTED or FOCUSED to LISTENING
if self.state == State.IDLE or self.state == State.INTERESTED or self.state == State.FOCUSED:
self.SetState(State.LISTENING)
self.UpdateStateDisplay()
def HandleSpeechEvents(self, msg):
# triggered when the robot starts or stops talking
self.last_talk_ts = rospy.get_rostime()
if msg.data == "start":
# transition from IDLE, INTERESTED, FOCUSED or LISTENING to SPEAKING
if self.state == State.IDLE or self.state == State.INTERESTED or self.state == State.FOCUSED or self.state == State.LISTENING:
self.SetState(State.SPEAKING)
self.UpdateStateDisplay()
elif msg.data == "stop":
# transition from SPEAKING to IDLE
if self.state == State.SPEAKING:
self.SetState(State.IDLE)
self.UpdateStateDisplay()
def HandleAudioDirection(self, msg):
# use to correlate with person speaking to select correct current face
()
def HandleMotion(self, msg):
# use to trigger awareness of people even without seeing them
()
class Attention:
def InitCounter(self, counter, minmax):
try:
min_v = min(int(getattr(self, "{}_min".format(minmax))),
int(getattr(self, "{}_max".format(minmax))))
max_v = max(int(getattr(self, "{}_min".format(minmax))),
int(getattr(self, "{}_max".format(minmax))))
val = random.randint(int(min_v * self.synthesizer_rate),
int(max_v * self.synthesizer_rate))
except:
val = 1
setattr(self, "{}_counter".format(counter), val)
def __getattr__(self, item):
# Allow configuration attributes to be accessed directly
if self.config is None:
raise AttributeError
return self.config.__getattr__(item)
def __init__(self):
# create lock
self.lock = threading.Lock()
self.robot_name = rospy.get_param("/robot_name")
self.eyecontact = 0
self.lookat = 0
self.mirroring = 0
# By default look with head and eyes
self.gaze = 2
# setup face, hand and saliency structures
self.state = State()
self.current_face_index = -1 # index to current face
self.wanted_face_id = 0 # ID for wanted face
self.current_saliency_index = -1 # index of current saliency vector
self.current_eye = 0 # current eye (0 = left, 1 = right, 2 = mouth)
self.interrupted_state = LookAt.IDLE # which state was interrupted to look at all faces
self.interrupting = False # LookAt state is currently interrupted to look at all faces
self.gaze_delay_counter = 0 # delay counter after with gaze or head follows head or gaze
self.gaze_pos = None # current gaze position
# counter to run if face not visible
self.no_face_counter = 0
self.no_switch_counter = 0
self.last_pose_counter = 0
self.last_pose = None
self.eyecontact_state = EyeContact.TRIANGLE
self.tf_listener = tf.TransformListener(False,
rospy.Duration.from_sec(1))
# tracks last face by fsdk_id, if changes informs eye tracking to pause
self.last_target = -2
rospy.Subscriber('/{}/perception/state'.format(self.robot_name), State,
self.HandleState)
self.head_focus_pub = rospy.Publisher('/blender_api/set_face_target',
Target,
queue_size=1)
self.gaze_focus_pub = rospy.Publisher('/blender_api/set_gaze_target',
Target,
queue_size=1)
self.expressions_pub = rospy.Publisher(
'/blender_api/set_emotion_state', EmotionState, queue_size=1)
self.gestures_pub = rospy.Publisher('/blender_api/set_gesture',
SetGesture,
queue_size=1)
self.animationmode_pub = rospy.Publisher(
'/blender_api/set_animation_mode', UInt8, queue_size=1)
self.setpau_pub = rospy.Publisher('/blender_api/set_pau',
pau,
queue_size=1)
# Topic to publish target changes
self.current_target_pub = rospy.Publisher('/behavior/current_target',
Int64,
queue_size=5,
latch=True)
self.synthesizer_rate = 30
self.hand_events_pub = rospy.Publisher('/hand_events',
String,
queue_size=1)
# Timer, started by config server
self.timer = None
# start dynamic reconfigure server
self.configs_init = False
self.config_server = Server(AttentionConfig,
self.HandleConfig,
namespace='/current/attention')
# API calls (when the overall state is not automatically setting these values via dynamic reconfigure)
self.eyecontact_sub = rospy.Subscriber(
'/behavior/attention/api/eyecontact', UInt8, self.HandleEyeContact)
self.lookat_sub = rospy.Subscriber('/behavior/attention/api/lookat',
APILookAt, self.HandleLookAt)
self.mirroring_sub = rospy.Subscriber(
'/behavior/attention/api/mirroring', UInt8, self.HandleMirroring)
self.gaze_sub = rospy.Subscriber('/behavior/attention/api/gaze', UInt8,
self.HandleGaze)
def ChangeTarget(self, id=None):
if id is None:
id = int(time.time() * 10)
if id <> self.last_target:
self.last_target = id
self.current_target_pub.publish(id)
def UpdateStateDisplay(self):
self.config_server.update_configuration({
"eyecontact_state": self.eyecontact,
"lookat_state": self.lookat,
"mirroring_state": self.mirroring,
"gaze_state": self.gaze,
})
def HandleConfig(self, config, level):
with self.lock:
self.config = config
# and set the states for each state machine
#self.SetEyeContact(config.eyecontact_state)
self.SetLookAt(config.lookat_state)
# self.SetMirroring(config.mirroring_state)
# self.SetGaze(config.gaze_state)
# Counters
if not self.configs_init:
self.timer = rospy.Timer(
rospy.Duration.from_sec(1.0 / self.synthesizer_rate),
self.HandleTimer)
# self.InitCounter("saliency","saliency_time")
self.InitCounter("faces", "faces_time")
self.InitCounter("region", "region_time")
# self.InitCounter("rest", "rest_time")
self.configs_init = True
return config
def getBlenderPos(self, pos, ts, frame_id):
if frame_id == 'blender':
return pos
else:
ps = PointStamped()
ps.header.seq = 0
ps.header.stamp = ts
ps.header.frame_id = frame_id
ps.point.x = pos.x
ps.point.y = pos.y
ps.point.z = pos.z
if self.tf_listener.canTransform("blender", frame_id, ts):
pst = self.tf_listener.transformPoint("blender", ps)
return pst.point
else:
raise Exception("tf from robot to blender did not work")
def SetGazeFocus(self, pos, speed, ts, frame_id='robot'):
try:
pos = self.getBlenderPos(pos, ts, frame_id)
msg = Target()
msg.x = max(0.3, pos.x)
msg.y = pos.y if not math.isnan(pos.y) else 0
msg.z = pos.z if not math.isnan(pos.z) else 0
msg.z = max(-0.3, min(0.3, msg.z))
msg.speed = speed
self.gaze_focus_pub.publish(msg)
except Exception as e:
logger.warn("Gaze focus exception: {}".format(e))
def SetHeadFocus(self, pos, speed, ts, frame_id='robot'):
try:
pos = self.getBlenderPos(pos, ts, frame_id)
msg = Target()
msg.x = max(0.3, pos.x)
msg.y = pos.y if not math.isnan(pos.y) else 0
msg.z = pos.z if not math.isnan(pos.z) else 0
msg.z = max(-0.3, min(0.3, msg.z))
msg.speed = speed
self.head_focus_pub.publish(msg)
except Exception as e:
logger.warn("Head focus exception: {}".format(e))
def UpdateGaze(self, pos, ts, frame_id="robot"):
self.gaze_pos = pos
if self.gaze == Gaze.GAZE_ONLY:
self.SetGazeFocus(pos, 5.0, ts, frame_id)
elif self.gaze == Gaze.HEAD_ONLY:
self.SetHeadFocus(pos, self.head_speed, ts, frame_id)
elif self.gaze == Gaze.GAZE_AND_HEAD:
self.SetGazeFocus(pos, 5.0, ts, frame_id)
self.SetHeadFocus(pos, self.head_speed, ts, frame_id)
elif self.gaze == Gaze.GAZE_LEADS_HEAD:
self.SetGazeFocus(pos, 5.0, ts, frame_id)
elif self.gaze == Gaze.HEAD_LEADS_GAZE:
self.SetHeadFocus(pos, self.head_speed, ts, frame_id)
def SelectNextFace(self):
# switch to the next (or first) face
if self.state is None or len(self.state.faces) == 0:
# there are no faces, so select none
self.current_face_index = -1
return
if self.lookat == LookAt.NEAREST_FACE:
self.current_face_index, f = min(
enumerate(self.state.faces),
key=lambda f: (f[1].position.x**2 + f[1].position.y**2))
else:
# Pick random or any other face from list
if self.current_face_index == -1:
self.current_face_index = 0
else:
self.current_face_index += 1
if self.current_face_index >= len(self.state.faces):
self.current_face_index = 0
def SelectNextPose(self):
# switch to the next (or first) face
if self.state is None or len(self.state.poses) == 0:
# there are no faces, so select none
self.last_pose_counter = 0
return False
# Select nearest pose to the robot, otherwise select nearest pose to previous
if self.last_pose is None or self.last_pose_counter <= 0:
i, p = min(enumerate(self.state.poses),
key=lambda f: (f[1].position.x**2 + f[1].position.y**2))
self.last_pose_counter = self.synthesizer_rate * self.min_time_between_targets
else:
i, p = min(enumerate(self.state.poses),
key=lambda f:
((self.last_pose.position.x - f[1].position.x)**2 +
(self.last_pose.position.y - f[1].position.y)**2 +
(self.last_pose.position.z - f[1].position.z)**2))
self.last_pose_counter -= 1
self.last_pose = p
return p
def SelectNextSalientPoint(self):
# switch to the next (or first) saliency vector
if (self.state == None):
self.current_saliency_index = -1
if len(self.state.salientpoints) == 0:
# there are no saliency vectors, so select none
self.current_saliency_index = -1
return
if self.current_saliency_index == -1:
self.current_saliency_index = 0
else:
self.current_saliency_index += 1
if self.current_saliency_index >= len(self.state.salientpoints):
self.current_saliency_index = 0
def SelectNextRegion(self):
# Check if setperformance has set regions
regions = rospy.get_param(
"/{}/performance_regions".format(self.robot_name), {})
if len(regions) == 0:
regions = rospy.get_param("/{}/regions".format(self.robot_name),
{})
point = AttentionRegion.get_point_from_regions(
regions, REGIONS[self.attention_region])
return Point(x=point['x'], y=point['y'], z=point['z'])
def StepLookAtFace(self, ts):
if self.current_face_index == -1:
raise Exception("No face available")
try:
curface = self.state.faces[self.current_face_index]
except:
raise Exception("No face available")
self.ChangeTarget(curface.fsdk_id)
face_pos = curface.position
# ==== handle eyecontact (only for LookAt.ONE_FACE and LookAt.ALL_FACES)
# calculate where left eye, right eye and mouth are on the current face
left_eye_pos = Point()
right_eye_pos = Point()
mouth_pos = Point()
# all are 5cm in front of the center of the face
left_eye_pos.x = face_pos.x - 0.05
right_eye_pos.x = face_pos.x - 0.05
mouth_pos.x = face_pos.x - 0.05
left_eye_pos.y = face_pos.y + 0.03 # left eye is 3cm to the left of the center
right_eye_pos.y = face_pos.y - 0.03 # right eye is 3cm to the right of the center
mouth_pos.y = face_pos.y # mouth is dead center
left_eye_pos.z = face_pos.z + 0.06 # left eye is 6cm above the center
right_eye_pos.z = face_pos.z + 0.06 # right eye is 6cm above the center
mouth_pos.z = face_pos.z - 0.04 # mouth is 4cm below the center
if self.eyecontact == EyeContact.IDLE:
# look at center of the head
self.UpdateGaze(face_pos, ts)
elif self.eyecontact == EyeContact.LEFT_EYE:
# look at left eye
self.UpdateGaze(left_eye_pos, ts)
elif self.eyecontact == EyeContact.RIGHT_EYE:
# look at right eye
self.UpdateGaze(right_eye_pos, ts)
elif self.eyecontact == EyeContact.BOTH_EYES:
# switch between eyes back and forth
self.eyes_counter -= 1
if self.eyes_counter == 0:
self.InitCounter("eyes", "eyes_time")
if self.current_eye == 1:
self.current_eye = 0
else:
self.current_eye = 1
# look at that eye
if self.current_eye == 0:
cur_eye_pos = left_eye_pos
else:
cur_eye_pos = right_eye_pos
self.UpdateGaze(cur_eye_pos, ts)
elif self.eyecontact == EyeContact.TRIANGLE:
# cycle between eyes and mouth
self.eyes_counter -= 1
if self.eyes_counter == 0:
self.InitCounter("eyes", "eyes_time")
if self.current_eye == 2:
self.current_eye = 0
else:
self.current_eye += 1
# look at that eye
if self.current_eye == 0:
cur_eye_pos = left_eye_pos
elif self.current_eye == 1:
cur_eye_pos = right_eye_pos
elif self.current_eye == 2:
cur_eye_pos = mouth_pos
self.UpdateGaze(cur_eye_pos, ts)
# mirroring
# msg = pau()
# msg.m_coeffs = []
# msg.m_shapekeys = []
# if self.mirroring == Mirroring.EYEBROWS or self.mirroring == Mirroring.EYES or self.mirroring == Mirroring.MOUTH_EYEBROWS or self.mirroring == Mirroring.ALL:
# # mirror eyebrows
# left_brow = curface.left_brow
# right_brow = curface.right_brow
# msg.m_coeffs.append("brow_outer_UP.L")
# msg.m_shapekeys.append(left_brow)
# msg.m_coeffs.append("brow_inner_UP.L")
# msg.m_shapekeys.append(left_brow * 0.8)
# msg.m_coeffs.append("brow_outer_DN.L")
# msg.m_shapekeys.append(1.0 - left_brow)
# msg.m_coeffs.append("brow_outer_up.R")
# msg.m_shapekeys.append(right_brow)
# msg.m_coeffs.append("brow_inner_UP.R")
# msg.m_shapekeys.append(right_brow * 0.8)
# msg.m_coeffs.append("brow_outer_DN.R")
# msg.m_shapekeys.append(1.0 - right_brow)
# if self.mirroring == Mirroring.EYELIDS or self.mirroring == Mirroring.EYES or self.mirroring == Mirroring.MOUTH_EYELIDS or self.mirroring == Mirroring.ALL:
# # mirror eyelids
# eyes_closed = ((1.0 - curface.left_eyelid) + (1.0 - curface.right_eyelid)) / 2.0
# msg.m_coeffs.append("eye-blink.UP.R")
# msg.m_shapekeys.append(eyes_closed)
# msg.m_coeffs.append("eye-blink.UP.L")
# msg.m_shapekeys.append(eyes_closed)
# msg.m_coeffs.append("eye-blink.LO.R")
# msg.m_shapekeys.append(eyes_closed)
# msg.m_coeffs.append("eye-blink.LO.L")
# msg.m_shapekeys.append(eyes_closed)
# if self.mirroring == Mirroring.MOUTH or self.mirroring == Mirroring.MOUTH_EYEBROWS or self.mirroring == Mirroring.MOUTH_EYELIDS:
# # mirror mouth
# mouth_open = curface.mouth_open
# msg.m_coeffs.append("lip-JAW.DN")
# msg.m_shapekeys.append(mouth_open)
# if self.mirroring != Mirroring.IDLE:
# self.StartPauMode()
# self.setpau_pub.publish(msg)
def HandleTimer(self, data):
looking_at_face = False
with self.lock:
if not self.configs_init:
return False
if not self.enable_flag:
self.ChangeTarget(-1)
return False
# this is the heart of the synthesizer, here the lookat and eyecontact state machines take care of where the robot is looking, and random expressions and gestures are triggered to look more alive (like RealSense Tracker)
ts = data.current_expected
#If nowhere to look, straighten the head
idle_point = Point(x=1, y=0, z=0)
# Fallback to look at region
region = False
# Fallback o rest
idle = False
# ==== handle lookat
if self.lookat == LookAt.AVOID:
# TODO: find out where there is no saliency, hand or face
# TODO: head_focus_pub
()
if self.lookat == LookAt.HOLD or self.lookat == LookAt.IDLE:
# Do nothing
pass
# if self.lookat == LookAt.SALIENCY:
# self.saliency_counter -= 1
# if self.saliency_counter == 0:
# self.SelectNextSalientPoint()
# # Reset head position if nothing happens
# if self.current_saliency_index == -1:
# self.UpdateGaze(idle_point, ts, frame_id='blender')
# else:
# # Init counter only if any salient point found
# self.InitCounter("saliency","saliency_time")
#
# if self.current_saliency_index != -1:
# cursaliency = self.state.salientpoints[self.current_saliency_index]
# self.UpdateGaze(cursaliency.position, ts)
elif self.lookat == LookAt.REGION:
self.region_counter -= 1
if self.region_counter == 0:
self.InitCounter("region", "region_time")
# SelectNextRegion returns idle point if no region set
point = self.SelectNextRegion()
# Attention points are calculated in blender frame
self.UpdateGaze(point, ts, frame_id='blender')
# elif self.lookat == LookAt.POSES:
# pose = self.SelectNextPose()
# if pose:
# self.UpdateGaze(pose.position, ts)
# self.no_switch_counter = self.synthesizer_rate * self.min_time_between_targets
# else:
# self.no_switch_counter -= 1
# if self.no_switch_counter < 0:
# point = self.SelectNextRegion()
# self.UpdateGaze(point, ts, frame_id='blender')
# self.no_switch_counter = self.synthesizer_rate * self.min_time_between_targets
else:
if self.lookat == LookAt.ALL_FACES or self.lookat == LookAt.NEAREST_FACE or self.lookat == LookAt.POSES:
self.faces_counter -= 1
if self.faces_counter == 0:
self.SelectNextFace()
self.no_switch_counter = self.synthesizer_rate * self.min_time_between_targets
self.InitCounter("faces", "faces_time")
try:
# This will make sure robot will look somewhere so eye contact only should be paused
# It should fallback to attention region if defined or idle point
looking_at_face = True
# only look at faces after switch is allowed:
self.StepLookAtFace(ts)
# Reset after face is found
self.no_face_counter = 0
except:
# Look at poses if no faces are visible, and then look at region otherwise
pose = self.SelectNextPose()
self.no_face_counter -= 1
if pose:
self.UpdateGaze(pose.position, ts)
self.no_switch_counter = self.synthesizer_rate * self.min_time_between_targets
else:
self.no_switch_counter -= 1
if self.no_switch_counter <= 0:
if self.no_face_counter <= 0:
point = self.SelectNextRegion()
self.InitCounter('no_face', 'region_time')
self.UpdateGaze(point,
ts,
frame_id='blender')
self.no_switch_counter = self.synthesizer_rate * self.min_time_between_targets
if self.lookat == LookAt.REGION or region:
self.region_counter -= 1
if self.region_counter == 0:
# Eye contact enabled when looking at region
if self.eyecontact > 0:
looking_at_face = True
self.InitCounter("region", "region_time")
# SelectNextRegion returns idle point if no region set
try:
point = self.SelectNextRegion()
# Attention points are calculated in blender frqame
self.UpdateGaze(point, ts, frame_id='blender')
# Target have been changed
except:
idle = True
# if self.lookat == LookAt.IDLE or idle:
# self.rest_counter -= 1
# if self.rest_counter == 0:
# self.InitCounter("rest", "rest_time")
# if self.eyecontact > 0:
# looking_at_face = True
# idle_point = Point(x=1, y=random.uniform(-self.rest_range_x, self.rest_range_y)
# , z=random.uniform(-self.rest_range_y, self.rest_range_y))
# # Attention points are calculated in blender frame
# self.UpdateGaze(idle_point, ts, frame_id='blender')
# # Target have been changed
# self.ChangeTarget()
# have gaze or head follow head or gaze after a while
if self.gaze_delay_counter > 0 and self.gaze_pos != None:
self.gaze_delay_counter -= 1
if self.gaze_delay_counter == 0:
if self.gaze == Gaze.GAZE_LEADS_HEAD:
self.SetHeadFocus(self.gaze_pos, self.gaze_speed, ts)
self.gaze_delay_counter = int(self.gaze_delay *
self.synthesizer_rate)
elif self.gaze == Gaze.HEAD_LEADS_GAZE:
self.SetGazeFocus(self.gaze_pos, self.gaze_speed, ts)
self.gaze_delay_counter = int(self.gaze_delay *
self.synthesizer_rate)
# when speaking, sometimes look at all faces
# if self.interrupt_to_all_faces:
#
# if self.interrupting:
# self.all_faces_duration_counter -= 1
# if self.all_faces_duration_counter <= 0:
# self.interrupting = False
# self.InitCounter("all_faces_duration", "all_faces_duration")
# self.SetLookAt(self.interrupted_state)
# self.UpdateStateDisplay()
# else:
# self.all_faces_start_counter -= 1
# if self.all_faces_start_counter <= 0:
# self.interrupting = True
# self.InitCounter("all_faces_start", "all_faces_start_time")
# self.interrupted_state = self.lookat
# self.SetLookAt(LookAt.ALL_FACES)
# self.UpdateStateDisplay()
if not looking_at_face:
self.ChangeTarget(-1)
def SetEyeContact(self, neweyecontact):
if neweyecontact == self.eyecontact:
return
self.eyecontact = neweyecontact
# if self.eyecontact == EyeContact.BOTH_EYES or self.eyecontact == EyeContact.TRIANGLE:
# self.InitCounter("eyes", "eyes_time")
def SetLookAt(self, newlookat):
if newlookat == self.lookat:
return
self.lookat = newlookat
if self.lookat == LookAt.SALIENCY:
self.InitCounter("saliency", "saliency_time")
# elif self.lookat == LookAt.ONE_FACE:
# self.InitCounter("eyes", "eyes_time")
elif self.lookat == LookAt.ALL_FACES:
self.InitCounter("faces", "faces_time")
# self.InitCounter("eyes", "eyes_time")
elif self.lookat == LookAt.REGION:
self.InitCounter("region", "region_time")
# elif self.lookat == LookAt.IDLE:
# self.InitCounter("rest", "rest_time")
def StartPauMode(self):
mode = UInt8()
mode.data = 148
self.animationmode_pub.publish(mode)
def StopPauMode(self):
mode = UInt8()
mode.data = 0
self.animationmode_pub.publish(mode)
def SetMirroring(self, newmirroring):
pass
# logger.warn("SetMirroring {}".format(newmirroring))
# if newmirroring == self.mirroring: return
#
# self.mirroring = newmirroring
#
# if self.mirroring == Mirroring.IDLE:
# self.StopPauMode()
# else:
# self.StartPauMode()
def SetGaze(self, newgaze):
if newgaze == self.gaze:
return
self.gaze = newgaze
if self.gaze == Gaze.GAZE_LEADS_HEAD or self.gaze == Gaze.HEAD_LEADS_GAZE:
self.gaze_delay_counter = int(self.gaze_delay *
self.synthesizer_rate)
def HandleState(self, data):
with self.lock:
self.state = data
# if there is a wanted face, try to find it and use that
if self.wanted_face_id != 0:
index = 0
self.current_face_index = -1
for face in self.state.faces:
if face.id == self.wanted_face_id:
self.current_face_index = index
break
index += 1
# otherwise, just make sure current_face_index is valid
elif (self.current_face_index >= len(
self.state.faces)) or (self.current_face_index == -1):
# Only try to look at new faces once there are no
if self.no_switch_counter <= 0:
self.SelectNextFace()
# TODO: it's better to have the robot look at the same ID, regardless of which index in the stateface list
# # if there is no current saliency or the current saliency is out of range, select a new current saliency
# if (self.current_saliency_index >= len(self.state.salientpoints)) or (self.current_saliency_index == -1):
# self.SelectNextSaliency()
def HandleEyeContact(self, data):
with self.lock:
self.SetEyeContact(data)
self.UpdateStateDisplay()
def HandleLookAt(self, data):
with self.lock:
if data.mode == LookAt.ONE_FACE: # if client wants to the robot to look at one specific face (even if the face temporarily disappears from the state)
self.wanted_face_id = data.id
elif data.mode == LookAt.REGION:
self.attention_region = data.id
else:
self.wanted_face_id = 0
self.SetLookAt(self, data.mode)
self.UpdateStateDisplay()
def HandleMirroring(self, data):
with self.lock:
self.SetMirroring(data)
self.UpdateStateDisplay()
def HandleGaze(self, data):
with self.lock:
self.SetGaze(data)
self.UpdateStateDisplay()
class Robot(HierarchicalMachine):
state_cls = InteractiveState
def __init__(self):
# Wait for service to set initial params
rospy.wait_for_service('/current/attention/set_parameters')
rospy.wait_for_service('/current/animations/set_parameters')
# Current state config
self.state_config = None
self.config = None
self.starting = True
# Current TTS mode: chatbot_responses - auto, web_sresponses - operator
self.current_tts_mode = "chatbot_responses"
# Controls states_timeouts
self._state_timer = None
# ROS Topics and services
self.robot_name = rospy.get_param('/robot_name')
# ROS publishers
self.topics = {
'running_performance':
rospy.Publisher('/running_performance', String, queue_size=1),
'chatbot_speech':
rospy.Publisher('/{}/chatbot_speech'.format(self.robot_name),
ChatMessage,
queue_size=10),
'soma_pub':
rospy.Publisher('/blender_api/set_soma_state',
SomaState,
queue_size=10),
'state_pub':
rospy.Publisher('/current_state', String, latch=True),
}
# ROS Subscribers
self.subscribers = {
'speech':
rospy.Subscriber('/{}/speech'.format(self.robot_name), ChatMessage,
self.speech_cb),
'running_performance':
rospy.Subscriber('/performances/running_performance', String,
self.performance_cb),
'performance_events':
rospy.Subscriber('/performances/events', PerformanceEvent,
self.performance_events_cb),
'speech_events':
rospy.Subscriber('/{}/speech_events'.format(self.robot_name),
String, self.speech_events_cb),
'chat_events':
rospy.Subscriber('/{}/chat_events'.format(self.robot_name), String,
self.chat_events_cb),
'state_switch':
rospy.Subscriber('/state_switch', String, self.state_callback),
'tts_mux':
rospy.Subscriber('/{}/tts_mux/selected'.format(self.robot_name),
String, self.tts_mux_cb)
}
# ROS Services
# Wait for all services to become available
rospy.wait_for_service('/blender_api/set_param')
rospy.wait_for_service('/performances/current')
# All services required.
self.services = {
'performance_runner':
rospy.ServiceProxy('/performances/run_full_performance',
srv.RunByName),
'blender_param':
rospy.ServiceProxy('/blender_api/set_param', SetParam),
}
# Configure clients
self.clients = {
'attention':
dynamic_reconfigure.client.Client('/current/attention',
timeout=0.1),
'animation':
dynamic_reconfigure.client.Client('/current/animations',
timeout=0.1),
}
# robot properties
self.props = {
'disable_attention': None,
'disable_animations': None,
'disable_blinking': None,
'disable_saccades': None,
'disable_keepalive': None,
}
self._before_presentation = ''
self._current_performance = None
# State server mostly used for checking current states settings
self.state_server = Server(StateConfig,
self.state_config_callback,
namespace='/current/state_settings')
# Machine starts
HierarchicalMachine.__init__(
self,
states=STATES,
transitions=TRANSITIONS,
initial='idle',
ignore_invalid_triggers=True,
after_state_change=self.state_changed,
before_state_change=self.on_before_state_change)
# Main param server
self.server = Server(StatesConfig,
self.config_callback,
namespace='/behavior/behavior_settings')
self.faces_db_file = None
self.known_faces = {}
self.faces_last_update = time.time()
# Faces last seen database:
try:
assemblies = rospy.get_param('/assemblies')
assembly = assemblies[0] if self.robot_name in assemblies[
0] else assemblies[1]
self.faces_db_file = os.path.join(assembly, 'known_faces.yaml')
with open(self.faces_db_file, 'r') as stream:
try:
self.known_faces = yaml.load(stream)
except yaml.YAMLError as exc:
logger.warn(exc)
except Exception as e:
logger.error("Cant load the known faces {}".format(e))
rospy.Subscriber('/{}/perception/state'.format(self.robot_name), State,
self.perception_state_cb)
def perception_state_cb(self, msg):
# Ignore perception while no interacting
if not self.state == 'interacting_interested':
return
if len(msg.faces) > 0:
for f in msg.faces:
if f.first_name is not "":
name = f.first_name
# repeating face, enroll but don't play any timeline
if name not in self.known_faces.keys():
self.known_faces[name] = {'last_seen': time.time()}
self.sync_faces()
else:
last_seen = self.known_faces[name]['last_seen']
rospy.set_param('/last_known_face', name)
current_time = time.time()
self.known_faces[name]['last_seen'] = current_time
# Update every 10s
if self.faces_last_update + 10 < time.time():
self.sync_faces()
if current_time - last_seen > 60 * 60 * 24:
performance_kwd = 'faceid_{}_{}'.format(
'long', name)
performances = self.find_performance_by_speech(
performance_kwd)
if len(performances) > 0:
self.services['performance_runner'](
random.choice(performances))
return
if current_time - last_seen > 60 * 15:
performance_kwd = 'faceid_{}_{}'.format(
'short', name)
performances = self.find_performance_by_speech(
performance_kwd)
if len(performances) > 0:
self.services['performance_runner'](
random.choice(performances))
return
if current_time - last_seen > 60 * 60 * 24:
performance_kwd = 'faceid_{}_unknown'.format(
'long', name)
performances = self.find_performance_by_speech(
performance_kwd)
if len(performances) > 0:
self.services['performance_runner'](
random.choice(performances))
return
if current_time - last_seen > 60 * 15:
performance_kwd = 'faceid_{}_unknown'.format(
'short', name)
performances = self.find_performance_by_speech(
performance_kwd)
if len(performances) > 0:
self.services['performance_runner'](
random.choice(performances))
return
def sync_faces(self):
try:
self.faces_last_update = time.time()
with open(self.faces_db_file, 'w') as outfile:
yaml.dump(self.known_faces, outfile, default_flow_style=False)
except:
pass
# Calls after each state change to apply new configs
def state_changed(self):
rospy.set_param('/current_state', self.state)
self.topics['state_pub'].publish(String(self.state))
# State object
state = self.get_state(self.state)
logger.warn(self.state)
if state.attention_config:
self.clients['attention'].update_configuration(
state.attention_config)
if state.animations_config:
self.clients['animation'].update_configuration(
state.animations_config)
# Aply general behavior for states
if state.state_config:
self.state_server.update_configuration(state.state_config)
def state_callback(self, msg):
state = msg.data
# Only 3 main switches for now
try:
if state == 'idle':
self.go_idle()
elif state == 'presenting':
self.start_presentation()
elif state == 'interacting':
self.start_interacting()
except Exception as e:
logger.error(e)
def state_config_callback(self, config, level):
# Apply properties
self.disable_animations = config.disable_animations
self.disable_attention = config.disable_attention
self.disable_blinking = config.disable_blinking
self.disable_keepalive = config.disable_keepalive
self.disable_saccades = config.disable_saccades
self.state_config = config
return config
# ROS Callback methods
def config_callback(self, config, level):
self.config = config
# Set correct init state on loading. Will set the parameters as well
if self.starting:
self.starting = False
if self.config['init_state'] == 'interacting':
self.start_interacting()
if self.config['init_state'] == 'presentation':
self.start_presentation()
return config
# Handles all speech inputs
def speech_cb(self, msg):
try:
speech = str(msg.utterance).lower()
# Check if performance is not waiting for same keyword to continue in timeline
if self.is_presenting(allow_substates=True
) and self.config['chat_during_performance']:
keywords = rospy.get_param('/performances/keywords_listening',
False)
# Don't pass the keywords if pause node waits for same keyword (i.e resume performance).
if keywords and pause.event_matched(keywords, msg.utterance):
return
# Allow trigger performances by keywords
if self.state_config.performances_by_keyword:
performances = self.find_performance_by_speech(speech)
# Split between performances for general modes and analysis
analysis_performances = [
p for p in performances
if ('shared/analysis' in p or 'robot/analysis' in p)
]
for a in analysis_performances:
performances.remove(a)
if performances and self.state != 'analysis':
self.services['performance_runner'](
random.choice(performances))
return
elif analysis_performances and self.state == 'analysis':
self.services['performance_runner'](
random.choice(analysis_performances))
return
# If chat is not enabled for specific state ignore it
if not self.state_config.chat_enabled:
return
# Need to respond to speech
# Check if state allows chat
self.speech_finished()
self.topics['chatbot_speech'].publish(msg)
except Exception as e:
logger.error(e)
self.topics['chatbot_speech'].publish(msg)
def speech_events_cb(self, msg):
if msg.data == 'start':
# Robot starts talking
self.start_talking()
if msg.data == 'stop':
# Talking finished, robot starts listening
self.finish_talking()
def chat_events_cb(self, msg):
if msg.data == 'speechstart':
self.speech_start()
def tts_mux_cb(self, msg):
self.current_tts_mode = msg.data
def find_performance_by_speech(self, speech):
""" Finds performances which one of keyword matches"""
performances = []
for performance, keywords in self.get_keywords().items():
if self.performance_keyword_match(keywords, speech):
performances.append(performance)
return performances
@staticmethod
def performance_keyword_match(keywords, input):
for keyword in keywords:
if not keyword:
continue
# Currently only simple matching
if re.search(r"\b{}\b".format(keyword), input,
flags=re.IGNORECASE):
return True
return False
def get_keywords(self, performances=None, keywords=None, path='.'):
if performances is None:
performances = rospy.get_param(
os.path.join('/', self.robot_name, 'webui/performances'))
keywords = {}
if 'properties' in performances and 'keywords' in performances[
'properties']:
keywords[path] = performances['properties']['keywords']
for key, value in performances.items():
if key != 'properties':
self.get_keywords(performances[key], keywords,
os.path.join(path, key).strip('./'))
return keywords
def performance_cb(self, msg):
try:
# Track current performance
self._current_performance = msg.data
if msg.data == "null":
if self._before_presentation:
if self._before_presentation.startswith("interacting"):
self.start_interacting()
# Might be usefull if we do idle out
elif self._before_presentation.startswith("idle"):
self.to_idle()
else:
self.timeline_finished()
else:
# Stay in presentation
self.timeline_finished()
else:
# New performance loaded
self._before_presentation = self.state
self.start_presentation()
except Exception as e:
logger.error(e)
def performance_events_cb(self, msg):
if msg.event in ['resume', 'running']:
self.run_timeline()
elif msg.event in ['paused']:
self.timeline_paused()
else:
self.timeline_finished()
def timeline_finished(self):
pass
def need_to_think(self):
# Think in operator control mode (semi automatic or manual)
return self.config.thinking_operator and self.current_tts_mode == 'web_responses'
def on_enter_interacting_listening(self):
# Listen only for some time
self._state_timer = threading.Timer(self.config.listening_time,
self.finish_listening)
self._state_timer.start()
def on_enter_interacting_thinking(self):
# If robot doesnt start speaking go back to listening
self._state_timer = threading.Timer(self.config.thinking_time,
self.could_think_of_anything)
self._state_timer.start()
def on_before_state_change(self):
# Clean state timer
if self._state_timer:
try:
logger.warn("STOP")
self._state_timer.cancel()
self._state_timer = False
except Exception as e:
logger.warn(e)
pass
@property
def disable_attention(self):
return self.props['disable_attention']
@disable_attention.setter
def disable_attention(self, val):
if self.props['disable_attention'] != val:
self.props['disable_attention'] = val
try:
self.clients['attention'].update_configuration(
{'enable_flag': not val})
except Exception as e:
logger.errot(e)
@property
def disable_animations(self):
return self.props['disable_animations']
@disable_animations.setter
def disable_animations(self, val):
if self.props['disable_animations'] != val:
self.props['disable_animations'] = val
try:
logger.warn("Aniamtions disabled: {}".format(val))
self.clients['animation'].update_configuration(
{'enable_flag': not val})
except Exception as e:
logger.error(e)
@property
def disable_blinking(self):
return self.props['disable_blinking']
@disable_blinking.setter
def disable_blinking(self, val):
if self.props['disable_blinking'] != val:
try:
self.services['blender_param'](
"bpy.data.scenes[\"Scene\"].actuators.ACT_blink_randomly.HEAD_PARAM_enabled",
str(not val))
except Exception as e:
logger.error(e)
@property
def disable_saccades(self):
return self.props['disable_saccades']
@disable_saccades.setter
def disable_saccades(self, val):
if self.props['disable_saccades'] != val:
try:
self.services['blender_param'](
"bpy.data.scenes[\"Scene\"].actuators.ACT_saccade.HEAD_PARAM_enabled",
str(not val))
except Exception as e:
logger.error(e)
@property
def disable_keepalive(self):
return self.props['disable_keepalive']
@disable_keepalive.setter
def disable_keepalive(self, val):
if self.props['disable_keepalive'] != val:
self.props['disable_keepalive'] = val
try:
magnitude = 0.0 if val else 1.0
self.topics['soma_pub'].publish(
self._get_soma('normal', magnitude))
self.topics['soma_pub'].publish(
self._get_soma('breathing', magnitude))
self.topics['soma_pub'].publish(
self._get_soma('normal-saccades', magnitude))
except Exception as e:
logger.error(e)
@staticmethod
def _get_soma(name, magnitude):
""" Speech"""
s = SomaState()
s.name = name
s.ease_in.secs = 0
s.ease_in.nsecs = 0.1 * 1000000000
s.magnitude = magnitude
s.rate = 1
return s
def main():
rospy.init_node('ur_driver', disable_signals=True)
if rospy.get_param("use_sim_time", False):
rospy.logwarn("use_sim_time is set!!!")
global prevent_programming
reconfigure_srv = Server(URDriverConfig, reconfigure_callback)
prefix = rospy.get_param("~prefix", "")
print "Setting prefix to %s" % prefix
global joint_names
joint_names = [prefix + name for name in JOINT_NAMES]
# Parses command line arguments
parser = optparse.OptionParser(usage="usage: %prog robot_hostname [reverse_port]")
(options, args) = parser.parse_args(rospy.myargv()[1:])
if len(args) < 1:
parser.error("You must specify the robot hostname")
elif len(args) == 1:
robot_hostname = args[0]
reverse_port = DEFAULT_REVERSE_PORT
elif len(args) == 2:
robot_hostname = args[0]
reverse_port = int(args[1])
if not (0 <= reverse_port <= 65535):
parser.error("You entered an invalid port number")
else:
parser.error("Wrong number of parameters")
# Reads the calibrated joint offsets from the URDF
global joint_offsets
joint_offsets = load_joint_offsets(joint_names)
if len(joint_offsets) > 0:
rospy.loginfo("Loaded calibration offsets from urdf: %s" % joint_offsets)
else:
rospy.loginfo("No calibration offsets loaded from urdf")
# Reads the maximum velocity
# The max_velocity parameter is only used for debugging in the ur_driver. It's not related to actual velocity limits
global max_velocity
max_velocity = rospy.get_param("~max_velocity", MAX_VELOCITY) # [rad/s]
rospy.loginfo("Max velocity accepted by ur_driver: %s [rad/s]" % max_velocity)
# Reads the minimum payload
global min_payload
min_payload = rospy.get_param("~min_payload", MIN_PAYLOAD)
# Reads the maximum payload
global max_payload
max_payload = rospy.get_param("~max_payload", MAX_PAYLOAD)
rospy.loginfo("Bounds for Payload: [%s, %s]" % (min_payload, max_payload))
# Sets up the server for the robot to connect to
server = TCPServer(("", reverse_port), CommanderTCPHandler)
thread_commander = threading.Thread(name="CommanderHandler", target=server.serve_forever)
thread_commander.daemon = True
thread_commander.start()
with open(roslib.packages.get_pkg_dir('ur_driver') + '/prog') as fin:
program = fin.read() % {"driver_hostname": get_my_ip(robot_hostname, PORT), "driver_reverseport": reverse_port}
connection = URConnection(robot_hostname, PORT, program)
connection.connect()
connection.send_reset_program()
connectionRT = URConnectionRT(robot_hostname, RT_PORT)
connectionRT.connect()
set_io_server()
service_provider = None
action_server = None
try:
while not rospy.is_shutdown():
# Checks for disconnect
if getConnectedRobot(wait=False):
time.sleep(0.2)
try:
prevent_programming = rospy.get_param("~prevent_programming")
update = {'prevent_programming': prevent_programming}
reconfigure_srv.update_configuration(update)
except KeyError, ex:
print "Parameter 'prevent_programming' not set. Value: " + str(prevent_programming)
pass
if prevent_programming:
print "Programming now prevented"
connection.send_reset_program()
else:
print "Disconnected. Reconnecting"
if action_server:
action_server.set_robot(None)
rospy.loginfo("Programming the robot")
while True:
# Sends the program to the robot
while not connection.ready_to_program():
print "Waiting to program"
time.sleep(1.0)
try:
prevent_programming = rospy.get_param("~prevent_programming")
update = {'prevent_programming': prevent_programming}
reconfigure_srv.update_configuration(update)
except KeyError, ex:
print "Parameter 'prevent_programming' not set. Value: " + str(prevent_programming)
pass
connection.send_program()
r = getConnectedRobot(wait=True, timeout=1.0)
if r:
break
rospy.loginfo("Robot connected")
#provider for service calls
if service_provider:
service_provider.set_robot(r)
else:
service_provider = URServiceProvider(r)
if action_server:
action_server.set_robot(r)
else:
action_server = URTrajectoryFollower(r, rospy.Duration(1.0))
action_server.start()
class img_interface_node: ## This is the LISTENER for the layer ABOVE
"""The 'img_interface_node' is the class which communicates to the layer above
according to this structure. It provides services and dynamic reconfigure server
and also publishes topics."""
###############################################
## ***** Static Data Members *****
###############################################
# **** Services provided to the above layer
#==================================================
listOf_services = {}
###############################################
## ***** Constructor Method *****
###############################################
def __init__(self, translator, driverMgr):
"""img_interface_node constructor is meant to launch the dynamic reconfigure server
until it is called to launch services for listening. It doesn't receive or return
anything."""
self.translator = translator
self.driverMgr = driverMgr
self.avoidRemoteReconf = 0
try:
## Dynamic Reconfigure
self.avoidRemoteReconf = self.avoidRemoteReconf + 1
# Launch self DynamicReconfigure server
self.dynServer = DynamicReconfigureServer(PropertiesConfig,
self.dynServerCallback)
# Launch request listener services
self.listenToRequests()
rospy.loginfo("Interface for requests created.")
except:
rospy.logerr(
"Error while trying to initialise dynamic parameter server.")
raise
return
###################################################
## ***** General Purpose Methods *****
###################################################
# **** Launch services for receiving requests
#==================================================
def listenToRequests(self):
"""Main services creator method in order to remain listening for requests.
It receives nothing and returns true just for future needs."""
self.listOf_services['get/StringProperty'] = rospy.Service(
'get/StringProperty', stringValue, self.getStringProperty)
self.listOf_services['get/IntProperty'] = rospy.Service(
'get/IntProperty', intValue, self.getIntProperty)
self.listOf_services['get/FloatProperty'] = rospy.Service(
'get/FloatProperty', floatValue, self.getFloatProperty)
self.listOf_services['get/BoolProperty'] = rospy.Service(
'get/BoolProperty', booleanValue, self.getBoolProperty)
##MICHI: 14Feb2012
self.listOf_services['get/DispImage'] = rospy.Service(
'get/DispImage', disparityImage, self.getDispImage)
self.listOf_services['get/Image'] = rospy.Service(
'get/Image', normalImage, self.getImage)
##MICHI: 13Mar2012
self.listOf_services['publishDispImages'] = rospy.Service(
'publishDispImages', requestTopic, self.publishDispImages)
self.listOf_services['publishImages'] = rospy.Service(
'publishImages', requestTopic, self.publishImages)
##MICHI: 16Apr2012
self.listOf_services['get/TopicLocation'] = rospy.Service(
'get/TopicLocation', stringValue, self.getStringProperty)
self.listOf_services['set/TopicLocation'] = rospy.Service(
'set/TopicLocation', setString, self.setTopicLocation)
self.listOf_services['set/StrProperty'] = rospy.Service(
'set/StrProperty', setString, self.setStrProperty)
self.listOf_services['set/IntProperty'] = rospy.Service(
'set/IntProperty', setInteger, self.setIntProperty)
self.listOf_services['set/FloatProperty'] = rospy.Service(
'set/FloatProperty', setFloat, self.setFloatProperty)
self.listOf_services['set/BoolProperty'] = rospy.Service(
'set/BoolProperty', setBoolean, self.setBoolProperty)
rospy.loginfo("Ready to answer service requests.")
return True
###################################################
# **** Dynamic Reconfigure related methods
#==================================================
def dynServerCallback(self, dynConfiguration, levelCode):
"""Handler for the changes in the Dynamic Reconfigure server
Must return a configuration in the same format as received."""
if self.avoidRemoteReconf > 0:
rospy.loginfo("LOCAL config was changed by self (levelCode = %s)" %
levelCode)
self.avoidRemoteReconf = self.avoidRemoteReconf - 1
elif levelCode != 0:
rospy.loginfo("LOCAL configuration changed.".rjust(80, '_'))
for elem in dynConfiguration:
if self.translator.canSet(elem):
success = self.setAnyProperty(elem, dynConfiguration[elem])
if success == False or success == None:
rospy.logerr(
'|__> ...error... while setting property "%s"' %
elem)
rospy.logerr("Changing %s failed with %s..." %
(elem, dynConfiguration[elem]))
dynConfiguration[elem] = self.getAnyProperty(elem)
rospy.logdebug("...%s used" % (dynConfiguration[elem]))
return dynConfiguration
def updateSelfParameters(self, dynConfiguration, avoidPropagation=True):
"""This method is responsible for changing the node's own dynamic reconfigure parameters
from its own code ***but avoiding a chain of uncontrolled callbacks!!.
It returns True if everything went as expected."""
newConfig = {}
for elem in dynConfiguration:
paramName = self.translator.reverseInterpret(elem)
if paramName != "":
newValue = dynConfiguration[elem]
newConfig[paramName] = newValue
else:
rospy.logerr("Error while retrieving reverse translation.")
if avoidPropagation == True:
self.avoidRemoteReconf = self.avoidRemoteReconf + 1
requestResult = self.dynServer.update_configuration(newConfig)
if requestResult != None:
return requestResult
return False
def setTopicLocation(self, setStrMsg):
rospy.loginfo(
str("Received call to set/TopicLocation " + setStrMsg.topicName +
" " + setStrMsg.newValue))
translation = self.translator.interpret(setStrMsg.topicName)
if translation != None and (translation[PPTY_KIND] == "publishedTopic"
or translation[PPTY_KIND] == "topic"):
oldAddress = translation[PPTY_REF]
elif manager3D.is_published(setStrMsg.topicName):
oldAddress = setStrMsg.topicName
else:
print str("The " + setStrMsg.topicName +
" topic wasn't found neither as topic or as a name:")
return str("Exception while relocating. Topic not found.")
try:
rospy.loginfo(str("...relocating " + oldAddress + "..."))
self.driverMgr.relocateTopic(oldAddress, setStrMsg.newValue)
## If it was a name; the value needs to be updated in the translator
if oldAddress == setStrMsg.topicName:
self.translator.updateValue(setStrMsg.topicName,
setStrMsg.newValue)
except Exception as e:
rospy.logerr("Exception while relocating: %s" % (e))
return str("Exception while relocating: %s" % (e))
return "Success!"
###################################################
# **** Generic handlers for getting and setting parameters
#==================================================
def setAnyProperty(self, propertyName, newValue):
"""Generic setter in order to redirect to the type-specific setter method."""
propertyData = self.translator.interpret(propertyName)
if propertyData == None:
return None
#else:
if (propertyData[PPTY_TYPE].lower()).find("string") >= 0:
# if type(newValue) != str:
# print "Error: value '%s' seems to be %s instead of '%s' which is needed"%(newValue, type(newValue), propertyData[PPTY_TYPE])
# return None
valueType = str
elif (propertyData[PPTY_TYPE].lower()).find("int") >= 0:
valueType = int
elif (propertyData[PPTY_TYPE].lower()).find("double") >= 0 or (
propertyData[PPTY_TYPE].lower()).find("float") >= 0:
valueType = float
elif (propertyData[PPTY_TYPE].lower()).find("bool") >= 0:
valueType = bool
else:
rospy.logerr("Error: non registered value type.")
return None
return self.setFixedTypeProperty(propertyName, newValue, valueType)
####################################
# Specific fixed type property setters
#===================================
## The next methods are the callbacks for the setter services
##all of them are supposed to return the resulting values to
##inform in case the set event failed.
## In case there's an error; the error message is sent no
##matter the returning type is
def setStrProperty(self, setterMessage):
return self.setFixedTypeProperty(setterMessage.topicName,
setterMessage.newValue, str)
def setIntProperty(self, setterMessage):
return self.setFixedTypeProperty(setterMessage.topicName,
setterMessage.newValue, int)
def setFloatProperty(self, setterMessage):
return self.setFixedTypeProperty(setterMessage.topicName,
setterMessage.newValue, float)
def setBoolProperty(self, setterMessage):
return self.setFixedTypeProperty(setterMessage.topicName,
setterMessage.newValue, bool)
def setFixedTypeProperty(self, propertyName, newValue, valueType):
"""Main property setter receiving the property name, the value to assign and its type
and returning the response from the "setParameter" method from the low level driver."""
propertyData = self.translator.interpret(propertyName)
#TODO: Any type checking here?
if propertyData == None:
rospy.logerr("Error: trying to set not found property.")
return None
# else:
# print "Obtained %s = %s."%(propertyName, propertyData[PPTY_REF])
resp1 = True
if propertyData[PPTY_KIND] == "dynParam":
paramName = propertyTranslator.getBasename(propertyData[PPTY_REF])
resp1 = self.driverMgr.setParameter(
paramName, newValue,
self.translator.getDynServerPath(paramName), self)
elif propertyData[PPTY_KIND] == "topic":
self.driverMgr.sendByTopic(
rospy.get_namespace() + propertyData[PPTY_REF], newValue,
remoteType[valueType])
else:
resp1 = False
rospy.logerr("Error: unable to set property %s of kind: '%s'" %
(propertyName, propertyData[PPTY_KIND]))
return valueType(resp1)
# Getter and getter handlers
# Generic setter in order to redirect to the appropriate method
def getAnyProperty(self, propertyName):
"""Generic getter in order to redirect to the type-specific getter method."""
propertyData = self.translator.interpret(propertyName)
if propertyData == None:
return None
#else:
if propertyData[PPTY_TYPE].find("string") >= 0:
valueType = str
elif (propertyData[PPTY_TYPE].lower()).find("int") >= 0:
valueType = int
elif (propertyData[PPTY_TYPE].lower()).find("double") >= 0 or (
propertyData[PPTY_TYPE].lower()).find("float") >= 0:
valueType = float
elif (propertyData[PPTY_TYPE].lower()).find("bool") >= 0:
valueType = bool
else:
rospy.logerr("Error: non registered value type")
return None
return self.getFixedTypeProperty(propertyName, valueType)
# Generic getter in order to redirect to the appropriate method
#Specific fixed type property getters
def getStringProperty(self, srvMsg):
return self.getFixedTypeProperty(srvMsg.topicName, str)
def getIntProperty(self, srvMsg):
return self.getFixedTypeProperty(srvMsg.topicName, int)
def getFloatProperty(self, srvMsg):
return self.getFixedTypeProperty(srvMsg.topicName, float)
def getBoolProperty(self, srvMsg):
return self.getFixedTypeProperty(srvMsg.topicName, bool)
def getDispImage(self, srvMsg):
respImages = []
while len(respImages) < srvMsg.nImages:
respImages.append(self.getFixedTypeProperty(
srvMsg.topicName, None))
for image in respImages:
self.driverMgr.sendByTopic("testImages", image, DisparityImage)
return respImages
def getImage(self, srvMsg):
respImages = []
while len(respImages) < srvMsg.nImages:
respImages.append(self.getFixedTypeProperty(
srvMsg.topicName, None))
return respImages
##AMPLIATION: I could add normalImage.timestamp (time[] timestamp) if needed (including the changes in the "normalImage.srv"
## in that case I would need a normalImage variable and set both fields: images and timestamp
def getFixedTypeProperty(self, propertyName, valueType):
"""Main property getter receiving the property name and the value type
and returning the current value from the low level driver."""
propertyData = self.translator.interpret(propertyName)
if propertyData == None:
return None
#else:
if propertyData[PPTY_KIND] == "dynParam":
paramName = propertyTranslator.getBasename(propertyData[PPTY_REF])
resp1 = self.driverMgr.getParameter(
paramName, self.translator.getDynServerPath(propertyName))
elif propertyData[PPTY_KIND] == "publishedTopic":
resp1 = self.driverMgr.getTopic(propertyData[PPTY_REF], valueType)
elif propertyData[PPTY_KIND] == "readOnlyParam":
paramName = propertyTranslator.getBasename(propertyData[PPTY_REF])
location = rospy.search_param(param_name)
resp1 = rospy.get_param(location)
elif propertyData[PPTY_KIND] == "topic":
##MICHI: 14Feb2012
if valueType == str:
resp1 = str(propertyData[PPTY_REF])
else: ## Special case for reading disparity images
valueType2 = DisparityImage
if propertyData[PPTY_TYPE].find("Disparity") < 0:
valueType2 = Image
resp1 = self.driverMgr.getTopic(propertyData[PPTY_REF],
valueType2)
else:
rospy.logerr("Error: unable to get property %s of kind %s" %
(propertyName, propertyData[PPTY_TYPE]))
resp1 = None
return resp1
# Method to request the complete list of properties
def get_property_list(self):
"""Auxiliary method for receiving the list of properties known by the translator."""
return self.translator.get_property_list()
# Methods related to requesting topics to publish
def publishDispImages(self, srvMsg):
"""Specific purpose method meant to retransmit a disparity image topic on a specific topic
path receiving the original path, the new path and the amount of messages. It returns a
message telling if it worked."""
topicPath = self.translator.get_topic_path(srvMsg.sourceTopic)
self.driverMgr.retransmitTopic(srvMsg.nImages, topicPath,
srvMsg.responseTopic, DisparityImage)
"%s images sent from %s topic to %s." % (
srvMsg.nImages, srvMsg.sourceTopic, srvMsg.responseTopic)
return "%s images sent from %s topic to %s." % (
srvMsg.nImages, srvMsg.sourceTopic, srvMsg.responseTopic)
def publishImages(self, srvMsg):
"""Specific purpose method meant to retransmit an image topic on a specific topic
path receiving the original path, the new path and the amount of messages. It returns
a message telling if it worked."""
topicPath = self.translator.get_topic_path(srvMsg.sourceTopic)
self.driverMgr.retransmitTopic(srvMsg.nImages, topicPath,
srvMsg.responseTopic, Image)
"%s images sent from %s topic to %s." % (
srvMsg.nImages, srvMsg.sourceTopic, srvMsg.responseTopic)
return "%s images sent from %s topic to %s." % (
srvMsg.nImages, srvMsg.sourceTopic, srvMsg.responseTopic)
class CameraSynchronizer:
def __init__(self, forearm=True):
stereo_camera_names = [ "narrow_stereo", "wide_stereo" ] # narrow must be first as it can be alternate, and hence has more period restrictions.
forearm_camera_names = [ "forearm_r", "forearm_l" ]
self.camera_names = stereo_camera_names
if forearm:
self.camera_names = self.camera_names + forearm_camera_names
# Parameter names are pretty symmetric. Build them up automatically.
parameters = [ param_rate, param_trig_mode ]
camera_parameters = dict((name, dict((suffix, name+"_"+suffix) for suffix in parameters)) for name in self.camera_names)
# Some parameters are common to the wide and narrow cameras. Also store
# the node names.
for camera in stereo_camera_names:
camera_parameters[camera][param_rate] = "stereo_rate"
camera_parameters[camera]["node_name"] = camera+"_both"
if forearm:
for camera in forearm_camera_names:
camera_parameters[camera]["node_name"] = camera[-1]+"_forearm_cam"
for i in range(0, len(self.camera_names)):
camera_parameters[self.camera_names[i]]["level"] = 1 << i; # This only works because of the specific levels in the .cfg file.
self.projector = Projector()
self.cameras = dict((name, Camera(proj = self.projector, **camera_parameters[name])) for name in self.camera_names)
self.prosilica_inhibit = ProsilicaInhibitTriggerController('prosilica_inhibit_projector_controller', "prosilica_projector_inhibit", 0x0A, 0x00)
self.controllers = [
ProjectorTriggerController('projector_trigger', self.projector),
DualCameraTriggerController('head_camera_trigger', *[self.cameras[name] for name in stereo_camera_names])]
if forearm:
self.controllers = self.controllers + [
SingleCameraTriggerController('r_forearm_cam_trigger', self.cameras["forearm_r"]),
SingleCameraTriggerController('l_forearm_cam_trigger', self.cameras["forearm_l"]),
]
self.server = DynamicReconfigureServer(ConfigType, self.reconfigure)
@staticmethod
def print_threads():
threads = threading.enumerate()
n = 0
for t in threads:
if not t.isDaemon() and t != threading.currentThread():
try:
print t.name
except:
print "Unknown thread ", t
n = n + 1
return n
def kill(self):
print "\nWaiting for all threads to die..."
killAsynchronousUpdaters()
#time.sleep(1)
while self.print_threads() > 0:
print "\nStill waiting for all threads to die..."
time.sleep(1)
print
def reconfigure(self, config, level):
# print "Reconfigure", config
# Reconfigure the projector.
self.projector.process_update(config, level)
self.prosilica_inhibit.process_update(config, level)
# Reconfigure the cameras.
for camera in self.cameras.values():
camera.process_update(config, level)
#for trig_controller in self.trig_controllers:
# trig_controller.update()
#for camera in self.cameras.keys():
# camera.update()
for controller in self.controllers:
controller.update();
for camera in self.cameras.values():
camera.apply_update()
#print config
self.config = config
return config
def update_diagnostics(self):
da = DiagnosticArray()
ds = DiagnosticStatus()
ds.name = rospy.get_caller_id().lstrip('/') + ": Tasks"
in_progress = 0;
longest_interval = 0;
for updater in list(asynchronous_updaters):
(name, interval) = updater.getStatus()
if interval == 0:
msg = "Idle"
else:
in_progress = in_progress + 1
msg = "Update in progress (%i s)"%interval
longest_interval = max(interval, longest_interval)
ds.values.append(KeyValue(name, msg))
if in_progress == 0:
ds.message = "Idle"
else:
ds.message = "Updates in progress: %i"%in_progress
if longest_interval > 10:
ds.level = 1
ds.message = "Update is taking too long: %i"%in_progress
ds.hardware_id = "none"
da.status.append(ds)
da.header.stamp = rospy.get_rostime()
self.diagnostic_pub.publish(da)
def spin(self):
self.diagnostic_pub = rospy.Publisher("/diagnostics", DiagnosticArray)
try:
reset_count = 0
rospy.loginfo("Camera synchronizer is running...")
controller_update_count = 0
while not rospy.is_shutdown():
if self.config['camera_reset'] == True:
reset_count = reset_count + 1
if reset_count > 3:
self.server.update_configuration({'camera_reset' : False})
else:
reset_count = 0
self.update_diagnostics()
# In case the controllers got restarted, refresh their state.
controller_update_count += 1
if controller_update_count >= 10:
controller_update_count = 0
for controller in self.controllers:
controller.update();
rospy.sleep(1)
finally:
rospy.signal_shutdown("Main thread exiting")
self.kill()
print "Main thread exiting"
class JoyController:
def __init__(self,options=None):
self.options = options
self.joy_scale = [-1,1,-1,1,1] #RPYTY
self.trim_roll = 0
self.trim_pitch = 0
self.max_angle = 30
self.max_yawangle = 200
self.max_thrust = 80.
self.min_thrust = 25.
self.max_thrust_raw = percentageToThrust(self.max_thrust)
self.min_thrust_raw = percentageToThrust(self.min_thrust)
self.old_thurst_raw = 0
self.slew_limit = 45
self.slew_rate = 30
self.slew_limit_raw = percentageToThrust(self.slew_limit)
self.slew_rate_raw = percentageToThrust(self.slew_rate)
self.dynserver = None
self.prev_cmd = None
self.curr_cmd = None
self.yaw_joy = True
self.sub_joy = rospy.Subscriber("/joy", JoyMSG, self.new_joydata)
self.sub_tf = tf.TransformListener()
self.pub_tf = tf.TransformBroadcaster()
self.pub_cfJoy = rospy.Publisher("/cfjoy", CFJoyMSG)
# Dynserver
self.dynserver = DynamicReconfigureServer(CFJoyCFG, self.reconfigure)
def released(self, id):
return self.prev_cmd.buttons[id] and not self.curr_cmd.buttons[id]
def pressed(self, id):
return not self.prev_cmd.buttons[id] and self.curr_cmd.buttons[id]
def held(self, id):
return self.prev_cmd.buttons[id] and self.curr_cmd.buttons[id]
#Needed so the class can change the dynserver values with button presses
def set_dynserver(self, server):
self.dynserver = server
def thurstToRaw(self, joy_thrust):
# Deadman button or invalid thrust
if not self.curr_cmd.buttons[Button.L1] or joy_thrust>1:
return 0
raw_thrust = 0
if joy_thrust > 0.01:
raw_thrust = self.min_thrust_raw + joy_thrust*(self.max_thrust_raw-self.min_thrust_raw)
if self.slew_rate_raw>0 and self.slew_limit_raw > raw_thrust:
if self.old_thurst_raw > self.slew_limit_raw:
self.old_thurst_raw = self.slew_limit_raw
if raw_thrust < (self.old_thurst_raw - (self.slew_rate_raw/100)):
raw_thrust = self.old_thurst_raw - self.slew_rate_raw/100
if joy_thrust < 0 or raw_thrust < self.min_thrust_raw:
raw_thrust = 0
self.old_thurst_raw = raw_thrust
return raw_thrust
def new_joydata(self, joymsg):
global Button
global Axes
#Should run at 100hz. Use launch files roslaunch crazyflie_ros joy.launch
if self.prev_cmd == None:
self.prev_cmd = joymsg
print len(joymsg.axes)
#USB mode
if len(joymsg.axes) == 27:
Button = enum(L1=10,R1=11,Select=0,Start=3,L2=8,R2=9,Up=4,Right=5,Down=6,Left=7,Square=15,Cross=14,Triangle=12,Circle=13)
Axes = enum(SLL=0,SLU=1,SRL=2,SRU=3,Up=4,Right=5,Down=6,Left=7,L2=4+8,R2=4+9,L1=4+10,R1=4+11,Triangle=4+12,Circle=4+13,Cross=4+14,Square=4+15,AccL=16,AccF=17,AccU=18,GyroY=19)
return
self.curr_cmd = joymsg
hover = False
# print "AXES"
# for i,x in enumerate(joymsg.axes):
# print " ",i,":",x
# print "BUTTONS
# for i,x in enumerate(joymsg.buttons):
# print " ",i,":",x
x = 0
y = 0
z = 0
r = 0
r2 = 0
# Get stick positions [-1 1]
if self.curr_cmd.buttons[Button.L1]:
x = self.joy_scale[0] * self.curr_cmd.axes[Axes.SLL] # Roll
y = self.joy_scale[1] * self.curr_cmd.axes[Axes.SLU] # Pitch
r = self.joy_scale[2] * self.curr_cmd.axes[Axes.SRL] # Yaw
z = self.joy_scale[3] * self.curr_cmd.axes[Axes.SRU] # Thrust
r2 = self.joy_scale[4] * (self.curr_cmd.axes[Axes.L2] - self.curr_cmd.axes[Axes.R2])
hover = self.curr_cmd.axes[Axes.L1]<-0.75
roll = x * self.max_angle
pitch = y * self.max_angle
yaw = 0
#TODO use dyn reconf to decide which to use
if r2!=0:
yaw = r2 * self.max_yawangle
else:
if self.yaw_joy:
# Deadzone
if r < -0.2 or r > 0.2:
if r < 0:
yaw = (r + 0.2) * self.max_yawangle * 1.25
else:
yaw = (r - 0.2) * self.max_yawangle * 1.25
if hover:
thrust = int(round(deadband(z,0.2)*32767 + 32767)) #Convert to uint16
else:
thrust = self.thurstToRaw(z)
trimmed_roll = roll + self.trim_roll
trimmed_pitch = pitch + self.trim_pitch
# Control trim manually
new_settings = {}
if self.curr_cmd.buttons[Button.Left]:
new_settings["trim_roll"] = max(self.trim_roll + self.curr_cmd.axes[Axes.Left]/10.0, -10)
if self.curr_cmd.buttons[Button.Right]:
new_settings["trim_roll"] = min(self.trim_roll - self.curr_cmd.axes[Axes.Right]/10.0, 10)
if self.curr_cmd.buttons[Button.Down]:
new_settings["trim_pitch"] = max(self.trim_pitch + self.curr_cmd.axes[Axes.Down]/10.0, -10)
if self.curr_cmd.buttons[Button.Up]:
new_settings["trim_pitch"] = min(self.trim_pitch - self.curr_cmd.axes[Axes.Up]/10.0, 10)
# Set trim to current input
if self.released(Button.R1):
new_settings["trim_roll"] = min(10, max(trimmed_roll, -10))
new_settings["trim_pitch"] = min(10, max(trimmed_pitch, -10))
rospy.loginfo("Trim updated Roll/Pitch: %r/%r", round(new_settings["trim_roll"],2), round(new_settings["trim_roll"],2))
# Reset Trim
if self.released(Button.Square):
new_settings["trim_roll"] = 0
new_settings["trim_pitch"] = 0
rospy.loginfo("Pitch reset to 0/0")
if new_settings != {} and self.dynserver!=None:
self.dynserver.update_configuration(new_settings)
# TODO: new firmware doesnt need hover_change, it just uses thrust directly
#(trimmed_roll,trimmed_pitch,yaw,thrust,hover,hover_set, z)
msg = CFJoyMSG()
msg.header.stamp = rospy.Time.now()
msg.roll = trimmed_roll
msg.pitch = trimmed_pitch
msg.yaw = yaw
msg.thrust = thrust
msg.hover = hover
#msg.calib = self.pressed(Button.Triangle)
self.pub_cfJoy.publish(msg)
# Cache prev joy command
self.prev_cmd = self.curr_cmd
def reconfigure(self, config, level):
self.trim_roll = config["trim_roll"]
self.trim_pitch = config["trim_pitch"]
self.max_angle = config["max_angle"]
self.max_yawangle = config["max_yawangle"]
self.max_thrust = config["max_thrust"]
self.min_thrust = config["min_thrust"]
self.slew_limit = config["slew_limit"]
self.slew_rate = config["slew_rate"]
self.max_thrust_raw = percentageToThrust(self.max_thrust)
self.min_thrust_raw = percentageToThrust(self.min_thrust)
self.slew_limit_raw = percentageToThrust(self.slew_limit)
self.slew_rate_raw = percentageToThrust(self.slew_rate)
self.yaw_joy = config["yaw_joy"]
return config
class ROSFollowControllers(object):
def __init__(self):
rospy.init_node('follow_controllers')
cmd_topic = rospy.get_param('~cmd_topic', 'cmd_vel_input')
use_mocap = rospy.get_param("~use_mocap", True)
self.active_controller_pub = rospy.Publisher(
'active_controller', UInt8, latch=True, queue_size=None)
self.controllers = FollowControllers(
v1_model_path=rospy.get_param('~v1_model_path'),
v2_model_path=rospy.get_param('~v2_model_path'),
v3_model_path=rospy.get_param('~v3_model_path'))
self.should_publish_all_controllers = rospy.get_param('~enable_debug_pubs')
self.active_controller = Controller(rospy.get_param('~controller'))
self.srv = Server(ControllersConfig, self.callback)
video = message_filters.Subscriber('video', CompressedImage)
vo_odom_drone = message_filters.Subscriber('vo_odom_drone', Odometry)
subs = [video, vo_odom_drone]
if use_mocap:
mocap_odom_drone = message_filters.Subscriber('mocap_odom_drone', Odometry)
mocap_odom_head = message_filters.Subscriber('mocap_odom_head', Odometry)
subs += [mocap_odom_drone, mocap_odom_head]
self.controller_pub = {controller: rospy.Publisher(
'output_{name}'.format(name=controller.name), Twist, queue_size=1)
for controller in Controller if controller != Controller.idle}
self.cmd_pub = rospy.Publisher(cmd_topic, Twist, queue_size=1)
message_filters.ApproximateTimeSynchronizer(
subs, queue_size=10, slop=0.1).registerCallback(self.update)
self.last_controller_duration = {}
updater = diagnostic_updater.Updater()
updater.setHardwareID("ML")
updater.add("Controllers", self.controllers_diagnostics)
def update_diagnostics(event):
updater.update()
rospy.Timer(rospy.Duration(1), update_diagnostics)
freq_bounds = {'min': 4, 'max': 6}
self.pub_freq = diagnostic_updater.HeaderlessTopicDiagnostic(
cmd_topic, updater, diagnostic_updater.FrequencyStatusParam(freq_bounds, 0.1, 10))
if use_mocap:
self.start_pose = PoseStamped()
self.start_pose.header.frame_id = 'World'
self.start_pose.pose.position = Point(
rospy.get_param('~start_x', 0),
rospy.get_param('~start_y', 0),
rospy.get_param('~start_z', 1))
start_yaw = rospy.get_param('~start_yaw', 0)
self.start_pose.pose.orientation.w = np.cos(start_yaw * 0.5)
self.start_pose.pose.orientation.z = np.sin(start_yaw * 0.5)
#self.goto = actionlib.SimpleActionClient('fence_control', GoToPoseAction)
#self.goto.wait_for_server()
else:
self.goto = None
self.start_pose = None
# self.target_pub = rospy.Publisher('target_pose', PoseStamped, queue_size=1)
rospy.Subscriber('joy', Joy, self.stop, queue_size=1)
rospy.loginfo("Ready to start")
def stop(self, msg):
#if msg.buttons[6] == 1 and self.goto is not None and self.start_pose is not None:
# self.start_pose.header.stamp = rospy.Time.now()
# goal = GoToPoseGoal(target_pose=self.start_pose)
# self.goto.send_goal(goal)
# self.goto.wait_for_result(rospy.Duration(20))
# return
old_controller = self.active_controller
if msg.buttons[7] == 1:
self.active_controller = Controller.idle
elif msg.axes[5] > 0:
self.active_controller = Controller.exact
elif msg.axes[4] < 0:
self.active_controller = Controller.v1
elif msg.axes[5] < 0:
self.active_controller = Controller.v2
elif msg.axes[4] > 0:
self.active_controller = Controller.v3
else:
return
if old_controller != self.active_controller:
self.srv.update_configuration(
{'controller': self.active_controller.value,
'enable_debug_pubs': self.should_publish_all_controllers})
self.active_controller_pub.publish(self.active_controller.value)
def controllers_diagnostics(self, stat):
stat.summary(diagnostic_msgs.msg.DiagnosticStatus.OK, "")
for c in Controller:
if c == Controller.idle:
continue
if c in self.last_controller_duration:
value = 'last update {0:.1f} [ms]'.format(1000 * self.last_controller_duration[c])
else:
value = '-'
if c == self.active_controller:
value += ' (active)'
stat.add(c.name, value)
def callback(self, config, level):
self.should_publish_all_controllers = config['enable_debug_pubs']
old_controller = self.active_controller
self.active_controller = Controller(config['controller'])
if old_controller != self.active_controller:
self.active_controller_pub.publish(self.active_controller.value)
return config
def update(self, video, vo_odom_drone, mocap_odom_drone=None, mocap_odom_head=None):
active_controller = self.active_controller
# TODO: check shape
frame = frame_from_msg(video)
rospy.loginfo(frame.shape)
v = vo_odom_drone.twist.twist.linear
drone_velocity = [v.x, v.y, v.z]
if mocap_odom_drone is not None and mocap_odom_head is not None:
p = mocap_odom_drone.pose.pose.position
drone_position_world = np.array([p.x, p.y, p.z])
p = mocap_odom_head.pose.pose.position
head_position_world = np.array([p.x, p.y, p.z])
q = mocap_odom_drone.pose.pose.orientation
# We assume pitch and roll = 0 (as the camera is stabilized)
_, _, drone_yaw_world = euler_from_quaternion([q.x, q.y, q.z, q.w])
q = mocap_odom_head.pose.pose.orientation
_, _, head_yaw_world = euler_from_quaternion([q.x, q.y, q.z, q.w])
head_position = rotate_z(head_position_world - drone_position_world, -drone_yaw_world)
head_yaw = head_yaw_world - drone_yaw_world
else:
head_position = None
head_yaw = None
last_controller_duration = {}
if active_controller != Controller.idle:
start = time()
output = self.controllers.update(
controller=active_controller, frame=frame, drone_velocity=drone_velocity,
target_position=head_position, target_yaw=head_yaw)
duration = time() - start
output = twist_from_control(output)
if self.should_publish_all_controllers:
self.controller_pub[active_controller].publish(output)
self.cmd_pub.publish(output)
self.pub_freq.tick()
last_controller_duration[active_controller] = duration
if self.should_publish_all_controllers:
for controller in Controller:
if controller not in [Controller.idle, active_controller]:
start = time()
output = self.controllers.update(
controller=controller, frame=frame, drone_velocity=drone_velocity,
target_position=head_position, target_yaw=head_yaw)
output = twist_from_control(output)
duration = time() - start
self.controller_pub[controller].publish(output)
last_controller_duration[controller] = duration
self.last_controller_duration = last_controller_duration
class VisionServer:
def __init__(self):
self.server = actionlib.SimpleActionServer('track_object', TrackObjectAction, self.execute, False)
self.server.start()
self.lower = None
self.upper = None
self.targetType = None
self.srv = Server(ThresholdsConfig, self.updateThresholds)
self.thresholds = {int(k):v for k,v in rospy.get_param("/vision_thresholds").items()}
self.bridge = CvBridge()
self.leftImage = np.zeros((1,1,3), np.uint8)
self.leftModel = image_geometry.PinholeCameraModel()
self.newLeft = False
self.rightImage = np.zeros((1,1,3), np.uint8)
self.rightModel = image_geometry.PinholeCameraModel()
self.newRight = False
self.downImage = np.zeros((1,1,3), np.uint8)
self.downModel = image_geometry.PinholeCameraModel()
self.newDown = False
self.disparityImage = np.zeros((1,1,3), np.uint8)
self.stereoModel = image_geometry.StereoCameraModel()
self.newDisparity = False
self.poleFinder = PoleFinder()
self.gateFinder = GateFinder()
self.diceFinder = DiceFinder()
self.pathFinder = PathFinder()
self.response = TrackObjectResult()
self.downSub = rospy.Subscriber('/down_camera/image_color', Image, self.downwardsCallback)
self.downInfoSub = rospy.Subscriber('/down_camera/info', CameraInfo, self.downInfoCallback)
self.stereoSub = rospy.Subscriber('/stereo/disparity', Image, self.stereoCallback)
#self.stereoInfoSub = rospy.Subscriber('/stereo/info', CameraInfo, self.stereoInfoCallback)
self.leftSub = rospy.Subscriber('/stereo/left/image', WFOVImage, self.leftCallback)
#self.leftInfoSub = rospy.Subscriber('/stereo/left/image', WFOVImage, self.leftInfoCallback)
self.rightSub = rospy.Subscriber('/stereo/right/image', WFOVImage, self.rightCallback)
#self.rightInfoSub = rospy.Subscriber('/stereo/right/image', WFOVImage, self.rightInfoCallback)
self.bridge = CvBridge()
self.image_pub = rospy.Publisher("/thresh_image", Image, queue_size=10)
#self.thresholds = self.loadThresholds()
def updateThresholds(self, config, level):
self.lower = np.array([config["lowH"], config["lowS"], config["lowL"]],dtype=np.uint8)
self.upper = np.array([config["upH"], config["upS"], config["upL"]],dtype=np.uint8)
if self.targetType is not None:
self.thresholds[self.targetType] = [self.lower.tolist(), self.upper.tolist()]
rospy.set_param("/vision_thresholds/"+str(self.targetType), [self.lower.tolist(), self.upper.tolist()])
return config
def setThresholds(self, lower, upper):
self.lower = np.array(lower,dtype=np.uint8)
self.upper = np.array(upper,dtype=np.uint8)
self.srv.update_configuration({"lowH":lower[0], "lowS":lower[1], "lowV":lower[2], "upH":upper[0], "upS":upper[1], "upV":upper[2]})
def execute(self, goal):
self.targetType = goal.objectType
self.setThresholds(*self.thresholds[self.targetType])
self.feedback = TrackObjectFeedback()
self.found = False
self.running = True
self.ok = True
rightImageRect = np.zeros(self.rightImage.shape, self.rightImage.dtype)
leftImageRect = np.zeros(self.leftImage.shape, self.leftImage.dtype)
downImageRect = np.zeros(self.downImage.shape, self.downImage.dtype)
r = rospy.Rate(60)
while self.running and not rospy.is_shutdown():
if self.server.is_preempt_requested() or self.server.is_new_goal_available():
self.running = False
continue
r.sleep()
if self.rightModel.width is not None and self.rightImage is not None:
self.rightModel.rectifyImage(self.rightImage, rightImageRect)
else:
rospy.logwarn_throttle(1, "No right camera model")
continue
if self.leftModel.width is not None and self.leftImage is not None:
self.leftModel.rectifyImage(self.leftImage, leftImageRect)
else:
rospy.logwarn_throttle(1, "No left camera model")
continue #We need the left camera model for stuff
if self.downModel.width is not None and self.downImage is not None:
self.downModel.rectifyImage(self.downImage, downImageRect)
else:
rospy.logwarn_throttle(1, "No down camera model")
continue #We need the down camera model for stuff
if self.targetType == TrackObjectGoal.startGate:
if self.newRight:
self.feedback = self.gateFinder.process(leftImageRect, rightImageRect, self.disparityImage, self.leftModel, self.stereoModel, self.upper, self.lower)
self.newRight = False
elif self.targetType == TrackObjectGoal.pole:
if self.newRight:
self.feedback = self.poleFinder.process(leftImageRect, rightImageRect, self.disparityImage, self.leftModel, self.stereoModel, self.upper, self.lower)
self.newRight = False
elif self.targetType == TrackObjectGoal.dice:
if self.newRight:
self.feedback = self.diceFinder.process(leftImageRect, rightImageRect, self.disparityImage, self.leftModel, self.stereoModel, goal.diceNum, self.upper, self.lower)
self.newRight = False
elif self.targetType == TrackObjectGoal.path:
if self.newDown:
self.feedback = self.pathFinder.process(downImageRect, self.downModel, self.upper, self.lower)
self.newDown = False
else:
self.ok = False
break
if self.feedback.found:
self.server.publish_feedback(self.feedback)
self.feedback.found = False
self.response.found=True
if not goal.servoing:
self.running = False
self.response.stoppedOk=self.ok
self.server.set_succeeded(self.response)
#TODO: Fix color thresholds
def downwardsCallback(self, msg):
try:
self.downImage = self.bridge.imgmsg_to_cv2(msg, "bgr8")
self.newDown = True
except CvBridgeError as e:
print(e)
#self.downModel.rectifyImage(self.downImage, self.downImage)
def stereoCallback(self, msg):
try:
self.disparityImage = self.bridge.imgmsg_to_cv2(msg, "bgr8")
self.newDisparity = True
except CvBridgeError as e:
print(e)
def leftCallback(self, msg):
try:
self.leftImage = self.bridge.imgmsg_to_cv2(msg.image, "bgr8")
self.leftModel.fromCameraInfo(msg.info)
self.newLeft = True
except CvBridgeError as e:
print(e)
def rightCallback(self, msg):
try:
self.rightImage = self.bridge.imgmsg_to_cv2(msg.image, "bgr8")
self.rightModel.fromCameraInfo(msg.info)
self.newRight = True
except CvBridgeError as e:
print(e)
def downInfoCallback(self, msg):
self.downModel.fromCameraInfo(msg)
def rightInfoCallback(self, msg):
self.rightModel.fromCameraInfo(msg.info)
def leftInfoCallback(self, msg):
self.leftModel.fromCameraInfo(msg.info)
def loadThresholds(self):
#with open(os.path.dirname(__file__) + '/../thresholds.json') as data_file:
#json_data = json.load(data_file)
data = {}
#for entry in json_data:
# high = entry['high']
#low = entry['low']
#data[entry['color']] = vision_utils.Thresholds(upperThresh=(high['hue'],high['sat'],high['val']), lowerThresh=(low['hue'],low['sat'],low['val']))
return data
class OGridServer:
def __init__(self, frame_id='enu', map_size=500, resolution=0.3, rate=1):
self.frame_id = frame_id
self.ogrids = {}
self.odom = None
# Some default values
self.plow = True
self.plow_factor = 0
self.ogrid_min_value = -1
self.draw_bounds = False
self.resolution = resolution
self.ogrid_timeout = 2
self.enforce_bounds = False
self.enu_bounds = [[0, 0, 1], [0, 0, 1], [0, 0, 1], [0, 0, 1]]
# Default to centering the ogrid
position = np.array([-(map_size * resolution) / 2, -(map_size * resolution) / 2, 0])
quaternion = np.array([0, 0, 0, 1])
self.origin = mil_tools.numpy_quat_pair_to_pose(position, quaternion)
self.global_ogrid = self.create_grid((map_size, map_size))
def set_odom(msg):
return setattr(self, 'odom', mil_tools.pose_to_numpy(msg.pose.pose))
rospy.Subscriber('/odom', Odometry, set_odom)
self.publisher = rospy.Publisher('/ogrid_master', OccupancyGrid, queue_size=1)
self.ogrid_server = Server(OgridConfig, self.dynamic_cb)
self.dynam_client = Client("bounds_server", config_callback=self.bounds_cb)
self.ogrid_server.update_configuration({'width': 500})
rospy.Service("/center_ogrid", Trigger, self.center_ogrid)
rospy.Timer(rospy.Duration(1.0 / rate), self.publish)
def center_ogrid(self, srv):
fprint("CENTERING OGRID AT POSITION", msg_color='blue')
if self.odom is None:
return
dim = -(self.map_size[0] * self.resolution) / 2
new_org = self.odom[0] + np.array([dim, dim, 0])
config = {}
config['origin_x'] = float(new_org[0])
config['origin_y'] = float(new_org[1])
config['set_origin'] = True
self.ogrid_server.update_configuration(config)
def bounds_cb(self, config):
fprint("BOUNDS DYNAMIC CONFIG UPDATE!", msg_color='blue')
if hasattr(config, "enu_1_lat"):
self.enu_bounds = [[config['enu_1_lat'], config['enu_1_long'], 1],
[config['enu_2_lat'], config['enu_2_long'], 1],
[config['enu_3_lat'], config['enu_3_long'], 1],
[config['enu_4_lat'], config['enu_4_long'], 1],
[config['enu_1_lat'], config['enu_1_long'], 1]]
self.enforce_bounds = config['enforce']
def dynamic_cb(self, config, level):
fprint("OGRID DYNAMIC CONFIG UPDATE!", msg_color='blue')
topics = config['topics'].replace(' ', '').split(',')
replace_topics = config['replace_topics'].replace(' ', '').split(',')
new_grids = {}
for topic in topics:
new_grids[topic] = OGrid(topic) if topic not in self.ogrids else self.ogrids[topic]
for topic in replace_topics:
new_grids[topic] = OGrid(topic, replace=True) if topic not in self.ogrids else self.ogrids[topic]
self.ogrids = new_grids
map_size = map(int, (config['height'], config['width']))
self.map_size = map_size
self.plow = config['plow']
self.plow_factor = config['plow_factor']
self.ogrid_min_value = config['ogrid_min_value']
self.draw_bounds = config['draw_bounds']
self.resolution = config['resolution']
self.ogrid_timeout = config['ogrid_timeout']
if config['set_origin']:
fprint("Setting origin!")
position = np.array([config['origin_x'], config['origin_y'], 0])
quaternion = np.array([0, 0, 0, 1])
self.origin = mil_tools.numpy_quat_pair_to_pose(position, quaternion)
else:
position = np.array([-(map_size[1] * self.resolution) / 2, -(map_size[0] * self.resolution) / 2, 0])
quaternion = np.array([0, 0, 0, 1])
self.origin = mil_tools.numpy_quat_pair_to_pose(position, quaternion)
self.global_ogrid = self.create_grid(map_size)
return config
def create_grid(self, map_size):
"""
Creates blank ogrids for everyone for the low low price of $9.95!
`map_size` should be in the form of (h, w)
"""
ogrid = OccupancyGrid()
ogrid.header.stamp = rospy.Time.now()
ogrid.header.frame_id = self.frame_id
ogrid.info.resolution = self.resolution
ogrid.info.width = map_size[1]
ogrid.info.height = map_size[0]
ogrid.info.origin = self.origin
# TODO: Make sure this produces the correct sized ogrid
ogrid.data = np.full((map_size[1], map_size[0]), -1).flatten()
# fprint('Created Occupancy Map', msg_color='blue')
return ogrid
def publish(self, *args):
# fprint("Merging Maps", newline=2)
global_ogrid = deepcopy(self.global_ogrid)
np_grid = numpyify(global_ogrid)
# Global ogrid (only compute once)
corners = get_enu_corners(global_ogrid)
index_limits = transform_enu_to_ogrid(corners, global_ogrid)[:, :2]
g_x_min = index_limits[0][0]
g_x_max = index_limits[1][0]
g_y_min = index_limits[0][1]
g_y_max = index_limits[1][1]
to_add = [o for o in self.ogrids.itervalues() if not o.replace]
to_replace = [o for o in self.ogrids.itervalues() if o.replace]
for ogrids in [to_add, to_replace]:
for ogrid in ogrids:
# Hard coded 5 second timeout - probably no need to reconfig this.
if ogrid.nav_ogrid is None or ogrid.callback_delta > self.ogrid_timeout:
# fprint("Ogrid too old!")
continue
# Proactively checking for errors.
# This should be temporary but probably wont be.
l_h, l_w = ogrid.nav_ogrid.info.height, ogrid.nav_ogrid.info.width
g_h, g_w = global_ogrid.info.height, global_ogrid.info.width
if l_h > g_h or l_w > g_w:
fprint("Proactively preventing errors in ogrid size.", msg_color="red")
new_size = max(l_w, g_w, l_h, g_h)
self.global_ogrid = self.create_grid([new_size, new_size])
# Local Ogrid (get everything in global frame though)
corners = get_enu_corners(ogrid.nav_ogrid)
index_limits = transform_enu_to_ogrid(corners, ogrid.nav_ogrid)
index_limits = transform_between_ogrids(index_limits, ogrid.nav_ogrid, global_ogrid)[:, :2]
l_x_min = index_limits[0][0]
l_x_max = index_limits[1][0]
l_y_min = index_limits[0][1]
l_y_max = index_limits[1][1]
xs = np.sort([g_x_max, g_x_min, l_x_max, l_x_min])
ys = np.sort([g_y_max, g_y_min, l_y_max, l_y_min])
# These are indicies in cell units
start_x, end_x = np.round(xs[1:3]) # Grabbing indices 1 and 2
start_y, end_y = np.round(ys[1:3])
# Should be indicies
l_ogrid_start = transform_between_ogrids([start_x, start_y, 1], global_ogrid, ogrid.nav_ogrid)
# fprint("ROI {},{} {},{}".format(start_x, start_y, end_x, end_y))
index_width = l_ogrid_start[0] + end_x - start_x # I suspect rounding will be a source of error
index_height = l_ogrid_start[1] + end_y - start_y
# fprint("width: {}, height: {}".format(index_width, index_height))
# fprint("Ogrid size: {}, {}".format(ogrid.nav_ogrid.info.height, ogrid.nav_ogrid.info.width))
to_add = ogrid.np_map[l_ogrid_start[1]:index_height, l_ogrid_start[0]:index_width]
# fprint("to_add shape: {}".format(to_add.shape))
# Make sure the slicing doesn't produce an error
end_x = start_x + to_add.shape[1]
end_y = start_y + to_add.shape[0]
try:
# fprint("np_grid shape: {}".format(np_grid[start_y:end_y, start_x:end_x].shape))
fprint("{}, {}".format(ogrid.topic, ogrid.replace))
if ogrid.replace:
np_grid[start_y:end_y, start_x:end_x] = to_add
else:
np_grid[start_y:end_y, start_x:end_x] += to_add
except Exception as e:
fprint("Exception caught, probably a dimension mismatch:", msg_color='red')
print e
fprint("w: {}, h: {}".format(global_ogrid.info.width, global_ogrid.info.height), msg_color='red')
if self.draw_bounds and self.enforce_bounds:
ogrid_bounds = transform_enu_to_ogrid(self.enu_bounds, global_ogrid).astype(np.int32)
for i, point in enumerate(ogrid_bounds[:, :2]):
if i == 0:
last_point = point
continue
cv2.line(np_grid, tuple(point), tuple(last_point), 100, 3)
last_point = point
if self.plow:
self.plow_snow(np_grid, global_ogrid)
# Clip and flatten grid
np_grid = np.clip(np_grid, self.ogrid_min_value, 100)
global_ogrid.data = np_grid.flatten().astype(np.int8)
self.publisher.publish(global_ogrid)
def plow_snow(self, np_grid, ogrid):
"""Remove region around the boat so we never touch an occupied cell (making lqrrt not break
if something touches us).
"""
if self.odom is None:
return np_grid
p, q = self.odom
yaw_rot = trns.euler_from_quaternion(q)[2] # rads
boat_width = params.boat_length + params.boat_buffer + self.plow_factor # m
boat_height = params.boat_width + params.boat_buffer + self.plow_factor # m
x, y, _ = transform_enu_to_ogrid([p[0], p[1], 1], ogrid)
theta = yaw_rot
w = boat_width / ogrid.info.resolution
h = boat_height / ogrid.info.resolution
box = cv2.boxPoints(((x, y), (w, h), np.degrees(theta)))
box = np.int0(box)
cv2.drawContours(np_grid, [box], 0, 0, -1)
# Draw a "boat" in the ogrid
boat_width = params.boat_length + params.boat_buffer
boat_height = params.boat_width + params.boat_buffer
x, y, _ = transform_enu_to_ogrid([p[0], p[1], 1], ogrid)
w = boat_width / ogrid.info.resolution
h = boat_height / ogrid.info.resolution
box = cv2.boxPoints(((x, y), (w, h), np.degrees(theta)))
box = np.int0(box)
cv2.drawContours(np_grid, [box], 0, 40, -1)
# fprint("Plowed snow!")
return np_grid
class VectorDriver:
def __init__(self):
"""
Variables to track communication frequency for debugging
"""
self.summer=0
self.samp = 0
self.avg_freq = 0.0
self.start_frequency_samp = False
self.need_to_terminate = False
self.flush_rcvd_data=True
self.update_base_local_planner = False
self.parameter_server_is_updating = False
self.last_move_base_update = rospy.Time.now().to_sec()
"""
Initialize the publishers for VECTOR
"""
self.vector_data = VECTOR_DATA()
"""
Start the thread for the linear actuator commands
"""
self._linear = LinearActuator()
if (False == self._linear.init_success):
rospy.logerr("Could not initialize the linear actuator interface! exiting...")
return
"""
Initialize faultlog related items
"""
self.is_init = True
self.extracting_faultlog = False
"""
Initialize the dynamic reconfigure server for VECTOR
"""
self.param_server_initialized = False
self.dyn_reconfigure_srv = Server(vectorConfig, self._dyn_reconfig_callback)
"""
Wait for the parameter server to set the configs and then set the IP address from that.
Note that this must be the current ethernet settings of the platform. If you want to change it
set the ethernet settings at launch to the current ethernet settings, power up, change them, power down, set the
the ethernet settings at launch to the new ones and relaunch
"""
r = rospy.Rate(10)
start_time = rospy.Time.now().to_sec()
while ((rospy.Time.now().to_sec() - start_time) < 3.0) and (False == self.param_server_initialized):
r.sleep()
if (False == self.param_server_initialized):
rospy.logerr("Parameter server not found, you must pass an initial yaml in the launch! exiting...")
return
"""
Create the thread to run VECTOR communication
"""
self.tx_queue_ = multiprocessing.Queue()
self.rx_queue_ = multiprocessing.Queue()
self.comm = IoEthThread((os.environ['VECTOR_IP_ADDRESS'],int(os.environ['VECTOR_IP_PORT_NUM'])),
self.tx_queue_,
self.rx_queue_,
max_packet_size=1248)
if (False == self.comm.link_up):
rospy.logerr("Could not open socket for VECTOR...")
self.comm.close()
return
"""
Initialize the publishers and subscribers for the node
"""
self.faultlog_pub = rospy.Publisher('/vector/feedback/faultlog', Faultlog, queue_size=10,latch=True)
rospy.Subscriber("/vector/cmd_vel", Twist, self._add_motion_command_to_queue)
rospy.Subscriber("/vector/gp_command",ConfigCmd,self._add_config_command_to_queue)
rospy.Subscriber("/move_base/DWAPlannerROS/parameter_updates",Config,self._update_move_base_params)
"""
Start the receive handler thread
"""
self.terminate_mutex = threading.RLock()
self.last_rsp_rcvd = rospy.Time.now().to_sec()
self._rcv_thread = threading.Thread(target = self._run)
self._rcv_thread.start()
"""
Start streaming continuous data
"""
rospy.loginfo("Stopping the data stream")
if (False == self._continuous_data(False)):
rospy.logerr("Could not stop VECTOR communication stream")
self.Shutdown()
return
"""
Extract the faultlog at startup
"""
self.flush_rcvd_data=False
rospy.loginfo("Extracting the faultlog")
self.extracting_faultlog = True
if (False == self._extract_faultlog()):
rospy.logerr("Could not get retrieve VECTOR faultlog")
self.Shutdown()
return
"""
Start streaming continuous data
"""
rospy.loginfo("Starting the data stream")
if (False == self._continuous_data(True)):
rospy.logerr("Could not start VECTOR communication stream")
self.Shutdown()
return
self.start_frequency_samp = True
"""
Force the configuration to update the first time to ensure that the variables are set to
the correct values on the machine
"""
if (False == self._force_machine_configuration(True)):
rospy.logerr("Initial configuration parameteters my not be valid, please check them in the yaml file")
rospy.logerr("The ethernet address must be set to the present address at startup, to change it:")
rospy.logerr("start the machine; change the address using rqt_reconfigure; shutdown; update the yaml and restart")
self.Shutdown()
return
"""
Uncomment the line below to always unlock gain tuning and force the upload based on the parameters in the yaml file
you must have the key to do this because it can be dangerous if one sets unstable gains. 0x00000000 needs to be replaced
by the key
"""
#self._unlock_gain_tuning(0x00000000)
#self._force_pid_configuration(True)
"""
Finally update the values for dynamic reconfigure with the ones reported by the machine
"""
new_config = dict({"x_vel_limit_mps" : self.vector_data.config_param.machcfg.x_vel_limit_mps,
"y_vel_limit_mps" : self.vector_data.config_param.machcfg.y_vel_limit_mps,
"accel_limit_mps2" : self.vector_data.config_param.machcfg.accel_limit_mps2,
"dtz_decel_limit_mps2": self.vector_data.config_param.machcfg.dtz_decel_limit_mps2,
"yaw_rate_limit_rps" : self.vector_data.config_param.machcfg.yaw_rate_limit_rps,
"wheel_diameter_m": self.vector_data.config_param.machcfg.wheel_diameter_m,
"wheel_base_length_m": self.vector_data.config_param.machcfg.wheelbase_length_m,
"wheel_track_width_m": self.vector_data.config_param.machcfg.wheel_track_width_m,
"gear_ratio" : self.vector_data.config_param.machcfg.gear_ratio,
"motion_while_charging" : ((self.vector_data.config_param.machcfg.config_bitmap >> DISABLE_AC_PRESENT_CSI_SHIFT) & 1) ^ 1,
"motion_ctl_input_filter" : (self.vector_data.config_param.machcfg.config_bitmap >> MOTION_MAPPING_FILTER_SHIFT) & VALID_MOTION_MAPPING_FILTER_MASK,
"p_gain_rps_per_rps" : self.vector_data.config_param.ctlconfig.p_gain_rps_per_rps,
"i_gain_rps_per_rad" : self.vector_data.config_param.ctlconfig.i_gain_rps_per_rad,
"d_gain_rps_per_rps2" : self.vector_data.config_param.ctlconfig.d_gain_rps_per_rps2,
"fdfwd_gain_rps_per_motor_rps" : self.vector_data.config_param.ctlconfig.fdfwd_gain_rps_per_motor_rps,
"p_error_limit_rps" : self.vector_data.config_param.ctlconfig.p_error_limit_rps,
"i_error_limit_rad" : self.vector_data.config_param.ctlconfig.i_error_limit_rad,
"i_error_drain_rate_rad_per_frame" : self.vector_data.config_param.ctlconfig.i_error_drain_rate_rad_per_frame,
"input_target_limit_rps" : self.vector_data.config_param.ctlconfig.input_target_limit_rps,
"output_limit_rps" : self.vector_data.config_param.ctlconfig.input_target_limit_rps})
self.dyn_reconfigure_srv.update_configuration(new_config)
rospy.loginfo("Vector Driver is up and running")
def Shutdown(self):
with self.terminate_mutex:
self.need_to_terminate = True
rospy.loginfo("Vector Driver has called the Shutdown method, terminating")
self._linear.Shutdown()
self.comm.Close()
self.tx_queue_.close()
self.rx_queue_.close()
def _run(self):
while not self.need_to_terminate:
"""
Run until signaled to stop
Perform the actions defined based on the flags passed out
"""
result = select.select([self.rx_queue_._reader],[],[],0.02)
if len(result[0]) > 0:
data = result[0][0].recv()
with self.terminate_mutex:
if not self.need_to_terminate:
self._handle_rsp(data)
def _add_command_to_queue(self,command):
"""
Create a byte array with the CRC from the command
"""
cmd_bytes = generate_cmd_bytes(command)
"""
Send it
"""
self.tx_queue_.put(cmd_bytes)
def _update_rcv_frq(self):
if (True == self.start_frequency_samp):
self.samp+=1
self.summer+=1.0/(rospy.Time.now().to_sec() - self.last_rsp_rcvd)
self.avg_freq = self.summer/self.samp
self.last_rsp_rcvd = rospy.Time.now().to_sec()
def _handle_rsp(self,data_bytes):
self._update_rcv_frq()
if (True == self.flush_rcvd_data):
return
valid_data,rsp_data = validate_response(data_bytes)
if (False == valid_data):
rospy.logerr("bad vector data packet")
return
if (self.extracting_faultlog):
if (len(rsp_data) == NUMBER_OF_FAULTLOG_WORDS):
self.extracting_faultlog = False
faultlog_msg = Faultlog()
faultlog_msg.data = rsp_data
self.faultlog_pub.publish(faultlog_msg)
elif (len(rsp_data) == NUMBER_OF_VECTOR_RSP_WORDS):
header_stamp = self.vector_data.status.parse(rsp_data[START_STATUS_BLOCK:END_STATUS_BLOCK])
wheel_circum = self.vector_data.config_param.parse(rsp_data[START_APP_CONFIG_BLOCK:END_FRAM_CONFIG_BLOCK],header_stamp)
self.vector_data.auxiliary_power.parse(rsp_data[START_BATTERY_DATA_BLOCK:END_BATTERY_DATA_BLOCK],header_stamp)
self.vector_data.propulsion.parse(rsp_data[START_PROPULSION_DATA_BLOCK:END_PROPULSION_DATA_BLOCK],header_stamp)
self.vector_data.dynamics.parse(rsp_data[START_DYNAMICS_DATA_BLOCK:END_DYNAMICS_DATA_BLOCK],header_stamp,wheel_circum)
self.vector_data.imu.parse_data(rsp_data[START_IMU_DATA_BLOCK:END_IMU_DATA_BLOCK],header_stamp)
rospy.logdebug("feedback received from vector")
def _add_motion_command_to_queue(self,command):
"""
Add the command to the queue, platform does command limiting and mapping
"""
cmds = [MOTION_CMD_ID,[convert_float_to_u32(command.linear.x),
convert_float_to_u32(command.linear.y),
convert_float_to_u32(command.angular.z)]]
if (False == self.parameter_server_is_updating):
self._add_command_to_queue(cmds)
def _add_config_command_to_queue(self,command):
try:
cmds = [GENERAL_PURPOSE_CMD_ID,[command_ids[command.gp_cmd],command.gp_param]]
self._add_command_to_queue(cmds)
except:
rospy.logerr("Config param failed, it is probably not known")
return
def _dyn_reconfig_callback(self,config,level):
"""
Note: for some reason the commands collide (queue gets garbled) if the
motion commands and reconfiguration are sent at the same time; so
we just gate the motion commands when reconfiguring which should be OK in general
since most result in zero velocity on the embedded side. Need to dig in further
to understand why this is happening
"""
self.parameter_server_is_updating = True
rospy.sleep(0.1)
"""
The first time through we want to ignore the values because they are just defaults from the ROS
parameter server which has no knowledge of the platform being used
"""
if (True == self.is_init):
self.is_init = False
return config
"""
Create the configuration bitmap from the appropriate variables
"""
config_bitmap = (((config.motion_while_charging^1) << DISABLE_AC_PRESENT_CSI_SHIFT)|
(config.motion_ctl_input_filter << MOTION_MAPPING_FILTER_SHIFT))
"""
Define the configuration parameters for all the platforms
"""
self.valid_config_cmd = [LOAD_MACH_CONFIG_CMD_ID,
[convert_float_to_u32(config.x_vel_limit_mps),
convert_float_to_u32(config.y_vel_limit_mps),
convert_float_to_u32(config.accel_limit_mps2),
convert_float_to_u32(config.decel_limit_mps2),
convert_float_to_u32(config.dtz_decel_limit_mps2),
convert_float_to_u32(config.yaw_rate_limit_rps),
convert_float_to_u32(config.yaw_accel_limit_rps2),
convert_float_to_u32(config.wheel_diameter_m),
convert_float_to_u32(config.wheel_base_length_m),
convert_float_to_u32(config.wheel_track_width_m),
convert_float_to_u32(config.gear_ratio),
config_bitmap]]
"""
The teleop limits are always the minimum of the actual machine limit and the ones set for teleop
"""
config.teleop_x_vel_limit_mps = minimum_f(config.teleop_x_vel_limit_mps, config.x_vel_limit_mps)
config.teleop_y_vel_limit_mps = minimum_f(config.teleop_y_vel_limit_mps, config.x_vel_limit_mps)
config.teleop_accel_limit_mps2 = minimum_f(config.teleop_accel_limit_mps2, config.accel_limit_mps2)
config.teleop_yaw_rate_limit_rps = minimum_f(config.teleop_yaw_rate_limit_rps, config.yaw_rate_limit_rps)
config.teleop_yaw_accel_limit_rps2 = minimum_f(config.teleop_yaw_accel_limit_rps2, config.teleop_yaw_accel_limit_rps2)
"""
Set the teleop configuration in the feedback
"""
self.vector_data.config_param.SetTeleopConfig([config.teleop_x_vel_limit_mps,
config.teleop_y_vel_limit_mps,
config.teleop_accel_limit_mps2,
config.teleop_yaw_rate_limit_rps,
config.teleop_yaw_accel_limit_rps2])
"""
Define the configuration parameters for all the platforms
"""
self.valid_pid_cmd = [SET_PID_CONFIG_CMD_ID,
[convert_float_to_u32(config.p_gain_rps_per_rps),
convert_float_to_u32(config.i_gain_rps_per_rad),
convert_float_to_u32(config.d_gain_rps_per_rps2),
convert_float_to_u32(config.fdfwd_gain_rps_per_motor_rps),
convert_float_to_u32(config.p_error_limit_rps),
convert_float_to_u32(config.i_error_limit_rad),
convert_float_to_u32(config.d_error_limit_rps2),
convert_float_to_u32(config.i_error_drain_rate_rad_per_frame),
convert_float_to_u32(config.output_limit_rps),
convert_float_to_u32(config.input_target_limit_rps)]]
"""
Update the linear actuator velocity limit
"""
"""
Check and see if we need to store the parameters in NVM before we try, although the NVM is F-RAM
with unlimited read/write, uneccessarily setting the parameters only introduces risk for error
"""
load_config_params = False
if self.param_server_initialized:
if ((1<<7) == ((1<<7) & level)):
self._linear.UpdateVelLimit(config.linear_actuator_vel_limit_mps)
if ((1<<2) == ((1<<2) & level)):
self._force_machine_configuration()
load_config_params = True
config.x_vel_limit_mps = round(self.vector_data.config_param.machcfg.x_vel_limit_mps,3)
config.y_vel_limit_mps = round(self.vector_data.config_param.machcfg.y_vel_limit_mps,3)
config.accel_limit_mps2 = round(self.vector_data.config_param.machcfg.accel_limit_mps2,3)
config.decel_limit_mps2 = round(self.vector_data.config_param.machcfg.decel_limit_mps2,3)
config.dtz_decel_limit_mps2 = round(self.vector_data.config_param.machcfg.dtz_decel_limit_mps2,3)
config.yaw_rate_limit_rps = round(self.vector_data.config_param.machcfg.yaw_rate_limit_rps,3)
config.yaw_accel_limit_rps2 = round(self.vector_data.config_param.machcfg.yaw_accel_limit_rps2,3)
config.wheel_diameter_m = round(self.vector_data.config_param.machcfg.wheel_diameter_m,3)
config.wheel_base_length_m = round(self.vector_data.config_param.machcfg.wheelbase_length_m,3)
config.wheel_track_width_m = round(self.vector_data.config_param.machcfg.wheel_track_width_m,3)
config.gear_ratio = round(self.vector_data.config_param.machcfg.gear_ratio,3)
config.motion_while_charging = ((self.vector_data.config_param.machcfg.config_bitmap >> DISABLE_AC_PRESENT_CSI_SHIFT) & 1) ^ 1
config.motion_ctl_input_filter = (self.vector_data.config_param.machcfg.config_bitmap >> MOTION_MAPPING_FILTER_SHIFT) & VALID_MOTION_MAPPING_FILTER_MASK
if self.param_server_initialized:
if ((1<<5) == ((1<<5) & level)):
if self.vector_data.config_param.ctlconfig.control_tuning_unlocked:
self._force_pid_configuration()
if (True == config.set_default_gains):
cmds = [GENERAL_PURPOSE_CMD_ID,[2002,0]]
self._add_command_to_queue(cmds)
config.set_default_gains = False
rospy.sleep(0.1)
config.p_gain_rps_per_rps = round(self.vector_data.config_param.ctlconfig.p_gain_rps_per_rps,3)
config.i_gain_rps_per_rad = round(self.vector_data.config_param.ctlconfig.i_gain_rps_per_rad,3)
config.d_gain_rps_per_rps2 = round(self.vector_data.config_param.ctlconfig.d_gain_rps_per_rps2,3)
config.fdfwd_gain_rps_per_motor_rps = round(self.vector_data.config_param.ctlconfig.fdfwd_gain_rps_per_motor_rps,3)
config.p_error_limit_rps = round(self.vector_data.config_param.ctlconfig.p_error_limit_rps,3)
config.i_error_limit_rad = round(self.vector_data.config_param.ctlconfig.i_error_limit_rad,3)
config.d_error_limit_rps2 = round(self.vector_data.config_param.ctlconfig.d_error_limit_rps2,3)
config.i_error_drain_rate_rad_per_frame = round(self.vector_data.config_param.ctlconfig.i_error_drain_rate_rad_per_frame,5)
config.output_limit_rps = round(self.vector_data.config_param.ctlconfig.output_limit_rps,3)
config.input_target_limit_rps = round(self.vector_data.config_param.ctlconfig.input_target_limit_rps,3)
if (True == config.send_unlock_request):
try:
key = int(config.unlock_key,16)
self._unlock_gain_tuning(key)
except:
rospy.logerr("Invalid Key Value!!!! Must be a string in 32bit hex format")
config.send_unlock_request = False
self.valid_config = config
if (True == load_config_params) or (False == self.param_server_initialized):
self.update_base_local_planner = True
self._update_move_base_params()
self.param_server_initialized = True
self.parameter_server_is_updating = False
return config
def _update_move_base_params(self):
"""
If parameter updates have not been called in the last 5 seconds allow the
subscriber callback to set them
"""
if ((rospy.Time.now().to_sec()-self.last_move_base_update) > 5.0):
self.update_base_local_planner = True
if self.update_base_local_planner:
self.update_base_local_planner = False
self.last_move_base_update = rospy.Time.now().to_sec()
try:
dyn_reconfigure_client= Client("/move_base/DWAPlannerROS",timeout=1.0)
changes = dict()
changes['acc_lim_x'] = maximum_f(self.valid_config.accel_limit_mps2, self.valid_config.decel_limit_mps2)
changes['acc_lim_y'] = maximum_f(self.valid_config.accel_limit_mps2, self.valid_config.decel_limit_mps2)
changes['acc_lim_th'] = self.valid_config.yaw_accel_limit_rps2
changes['max_vel_x'] = self.valid_config.x_vel_limit_mps
changes['max_vel_y'] = self.valid_config.y_vel_limit_mps
changes['max_rot_vel'] = self.valid_config.yaw_rate_limit_rps
dyn_reconfigure_client.update_configuration(changes)
dyn_reconfigure_client.close()
except:
pass
rospy.loginfo("Vector Driver updated move_base parameters to match machine parameters")
def _unlock_gain_tuning(self,key):
r = rospy.Rate(10)
attempts = 0
while (False == self.vector_data.config_param.ctlconfig.control_tuning_unlocked) and (attempts < 3):
cmds = [GENERAL_PURPOSE_CMD_ID,[2001,key]]
self._add_command_to_queue(cmds)
attempts += 1
r.sleep()
if (True == self.vector_data.config_param.ctlconfig.control_tuning_unlocked):
rospy.loginfo("Controller tuning successfully unlocked")
else:
rospy.logerr("Failed to unlock controller tuning, the key is likely incorrect")
def _continuous_data(self,start_cont):
set_continuous = [GENERAL_PURPOSE_CMD_ID,[GENERAL_PURPOSE_CMD_SEND_CONTINUOUS_DATA,start_cont]]
ret = False
if (True == start_cont):
r = rospy.Rate(10)
start_time = rospy.Time.now().to_sec()
while ((rospy.Time.now().to_sec() - start_time) < 3.0) and (True == self.vector_data.status.init):
self._add_command_to_queue(set_continuous)
r.sleep()
ret = not self.vector_data.status.init
else:
r = rospy.Rate(5)
start_time = rospy.Time.now().to_sec()
while ((rospy.Time.now().to_sec() - start_time) < 3.0) and (False == ret):
self._add_command_to_queue(set_continuous)
rospy.sleep(0.6)
if ((rospy.Time.now().to_sec() - self.last_rsp_rcvd) > 0.5):
ret = True
r.sleep()
self.vector_data.status.init = True
return ret
def _extract_faultlog(self):
r = rospy.Rate(2)
start_time = rospy.Time.now().to_sec()
while ((rospy.Time.now().to_sec() - start_time) < 3.0) and (True == self.extracting_faultlog):
self._add_command_to_queue([GENERAL_PURPOSE_CMD_ID,[GENERAL_PURPOSE_CMD_SEND_FAULTLOG,0]])
r.sleep()
return not self.extracting_faultlog
def _force_machine_configuration(self,echo=False):
"""
Load all the parameters on the machine at startup; first check if they match, if they do continue.
Otherwise load them and check again.
"""
r = rospy.Rate(5)
start_time = rospy.get_time()
params_loaded = False
new_params = False
while ((rospy.get_time() - start_time) < 10.0) and (False == params_loaded):
load_params = False
for i in range(NUMBER_OF_CONFIG_PARAM_VARIABLES):
if (self.vector_data.config_param.configuration_feedback[i] != self.valid_config_cmd[1][i]):
load_params = True
if (True == load_params):
self._add_command_to_queue(self.valid_config_cmd)
new_params = True
r.sleep()
else:
params_loaded = True
if (new_params == True) or (echo == True):
rospy.loginfo("New Machine Parameters Loaded.......")
rospy.loginfo("x_vel_limit_mps: %.4f"%self.vector_data.config_param.machcfg.x_vel_limit_mps)
rospy.loginfo("y_vel_limit_mps: %.4f"%self.vector_data.config_param.machcfg.y_vel_limit_mps)
rospy.loginfo("accel_limit_mps2: %.4f"%self.vector_data.config_param.machcfg.accel_limit_mps2)
rospy.loginfo("decel_limit_mps2: %.4f"%self.vector_data.config_param.machcfg.decel_limit_mps2)
rospy.loginfo("dtz_decel_limit_mps2: %.4f"%self.vector_data.config_param.machcfg.dtz_decel_limit_mps2)
rospy.loginfo("yaw_rate_limit_rps: %.4f"%self.vector_data.config_param.machcfg.yaw_rate_limit_rps)
rospy.loginfo("yaw_accel_limit_rps2: %.4f"%self.vector_data.config_param.machcfg.yaw_accel_limit_rps2)
rospy.loginfo("wheel_diameter_m: %.4f"%self.vector_data.config_param.machcfg.wheel_diameter_m)
rospy.loginfo("wheel_base_length_m: %.4f"%self.vector_data.config_param.machcfg.wheelbase_length_m)
rospy.loginfo("wheel_track_width_m: %.4f"%self.vector_data.config_param.machcfg.wheel_track_width_m)
rospy.loginfo("gear_ratio: %.4f"%self.vector_data.config_param.machcfg.gear_ratio)
rospy.loginfo("motion_while_charging: %u"%(((self.vector_data.config_param.machcfg.config_bitmap >> DISABLE_AC_PRESENT_CSI_SHIFT) & 1) ^ 1))
rospy.loginfo("motion_ctl_input_filter: %u\n"%((self.vector_data.config_param.machcfg.config_bitmap >> MOTION_MAPPING_FILTER_SHIFT) & VALID_MOTION_MAPPING_FILTER_MASK))
elif not params_loaded:
rospy.logerr("Failed to load machine parameters!!!!!!!!!!")
return params_loaded
def _force_pid_configuration(self,echo=False):
r = rospy.Rate(5)
start_time = rospy.get_time()
params_loaded = False
new_params = False
while ((rospy.get_time() - start_time) < 10.0) and (False == params_loaded):
load_params = False
for i in range(16,(16+NUMBER_OF_PID_PARAM_VARIABLES)):
if (self.vector_data.config_param.configuration_feedback[i] != self.valid_pid_cmd[1][i-16]):
load_params = True
if (True == load_params):
self._add_command_to_queue(self.valid_pid_cmd)
r.sleep()
new_params = True
else:
params_loaded = True
if (new_params == True) or (echo == True):
rospy.loginfo("New PID Parameters Loaded.......")
rospy.loginfo("p_gain_rps_per_rps: %.4f"%self.vector_data.config_param.ctlconfig.p_gain_rps_per_rps)
rospy.loginfo("i_gain_rps_per_rps: %.4f"%self.vector_data.config_param.ctlconfig.i_gain_rps_per_rad)
rospy.loginfo("d_gain_rps_per_rps: %.4f"%self.vector_data.config_param.ctlconfig.d_gain_rps_per_rps2)
rospy.loginfo("fdfwd_gain_rps_per_motor_rps: %.4f"%self.vector_data.config_param.ctlconfig.fdfwd_gain_rps_per_motor_rps)
rospy.loginfo("p_error_limit_rps: %.4f"%self.vector_data.config_param.ctlconfig.p_error_limit_rps)
rospy.loginfo("i_error_limit_rps: %.4f"%self.vector_data.config_param.ctlconfig.i_error_limit_rad)
rospy.loginfo("d_error_limit_rps: %.4f"%self.vector_data.config_param.ctlconfig.d_error_limit_rps2)
rospy.loginfo("i_error_drain_rate_rad_per_frame: %.4f"%self.vector_data.config_param.ctlconfig.i_error_drain_rate_rad_per_frame)
rospy.loginfo("output_limit_rps: %.4f"%self.vector_data.config_param.ctlconfig.output_limit_rps)
rospy.loginfo("input_target_limit_rps: %.4f\n"%self.vector_data.config_param.ctlconfig.input_target_limit_rps)
elif not params_loaded:
rospy.logerr("Failed to load machine parameters!!!!!!!!!!")
return params_loaded
class CameraSynchronizer:
def __init__(self, forearm=True):
stereo_camera_names = [
"narrow_stereo", "wide_stereo"
] # narrow must be first as it can be alternate, and hence has more period restrictions.
forearm_camera_names = ["forearm_r", "forearm_l"]
self.camera_names = stereo_camera_names
if forearm:
self.camera_names = self.camera_names + forearm_camera_names
# Parameter names are pretty symmetric. Build them up automatically.
parameters = [param_rate, param_trig_mode]
camera_parameters = dict(
(name, dict((suffix, name + "_" + suffix)
for suffix in parameters))
for name in self.camera_names)
# Some parameters are common to the wide and narrow cameras. Also store
# the node names.
for camera in stereo_camera_names:
camera_parameters[camera][param_rate] = "stereo_rate"
camera_parameters[camera]["node_name"] = camera + "_both"
if forearm:
for camera in forearm_camera_names:
camera_parameters[camera][
"node_name"] = camera[-1] + "_forearm_cam"
for i in range(0, len(self.camera_names)):
camera_parameters[self.camera_names[i]]["level"] = 1 << i
# This only works because of the specific levels in the .cfg file.
self.projector = Projector()
self.cameras = dict(
(name, Camera(proj=self.projector, **camera_parameters[name]))
for name in self.camera_names)
self.prosilica_inhibit = ProsilicaInhibitTriggerController(
'prosilica_inhibit_projector_controller',
"prosilica_projector_inhibit", 0x0A, 0x00)
self.controllers = [
ProjectorTriggerController('projector_trigger', self.projector),
DualCameraTriggerController(
'head_camera_trigger',
*[self.cameras[name] for name in stereo_camera_names])
]
if forearm:
self.controllers = self.controllers + [
SingleCameraTriggerController('r_forearm_cam_trigger',
self.cameras["forearm_r"]),
SingleCameraTriggerController('l_forearm_cam_trigger',
self.cameras["forearm_l"]),
]
self.server = DynamicReconfigureServer(ConfigType, self.reconfigure)
@staticmethod
def print_threads():
threads = threading.enumerate()
n = 0
for t in threads:
if not t.isDaemon() and t != threading.currentThread():
try:
print t.name
except:
print "Unknown thread ", t
n = n + 1
return n
def kill(self):
print "\nWaiting for all threads to die..."
killAsynchronousUpdaters()
#time.sleep(1)
while self.print_threads() > 0:
print "\nStill waiting for all threads to die..."
time.sleep(1)
print
def reconfigure(self, config, level):
# print "Reconfigure", config
# Reconfigure the projector.
self.projector.process_update(config, level)
self.prosilica_inhibit.process_update(config, level)
# Reconfigure the cameras.
for camera in self.cameras.values():
camera.process_update(config, level)
#for trig_controller in self.trig_controllers:
# trig_controller.update()
#for camera in self.cameras.keys():
# camera.update()
for controller in self.controllers:
controller.update()
for camera in self.cameras.values():
camera.apply_update()
#print config
self.config = config
return config
def update_diagnostics(self):
da = DiagnosticArray()
ds = DiagnosticStatus()
ds.name = rospy.get_caller_id().lstrip('/') + ": Tasks"
in_progress = 0
longest_interval = 0
for updater in list(asynchronous_updaters):
(name, interval) = updater.getStatus()
if interval == 0:
msg = "Idle"
else:
in_progress = in_progress + 1
msg = "Update in progress (%i s)" % interval
longest_interval = max(interval, longest_interval)
ds.values.append(KeyValue(name, msg))
if in_progress == 0:
ds.message = "Idle"
else:
ds.message = "Updates in progress: %i" % in_progress
if longest_interval > 10:
ds.level = 1
ds.message = "Update is taking too long: %i" % in_progress
ds.hardware_id = "none"
da.status.append(ds)
da.header.stamp = rospy.get_rostime()
self.diagnostic_pub.publish(da)
def spin(self):
self.diagnostic_pub = rospy.Publisher("/diagnostics", DiagnosticArray)
try:
reset_count = 0
rospy.loginfo("Camera synchronizer is running...")
controller_update_count = 0
while not rospy.is_shutdown():
if self.config['camera_reset'] == True:
reset_count = reset_count + 1
if reset_count > 3:
self.server.update_configuration(
{'camera_reset': False})
else:
reset_count = 0
self.update_diagnostics()
# In case the controllers got restarted, refresh their state.
controller_update_count += 1
if controller_update_count >= 10:
controller_update_count = 0
for controller in self.controllers:
controller.update()
rospy.sleep(1)
finally:
rospy.signal_shutdown("Main thread exiting")
self.kill()
print "Main thread exiting"
class JoyController:
def __init__(self,options=None):
self.options = options
self.joy_scale = [-1,1,-1,1,1] #RPYTY
self.trim_roll = 0
self.trim_pitch = 0
self.max_angle = 30
self.max_yawangle = 200
self.max_thrust = 80.
self.min_thrust = 25.
self.max_thrust_raw = percentageToThrust(self.max_thrust)
self.min_thrust_raw = percentageToThrust(self.min_thrust)
self.old_thurst_raw = 0
self.slew_limit = 45
self.slew_rate = 30
self.slew_limit_raw = percentageToThrust(self.slew_limit)
self.slew_rate_raw = percentageToThrust(self.slew_rate)
self.dynserver = None
self.prev_cmd = None
self.curr_cmd = None
self.hover = False
self.sub_joy = rospy.Subscriber("/joy", JoyMSG, self.new_joydata)
self.sub_tf = tf.TransformListener()
self.pub_tf = tf.TransformBroadcaster()
self.pub_cfJoy = rospy.Publisher("/cfjoy", CFJoyMSG)
# Dynserver
self.dynserver = DynamicReconfigureServer(CFJoyCFG, self.reconfigure)
def released(self, id):
return self.prev_cmd.buttons[id] and not self.curr_cmd.buttons[id]
def pressed(self, id):
return not self.prev_cmd.buttons[id] and self.curr_cmd.buttons[id]
def held(self, id):
return self.prev_cmd.buttons[id] and self.curr_cmd.buttons[id]
#Needed so the class can change the dynserver values with button presses
def set_dynserver(self, server):
self.dynserver = server
def thurstToRaw(self, joy_thrust):
# Deadman button or invalid thrust
if not self.curr_cmd.buttons[Button.L1] or joy_thrust>1:
return 0
raw_thrust = 0
if joy_thrust > 0.01:
raw_thrust = self.min_thrust_raw + joy_thrust*(self.max_thrust_raw-self.min_thrust_raw)
if self.slew_rate_raw>0 and self.slew_limit_raw > raw_thrust:
if self.old_thurst_raw > self.slew_limit_raw:
self.old_thurst_raw = self.slew_limit_raw
if raw_thrust < (self.old_thurst_raw - (self.slew_rate_raw/100)):
raw_thrust = self.old_thurst_raw - self.slew_rate_raw/100
if joy_thrust < 0 or raw_thrust < self.min_thrust_raw:
raw_thrust = 0
self.old_thurst_raw = raw_thrust
return raw_thrust
def new_joydata(self, joymsg):
global Button
global Axes
#Should run at 100hz. Use launch files roslaunch crazyflie_row joy.launch
if self.prev_cmd == None:
self.prev_cmd = joymsg
print len(joymsg.axes)
#USB mode
if len(joymsg.axes) == 27:
Button = enum(L1=10,R1=11,Select=0,Start=3,L2=8,R2=9,Up=4,Right=5,Down=6,Left=7,Square=15,Cross=14,Triangle=12,Circle=13)
Axes = enum(SLL=0,SLU=1,SRL=2,SRU=3,Up=4,Right=5,Down=6,Left=7,L2=4+8,R2=4+9,L1=4+10,R1=4+11,Triangle=4+12,Circle=4+13,Cross=4+14,Square=4+15,AccL=16,AccF=17,AccU=18,GyroY=19)
return (0,0,0,0,False,False,0)
self.curr_cmd = joymsg
hover = False
# print "AXES"
# for i,x in enumerate(joymsg.axes):
# print " ",i,":",x
# print "BUTTONS
# for i,x in enumerate(joymsg.buttons):
# print " ",i,":",x
x = 0
y = 0
z = 0
r = 0
r2 = 0
# Get stick positions [-1 1]
if self.curr_cmd.buttons[Button.L1]:
x = self.joy_scale[0] * self.curr_cmd.axes[Axes.SLL] # Roll
y = self.joy_scale[1] * self.curr_cmd.axes[Axes.SLU] # Pitch
r = self.joy_scale[2] * self.curr_cmd.axes[Axes.SRL] # Yaw
z = self.joy_scale[3] * self.curr_cmd.axes[Axes.SRU] # Thrust
r2 = self.joy_scale[4] * (self.curr_cmd.axes[Axes.L2] - self.curr_cmd.axes[Axes.R2])
hover = self.curr_cmd.axes[Axes.L1]<-0.75
roll = x * self.max_angle
pitch = y * self.max_angle
yaw = 0
#TODO use dyn reconf to decide which to use
if r2!=0:
yaw = r2 * self.max_yawangle
else:
# Deadzone
if r < -0.2 or r > 0.2:
if r < 0:
yaw = (r + 0.2) * self.max_yawangle * 1.25
else:
yaw = (r - 0.2) * self.max_yawangle * 1.25
thrust = self.thurstToRaw(z)
trimmed_roll = roll + self.trim_roll
trimmed_pitch = pitch + self.trim_pitch
# Control trim manually
new_settings = {}
if self.curr_cmd.buttons[Button.Left]:
new_settings["trim_roll"] = max(self.trim_roll + self.curr_cmd.axes[Axes.Left]/10.0, -10)
if self.curr_cmd.buttons[Button.Right]:
new_settings["trim_roll"] = min(self.trim_roll - self.curr_cmd.axes[Axes.Right]/10.0, 10)
if self.curr_cmd.buttons[Button.Down]:
new_settings["trim_pitch"] = max(self.trim_pitch + self.curr_cmd.axes[Axes.Down]/10.0, -10)
if self.curr_cmd.buttons[Button.Up]:
new_settings["trim_pitch"] = min(self.trim_pitch - self.curr_cmd.axes[Axes.Up]/10.0, 10)
# Set trim to current input
if self.released(Button.R1):
new_settings["trim_roll"] = min(10, max(trimmed_roll, -10))
new_settings["trim_pitch"] = min(10, max(trimmed_pitch, -10))
rospy.loginfo("Trim updated Roll/Pitch: %r/%r", round(new_settings["trim_roll"],2), round(new_settings["trim_roll"],2))
# Reset Trim
if self.released(Button.Square):
new_settings["trim_roll"] = 0
new_settings["trim_pitch"] = 0
rospy.loginfo("Pitch reset to 0/0")
if new_settings != {} and self.dynserver!=None:
self.dynserver.update_configuration(new_settings)
hover_set = hover and not self.hover
self.hover = hover
#(trimmed_roll,trimmed_pitch,yaw,thrust,hover,hover_set, z)
msg = CFJoyMSG()
msg.header.stamp = rospy.Time.now()
msg.roll = trimmed_roll
msg.pitch = trimmed_pitch
msg.yaw = yaw
msg.thrust = thrust
msg.hover = hover
msg.hover_set = hover_set
msg.hover_change = z
msg.calib = self.pressed(Button.Triangle)
self.pub_cfJoy.publish(msg)
# Cache prev joy command
self.prev_cmd = self.curr_cmd
def reconfigure(self, config, level):
self.trim_roll = config["trim_roll"]
self.trim_pitch = config["trim_pitch"]
self.max_angle = config["max_angle"]
self.max_yawangle = config["max_yawangle"]
self.max_thrust = config["max_thrust"]
self.min_thrust = config["min_thrust"]
self.slew_limit = config["slew_limit"]
self.slew_rate = config["slew_rate"]
self.max_thrust_yaw = percentageToThrust(self.max_thrust)
self.min_thrust_yaw = percentageToThrust(self.min_thrust)
self.slew_limit_yaw = percentageToThrust(self.slew_limit)
self.slew_rate_yaw = percentageToThrust(self.slew_rate)
return config
vel_topic = rospy.get_param('~vel_topic', vel_topic)
## GPIO
servo_pin = rospy.get_param('~servo_pin', servo_pin)
motor_pin = rospy.get_param('~motor_pin', motor_pin)
middle_servo = rospy.get_param('~middle_servo', middle_servo)
middle_motor = rospy.get_param('~middle_motor', middle_motor)
revers_servo = rospy.get_param('~revers_servo', revers_servo)
offset = rospy.get_param('~servo_offset', offset)
## rover params
wheelbase = rospy.get_param('~wheelbase', wheelbase)
if rospy.has_param('~max_vel'):
max_vel = rospy.get_param('~max_vel', max_vel)
cfg_srv.update_configuration({"max_vel": max_vel})
if rospy.has_param('~min_vel'):
min_vel = rospy.get_param('~min_vel', min_vel)
cfg_srv.update_configuration({"min_vel": min_vel})
if rospy.has_param('~max_angle'):
max_angle = rospy.get_param('~max_angle', max_angle)
cfg_srv.update_configuration({"max_angle": max_angle})
## PID params
if rospy.has_param('~use_imu_vel'):
use_odometry_vel = rospy.get_param('~use_imu_vel',
use_odometry_vel)
cfg_srv.update_configuration({"use_imu_vel": use_odometry_vel})
if rospy.has_param('~kP'):
kP = rospy.get_param('~kP', kP)
cfg_srv.update_configuration({"kP": kP})
return config
if __name__ == "__main__":
rospy.init_node("pid_coeff_publisher", anonymous=True)
rospy.loginfo("Start pid coefficient publisher")
pid_coeff_cmd_topic = rospy.get_param("~pid_coeff_cmd_topic")
default_pid_coeff_file = rospy.get_param("~default_pid_coeff_file",
"../config/PidCoeffDefault.yaml")
pub = rospy.Publisher(pid_coeff_cmd_topic, Float32MultiArray, queue_size=1)
srv = Server(PidCoeffConfig, callback)
try:
with open(default_pid_coeff_file, 'r') as f:
def_pid_coeff = yaml.load(f)
for item in coeff_names:
PidCoeffConfig.defaults[item] = def_pid_coeff[item]
except Exception as e:
rospy.logerr(
"Failed to setup default pid coefficients. Error : {}".format(e))
# Send once to initialize:
#print "!!!! PUBLISH DEFAULT VALUES : {}".format(PidCoeffConfig.defaults)
# !!! Probably, this does not work !!!
srv.update_configuration(PidCoeffConfig.defaults)
rospy.spin()
