class WrenchRot(object):
def __init__(self):
self.target_frame = rospy.get_param("~target_frame", "/ee_link")
self.tf_list = TransformListener()
self.ws_sub = rospy.Subscriber("old_wrench",
WrenchStamped,
self.trans_wrench_cb,
queue_size=1)
self.ws_pub = rospy.Publisher("new_wrench", WrenchStamped)
def trans_wrench_cb(self, ws_old):
ws_new = WrenchStamped()
ws_new.header = copy.deepcopy(ws_old.header)
ws_new.header.frame_id = self.target_frame
vec3_stamped = Vector3Stamped()
vec3_stamped.header = ws_old.header
try:
self.tf_list.waitForTransform(self.target_frame,
ws_old.header.frame_id,
ws_old.header.stamp,
rospy.Duration(1.))
except Exception as e:
print e
rospy.logwarn(
"Timeout waiting for transform from %s to target frame %s" %
(ws_old.header.frame_id, self.target_frame))
return
vec3_stamped.vector = ws_old.wrench.force
ws_new.wrench.force = self.tf_list.transformVector3(
self.target_frame, vec3_stamped).vector
vec3_stamped.vector = ws_old.wrench.torque
ws_new.wrench.torque = self.tf_list.transformVector3(
self.target_frame, vec3_stamped).vector
self.ws_pub.publish(ws_new)
class ColorTrackerROS(object):
def __init__(self):
"""
Initialize Color Tracking ROS interface.
"""
rospy.init_node('color_tracker')
self.tracker = ColorTracker()
self.cv_image = self.processed_image = None # the latest image from the camera
self.boxes = []
self.bridge = CvBridge() # used to convert ROS messages to OpenCV
self.cameraModel = PinholeCameraModel()
self.tf = TransformListener(cache_time=rospy.Duration.from_sec(20))
# parameters ...
self._gui = bool(rospy.get_param('~gui', default=False)) # set to _gui:=true for debug display
self._rate = float(rospy.get_param('~rate', default=50.0)) # processing rate
self._par = float(rospy.get_param('~plate_area', default=0.0338709))
# publishers ...
self.debug_pub = rospy.Publisher("tracker/debug", PoseWithCovarianceStamped, queue_size=10)
self.roomba_pub = rospy.Publisher("visible_roombas", RoombaList, queue_size = 10)
rospy.loginfo("Initializing Color Tracker")
if self._gui:
cv2.namedWindow('preview_window')
cv2.namedWindow('binary')
# start listening ...
rospy.Subscriber("/ardrone/bottom/image_raw", Image, self.process_image)
rospy.Subscriber("/ardrone/bottom/camera_info", CameraInfo, self.on_camera_info)
def process_image(self, msg):
"""
Process image messages from ROS and stash them in an attribute
called cv_image for subsequent processing
"""
try:
self.cv_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding="bgr8")
# get drone position ...
# proper tf below ... hacking for now
self.tf.waitForTransform('map', msg.header.frame_id,
msg.header.stamp, rospy.Duration(0.1))
pos, _ = self.tf.lookupTransform('map', msg.header.frame_id, msg.header.stamp)
# area scale + tolerance
xy2uv = self.cameraModel.getDeltaU(1.0, pos[2]) * self.cameraModel.getDeltaV(1.0, pos[2])
min_area = 0.75 * self._par * xy2uv
# do image processing here
self.boxes, self.processed_image = self.tracker.find_bounding_boxes(self.cv_image, min_area)
listOfRoombas = []
for box in self.boxes:
center = np.mean(box, axis=0)
heading, covarianceOfHeading = get_heading(box, center, self.processed_image)
ray = self.cameraModel.projectPixelTo3dRay(center)
camera_ray = Vector3Stamped(header=msg.header,
vector=Vector3(*ray))
print('camera_ray', camera_ray)
drone_ray = self.tf.transformVector3('base_link', camera_ray)
#print('drone_ray', drone_ray)
# get drone height for ground-plane projection
#multiplier = -pos[2] / drone_ray.vector.z
multiplier = pos[2] / camera_ray.vector.z
camera_to_roomba = np.array([camera_ray.vector.x, camera_ray.vector.y, camera_ray.vector.z]) * multiplier
quat = quaternion_from_euler(0,0,heading)
pose = Pose(position=Point(*camera_to_roomba), orientation = Quaternion(*quat))
pwcs = PoseWithCovarianceStamped(header=Header(frame_id=msg.header.frame_id, stamp=msg.header.stamp),
pose=PoseWithCovariance(
pose=pose,
covariance=np.diag([.2, .2, 0, 0, 0, covarianceOfHeading]).flatten()
))
rb = Roomba(last_seen=msg.header.stamp, frame_id=msg.header.frame_id, type=Roomba.RED,
visible_location=pwcs)
listOfRoombas.append(rb)
self.debug_pub.publish(pwcs)
self.roomba_pub.publish(header=Header(frame_id=msg.header.frame_id,stamp=msg.header.stamp),data=listOfRoombas)
rospy.loginfo_throttle(1.0, 'Boxes : {}'.format(self.boxes))
# TODO: Do something with the boxes
# TODO: make sure it doesn't get too far behind
# self.binary_image = binary_image
# self.cv_image = cv_image
except CvBridgeError as e:
rospy.loginfo_throttle(0.5, "Error loading image : {}".format(e))
def on_camera_info(self, msg):
self.cameraModel.fromCameraInfo(msg)
def run(self):
""" The main run loop, in this node it doesn't do anything """
r = rospy.Rate(self._rate)
while not rospy.is_shutdown():
# start out not issuing any motor commands
if not self.cv_image is None and self._gui:
try:
cv2.imshow('preview_window', self.cv_image)
cv2.imshow('binary', self.processed_image)
cv2.waitKey(5)
except Exception as e:
pass
r.sleep()
class ThrusterController():
CONTROLS_MOVE_TOPIC = '/control_effort'
CONTROLS_MOVE_X_TOPIC = CONTROLS_MOVE_TOPIC + '/x'
CONTROLS_MOVE_Y_TOPIC = CONTROLS_MOVE_TOPIC + '/y'
CONTROLS_MOVE_Z_TOPIC = CONTROLS_MOVE_TOPIC + '/z'
CONTROLS_MOVE_ROLL_TOPIC = CONTROLS_MOVE_TOPIC + '/roll'
CONTROLS_MOVE_PITCH_TOPIC = CONTROLS_MOVE_TOPIC + '/pitch'
CONTROLS_MOVE_YAW_TOPIC = CONTROLS_MOVE_TOPIC + '/yaw'
CONTROLS_POWER_X_TOPIC = '/controls/power/x'
CONTROLS_POWER_Y_TOPIC = '/controls/power/y'
CONTROLS_POWER_Z_TOPIC = '/controls/power/z'
CONTROLS_POWER_ROLL_TOPIC = '/controls/power/roll'
CONTROLS_POWER_PITCH_TOPIC = '/controls/power/pitch'
CONTROLS_POWER_YAW_TOPIC = '/controls/power/yaw'
SIM_PUB_TOPIC = '/sim/move'
ROBOT_PUB_TOPIC = '/offboard/thruster_speeds'
enabled = False
def __init__(self):
rospy.init_node('thruster_controls')
self.sim = rospy.get_param('~/thruster_controls/sim')
if self.sim == False:
self.pub = rospy.Publisher(self.ROBOT_PUB_TOPIC, ThrusterSpeeds, queue_size=3)
elif self.sim == True:
self.pub = rospy.Publisher(self.SIM_PUB_TOPIC, Float32MultiArray, queue_size=3)
else:
# TODO: alert that unrecognized mode destination has been set
pass
self.enable_service = rospy.Service('enable_controls', SetBool, self.handle_enable_controls)
self.tm = ThrusterManager(os.path.join(sys.path[0], '../config/cthulhu.config'))
self.listener = TransformListener()
rospy.Subscriber(self.CONTROLS_MOVE_X_TOPIC, Float64, self._on_x)
rospy.Subscriber(self.CONTROLS_MOVE_Y_TOPIC, Float64, self._on_y)
rospy.Subscriber(self.CONTROLS_MOVE_Z_TOPIC, Float64, self._on_z)
rospy.Subscriber(self.CONTROLS_MOVE_ROLL_TOPIC, Float64, self._on_roll)
rospy.Subscriber(self.CONTROLS_MOVE_PITCH_TOPIC, Float64, self._on_pitch)
rospy.Subscriber(self.CONTROLS_MOVE_YAW_TOPIC, Float64, self._on_yaw)
rospy.Subscriber(self.CONTROLS_POWER_X_TOPIC, Float64, self._on_x_power)
rospy.Subscriber(self.CONTROLS_POWER_Y_TOPIC, Float64, self._on_y_power)
rospy.Subscriber(self.CONTROLS_POWER_Z_TOPIC, Float64, self._on_z_power)
rospy.Subscriber(self.CONTROLS_POWER_ROLL_TOPIC, Float64, self._on_roll_power)
rospy.Subscriber(self.CONTROLS_POWER_PITCH_TOPIC, Float64, self._on_pitch_power)
rospy.Subscriber(self.CONTROLS_POWER_YAW_TOPIC, Float64, self._on_yaw_power)
self.pid_outputs = np.zeros(6)
self.pid_outputs_local = np.zeros(6)
self.powers = np.zeros(6)
self.t_allocs = np.zeros(8)
def handle_enable_controls(self, req):
self.enabled = req.data
return {'success': True, 'message': 'Successfully set enabled to ' + str(req.data)}
def transform_twist(self, base_frame, target_frame, twist):
lin = Vector3Stamped()
ang = Vector3Stamped()
lin.header.frame_id = base_frame
ang.header.frame_id = base_frame
lin.vector.x = twist[0]
lin.vector.y = twist[1]
lin.vector.z = twist[2]
ang.vector.x = twist[3]
ang.vector.y = twist[4]
ang.vector.z = twist[5]
lin_local = self.listener.transformVector3(target_frame, lin)
ang_local = self.listener.transformVector3(target_frame, ang)
return np.array([lin_local.vector.x,
lin_local.vector.y,
lin_local.vector.z,
ang_local.vector.x,
ang_local.vector.y,
ang_local.vector.z])
def update_thruster_allocs(self):
self.pid_outputs_local = self.transform_twist('odom', 'base_link', self.pid_outputs)
#rospy.loginfo(pid_outputs_local)
for i in range(len(self.powers)):
if(self.powers[i]!=0):
self.pid_outputs_local[i]=self.powers[i]
self.t_allocs = self.tm.calc_t_allocs(self.pid_outputs_local)
def _on_x(self, x):
self.pid_outputs[0] = x.data
self.update_thruster_allocs()
def _on_y(self, y):
self.pid_outputs[1] = y.data
self.update_thruster_allocs()
def _on_z(self, z):
self.pid_outputs[2] = z.data
self.update_thruster_allocs()
def _on_roll(self, roll):
self.pid_outputs[3] = roll.data
self.update_thruster_allocs()
def _on_pitch(self, pitch):
self.pid_outputs[4] = pitch.data
self.update_thruster_allocs()
def _on_yaw(self, yaw):
self.pid_outputs[5] = yaw.data
self.update_thruster_allocs()
def _on_x_power(self, x):
self.powers[0] = x.data
self.update_thruster_allocs()
def _on_y_power(self, y):
self.powers[1] = y.data
self.update_thruster_allocs()
def _on_z_power(self, z):
self.powers[2] = z.data
self.update_thruster_allocs()
def _on_roll_power(self, roll):
self.powers[3] = roll.data
self.update_thruster_allocs()
def _on_pitch_power(self, pitch):
self.powers[4] = pitch.data
self.update_thruster_allocs()
def _on_yaw_power(self, yaw):
self.powers[5] = yaw.data
self.update_thruster_allocs()
def run(self):
rate = rospy.Rate(10) # 10 Hz
while not rospy.is_shutdown():
if not self.enabled:
# If not enabled, publish all 0s.
if self.sim == False:
i8_t_allocs = ThrusterSpeeds()
i8_t_allocs.speeds = np.zeros(8)
self.pub.publish(i8_t_allocs)
elif self.sim == True:
f32_t_allocs = Float32MultiArray()
f32_t_allocs.data = np.zeros(8)
self.pub.publish(f32_t_allocs)
if self.enabled:
# Scale thruster alloc max to PID max
t_alloc_max = float(np.max(np.absolute(self.t_allocs)))
pid_max = float(np.max(np.absolute(self.pid_outputs_local)))
if(t_alloc_max != 0):
# Multiply each thruster allocation by scaling ratio
self.t_allocs *= pid_max / t_alloc_max
if self.sim == False:
i8_t_allocs = ThrusterSpeeds()
i8_t_allocs.speeds = (self.t_allocs * 127).astype(int)
self.pub.publish(i8_t_allocs)
elif self.sim == True:
f32_t_allocs = Float32MultiArray()
f32_t_allocs.data = self.t_allocs
self.pub.publish(f32_t_allocs)
rate.sleep()
class VelocityCostmapServer(object):
def __init__(self, name):
rospy.loginfo("Starting %s..." % name)
self.vis_pub = rospy.Publisher("~visualization_marker",
Marker,
queue_size=1)
self.tf = TransformListener()
self.cc = CostmapCreator(
rospy.Publisher("/velocity_costmap",
OccupancyGrid,
queue_size=10,
latch=True),
rospy.Publisher("~origin", PoseStamped, queue_size=10))
self.dyn_srv = DynServer(VelocityCostmapsConfig, self.dyn_callback)
subs = [
Subscriber(
rospy.get_param("~qtc_topic",
"/qtc_state_predictor/prediction_array"),
QTCPredictionArray),
Subscriber(
rospy.get_param("~ppl_topic", "/people_tracker/positions"),
PeopleTracker)
]
ts = TimeSynchronizer(fs=subs, queue_size=60)
ts.registerCallback(self.callback)
rospy.loginfo("... all done.")
def dyn_callback(self, config, level):
self.cc.resolution = config["costmap_resolution"]
self.cc.min_costs = config["min_costs"]
self.cc.max_costs = config["max_costs"]
return config
def callback(self, qtc, ppl):
vels = []
try:
t = self.tf.getLatestCommonTime("base_link", ppl.header.frame_id)
vs = Vector3Stamped(header=ppl.header)
vs.header.stamp = t
for v in ppl.velocities:
vs.vector = v
vels.append(self.tf.transformVector3("base_link", vs).vector)
except Exception as e:
rospy.logwarn(e)
return
data_buffer = {
e.uuid: {
"qtc": json.loads(e.qtc_serialised),
"angle": ppl.angles[ppl.uuids.index(e.uuid)],
"velocity": vels[ppl.uuids.index(e.uuid)]
}
for e in qtc.qtc
}
try:
element = data_buffer[ppl.uuids[ppl.distances.index(
ppl.min_distance)]] # Only taking the closest person for now
self.publish_closest_person_marker(
ppl.poses[ppl.distances.index(ppl.min_distance)],
ppl.header.frame_id)
except KeyError:
return
qtc = element["qtc"].split(',')
self.cc.publish(angle=element["angle"],
qtc_symbol=','.join([qtc[0]] if len(qtc) ==
2 else [qtc[0], qtc[2]]),
velocity=element["velocity"])
def publish_closest_person_marker(self, pose, frame_id):
if self.vis_pub.get_num_connections() > 0:
m = Marker()
m.header.stamp = rospy.Time.now()
m.header.frame_id = frame_id
m.ns = "velocity_costmaps"
m.id = 0
m.type = m.SPHERE
m.action = m.MODIFY
m.pose = pose
m.pose.position.z = 2.0
m.scale.x = .25
m.scale.y = .25
m.scale.z = .25
m.color.a = 1.0
m.color.r = 0.0
m.color.g = 1.0
m.color.b = 0.0
m.lifetime = rospy.Duration(1.)
self.vis_pub.publish(m)
class ThrusterController:
SIM_PUB_TOPIC = '/sim/move'
ROBOT_PUB_TOPIC = '/offboard/thruster_speeds'
enabled = False
def __init__(self):
rospy.init_node('thruster_controls')
self.sim = rospy.get_param('~/thruster_controls/sim')
if not self.sim:
self.pub = rospy.Publisher(self.ROBOT_PUB_TOPIC,
ThrusterSpeeds,
queue_size=3)
else:
self.pub = rospy.Publisher(self.SIM_PUB_TOPIC,
Float32MultiArray,
queue_size=3)
self.enable_service = rospy.Service('enable_controls', SetBool,
self.handle_enable_controls)
self.tm = ThrusterManager(
os.path.join(sys.path[0], '../config/cthulhu.config'))
self.listener = TransformListener()
for d in utils.get_axes():
rospy.Subscriber(utils.get_controls_move_topic(d), Float64,
self._on_pid_received, d)
rospy.Subscriber(utils.get_power_topic(d), Float64,
self._on_power_received, d)
self.pid_outputs = np.zeros(6)
self.pid_outputs_local = np.zeros(6)
self.powers = np.zeros(6)
self.t_allocs = np.zeros(8)
def handle_enable_controls(self, req):
self.enabled = req.data
return {
'success': True,
'message': 'Successfully set enabled to ' + str(req.data)
}
def transform_twist(self, base_frame, target_frame, twist):
lin = Vector3Stamped()
ang = Vector3Stamped()
lin.header.frame_id = base_frame
ang.header.frame_id = base_frame
lin.vector.x = twist[0]
lin.vector.y = twist[1]
lin.vector.z = twist[2]
ang.vector.x = twist[3]
ang.vector.y = twist[4]
ang.vector.z = twist[5]
lin_local = self.listener.transformVector3(target_frame, lin)
ang_local = self.listener.transformVector3(target_frame, ang)
return np.array([
lin_local.vector.x, lin_local.vector.y, lin_local.vector.z,
ang_local.vector.x, ang_local.vector.y, ang_local.vector.z
])
def update_thruster_allocs(self):
if self.enabled:
self.pid_outputs_local = self.transform_twist(
'odom', 'base_link', self.pid_outputs)
for i in range(len(self.powers)):
if self.powers[i] != 0:
self.pid_outputs_local[i] = self.powers[i]
self.t_allocs = self.tm.calc_t_allocs(self.pid_outputs_local)
def _on_pid_received(self, val, direction):
self.pid_outputs[utils.get_axes().index(direction)] = val.data
self.update_thruster_allocs()
def _on_power_received(self, val, direction):
self.powers[utils.get_axes().index(direction)] = val.data
self.update_thruster_allocs()
def run(self):
rate = rospy.Rate(10) # 10 Hz
while not rospy.is_shutdown():
if not self.enabled:
# If not enabled, publish all 0s.
if not self.sim:
i8_t_allocs = ThrusterSpeeds()
i8_t_allocs.speeds = np.zeros(8)
self.pub.publish(i8_t_allocs)
else:
f32_t_allocs = Float32MultiArray()
f32_t_allocs.data = np.zeros(8)
self.pub.publish(f32_t_allocs)
if self.enabled:
# Scale thruster alloc max to PID max
t_alloc_max = float(np.max(np.absolute(self.t_allocs)))
pid_max = float(np.max(np.absolute(self.pid_outputs_local)))
if t_alloc_max != 0:
# Multiply each thruster allocation by scaling ratio
self.t_allocs *= pid_max / t_alloc_max
# Clamp values of t_allocs to between -1 to 1
self.t_allocs = np.clip(self.t_allocs, -1, 1)
if not self.sim:
i8_t_allocs = ThrusterSpeeds()
i8_t_allocs.speeds = (self.t_allocs * 127).astype(int)
self.pub.publish(i8_t_allocs)
else:
f32_t_allocs = Float32MultiArray()
f32_t_allocs.data = self.t_allocs
self.pub.publish(f32_t_allocs)
rate.sleep()
