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()