def __init__(self, name):
rospy.loginfo("[" + rospy.get_name() + "] " + "Starting ... ")
self.vis_marker_topic = rospy.get_param("~vis_marker_topic",
"~visualization_marker")
self.vel_costmap_topic = rospy.get_param("~vel_costmap_topic",
"/velocity_costmap")
self.origin_topic = rospy.get_param("~origin_topic", "origin")
self.base_link_tf = rospy.get_param("~base_link_tf", "base_link")
self.vis_pub = rospy.Publisher(self.vis_marker_topic,
Marker,
queue_size=1)
self.tf = TransformListener()
self.cc = CostmapCreator(
rospy.Publisher(self.vel_costmap_topic,
OccupancyGrid,
queue_size=10,
latch=True),
rospy.Publisher(self.origin_topic, 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)
]
self.ts = ApproximateTimeSynchronizer(fs=subs, queue_size=60, slop=0.1)
self.ts.registerCallback(self.callback)
rospy.loginfo("[" + rospy.get_name() + "] " + "... all done.")
def __init__(self):
FrontComms.__init__(self, RgbBuoyVision(comms=self))
self.defaultDepth = 1.50
# self.defaultDepth = 0.50 # For US house
self.depthFromMission = self.defaultDepth
self.dynServer = DynServer(Config, self.reconfigure)
def __init__(self):
self.image_pub = rospy.Publisher("/image_filtered", Image)
cv2.namedWindow("Image window", 1)
self.bridge = CvBridge()
self.image_sub = rospy.Subscriber("/image_raw", Image,
self.frontcam_callback)
self.dyn_reconf_server = DynServer(Config, self.reconfigure)
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 __init__(self, name):
rospy.loginfo("Starting %s" % name)
self.input = OnlineInput()
ppl_topic = rospy.get_param("~ppl_topic", "/people_tracker/positions")
robot_topic = rospy.get_param("~robot_topic", "/robot_pose")
robotSt_topic = rospy.get_param("~robotST_topic", "/robotST_pose")
goal_topic = rospy.get_param("~goal_topic", "/move_base/current_goal")
qtc_arr_topic = rospy.get_param("~qtc_arr_topic", "qtc_array")
self.target_frame = rospy.get_param("~target_frame", "/map")
self.processing_rate = rospy.get_param("~processing_rate", 30)
self.dyn_srv = DynServer(OnlineQTCCreatorConfig, self.dyn_callback)
self.listener = tf.TransformListener()
self.pub = rospy.Publisher(qtc_arr_topic, QTCArray, queue_size=10)
self.last_msg = QTCArray()
rospy.Subscriber(ppl_topic,
PeopleTracker,
callback=self.ppl_callback,
queue_size=10)
rospy.Subscriber(robot_topic,
Pose,
callback=self.pose_callback,
queue_size=10)
rospy.Subscriber(robotSt_topic,
PoseStamped,
callback=self.poseSt_callback,
queue_size=10)
rospy.Subscriber(goal_topic,
PoseStamped,
callback=self.goal_callback,
queue_size=10)
self.request_thread = thread.start_new(self.generate_qtc, ())
def __init__(self, taskMode='pickup'):
GenericComms.__init__(self, PickupVision(self))
self.curDepth = 0.0
self.taskMode = taskMode
if taskMode == 'pickup':
self.defaultDepth = 0.2
self.sinkingDepth = 2.6 #0.8 #1.9 #2.0 #robosub 8.9
self.grabbingDepth = 2.8 #0.9 #2.0 #3.2 #robosub 9
self.lastDepth = 3.8 #2.1 #2.9 #3.8 #robosub 9.9
self.grabbingArea = 20000
self.visionMode = PickupVision.SITE
elif taskMode == 'drop':
self.defaultDepth = 0.2 #1.2
self.sinkingDepth = 0.8 #1.5 #2.5
self.droppingDepth = 1.7
self.visionMode = PickupVision.BOX
self.calledCount = 0
self.depthSub = rospy.Subscriber("/depth", depth, self.depthCb)
self.maniPub = rospy.Publisher("/manipulators", manipulator)
self.dynServer = DynServer(Config, self.reconfigure)
if not self.isAlone:
self.initComms(taskMode)
def __init__(self):
FrontComms.__init__(self, TorpedoVision(comms=self))
self.defaultDepth = 2.5
self.depthFromMission = self.defaultDepth
self.sonarFilter = SonarFilter(comms=self)
self.dynServer = DynServer(Config, self.reconfigure)
self.depthSub = rospy.Subscriber('/depth', depth, self.depthCallback)
def __init__(self):
rospy.Subscriber('/WH_DVL_data', Odometry, self._ondvl)
rospy.Subscriber('/euler', compass_data, self._onimu)
self.epub = rospy.Publisher('/earth_odom', Odometry)
self.old_data = None
self.yaw = None
self.x = 0
self.y = 0
self.dynServer = DynServer(earth_odomConfig, self.reconfcb)
self.time = 0.0
def __init__(self):
self.dyn_reconfigure_server=DynServer(Config,self.reconfigure)
#image= np.zeros((320,512,3), np.uint8)
signal.signal(signal.SIGINT,self.userQuit)
self.imgData={'detected':False}
self.bridge=CvBridge()
self.camera_topic=rospy.get_param('~image', '/sedna/camera/front/image_raw')
self.image_filter_pub=rospy.Publisher("/Vision/image_filter",Image)
self.register()
self.previousCentroid=(-1,-1)
self.previousArea=0
def __init__(self):
GenericComms.__init__(self, BinsVision(self))
self.defaultDepth = 0.2 #0.2
self.aligningDepth = 1.4 #0.2 #0.8 #1.9 #robosub 9.4
self.sinkingDepth = 3.0 #2.0 #2.7 #robosub 9.6
self.search2Depth = 0.5 #0.2 #1.2 #robosub 8
self.turnDepth = 0.5 #0.8 #1.9 #robosub 8.2
self.dynServer = DynServer(Config, self.reconfigure)
self.maniPub = rospy.Publisher("/manipulators", manipulator)
if not self.isAlone:
self.initComms("bins")
def __init__(self, name):
rospy.loginfo("Starting %s", name)
self._action_name = name
self.fast = True
self.gaze_type = DynamicVelocityReconfigure.GAZE
rospy.loginfo("Creating dynamic reconfigure client")
self.client = dynamic_reconfigure.client.Client(
"/move_base/DWAPlannerROS")
rospy.loginfo(" ...done")
rospy.loginfo("Creating base movement client.")
self.baseClient = actionlib.SimpleActionClient(
'move_base', move_base_msgs.msg.MoveBaseAction)
self.baseClient.wait_for_server()
rospy.loginfo("Creating gaze client.")
self.gazeClient = actionlib.SimpleActionClient(
'gaze_at_pose', strands_gazing.msg.GazeAtPoseAction)
self.gazeClient.wait_for_server()
rospy.loginfo("...done")
rospy.loginfo("Reading move_base parameters")
self.getCurrentSettings()
rospy.loginfo("Reading parameters")
# Magic numbers overwritten in dyn_callback
self.threshold = 4.0
self.max_dist = 5.0
self.min_dist = 1.5
self.detection_angle = 90.0
self.dyn_srv = DynServer(HumanAwareNavigationConfig, self.dyn_callback)
current_time = rospy.get_time()
self.timeout = current_time + self.threshold
rospy.loginfo("Creating action server.")
self._as = actionlib.SimpleActionServer(
self._action_name,
move_base_msgs.msg.MoveBaseAction,
self.goalCallback,
auto_start=False)
#self._as.register_goal_callback()
#self._as.register_preempt_callback(self.preemptCallback)
rospy.loginfo(" ...starting")
self._as.start()
rospy.loginfo(" ...done")
self.sub_topic = rospy.get_param("~people_positions",
'/people_tracker/positions')
self.ppl_sub = None
self.last_cancel_time = rospy.Time(0)
rospy.Subscriber("/human_aware_navigation/cancel",
actionlib_msgs.msg.GoalID,
self.cancel_time_checker_cb)
def __init__(self):
FrontComms.__init__(self, PegsVision(comms=self))
self.defaultDepth = 2.0
self.dynServer = DynServer(Config, self.reconfigure)
# Initialise mission planner
if not self.isAlone:
self.comServer = rospy.Service("/pegs/mission_to_vision",
mission_to_vision, self.handle_srv)
rospy.loginfo("Waiting for mission planner")
self.toMission = rospy.ServiceProxy("/pegs/vision_to_mission",
vision_to_mission)
self.toMission.wait_for_service()
def __init__(self, name):
rospy.loginfo("Starting %s" % name)
self.client = None
action_list = rospy.get_param("han_action_dispatcher")["han_actions"]
self.default_action = rospy.get_param("~default_action")
self.dyn_srv = DynServer(HanActionDispatcherConfig, self.dyn_callback)
self.servers = {}
for a in action_list.items():
name = a[0]
self.servers[name] = SimpleActionServer(
name,
MoveBaseAction,
execute_cb=(lambda x: lambda msg: self.execute_cb(
msg, x[0], x[1]["action"]))(a),
auto_start=False)
self.servers[name].register_preempt_callback(self.preempt_cb)
self.servers[name].start()
rospy.loginfo("done")
def __init__(self, name):
rospy.loginfo("[" + rospy.get_name() + "] " + "Starting ... ")
# ................................................................
# read ros parameters
self.loadROSParams()
# ................................................................
# start ros subs/pubs/servs....
self.initROS()
# other params
qtc_rules = json.load(open(self.qtc_rules_file))
self.icc = IliadCostmapCreator(self.base_frame_id,
self.vel_costmap_topic_pub,
self.origin_topic_pub, qtc_rules)
self.dyn_srv = DynServer(VelocityCostmapsConfig, self.dyn_callback)
rospy.loginfo("[" + rospy.get_name() + "] " + "... all done.")
def __init__(self):
self.testing = rospy.get_param("~testing", False)
self.isAborted = False
self.isKilled = False
self.canPublish = False
signal.signal(signal.SIGINT, self.userQuit)
self.rectData = { 'detected' : False }
self.bridge = CvBridge()
#Initialize Subscribers and Publishers
self.image_topic = rospy.get_param('~image', '/bot_camera/camera/image_raw')
self.image_pub = rospy.Publisher("/Vision/image_filter", Image)
self.register()
# Setup dynamic reconfigure server
self.dyn_reconf_server = DynServer(Config, self.reconfigure)
#Initialize mission planner communication server and client
self.comServer = rospy.Service("/bucket/mission_to_vision", mission_to_vision, self.handleSrv)
if not self.testing:
self.isAborted = True
rospy.loginfo("Waiting for vision to mission service")
self.toMission = rospy.ServiceProxy("/bucket/vision_to_mission", vision_to_mission)
self.toMission.wait_for_service(timeout=60)
#Make sure locomotion server is up
try:
self.locomotionClient.wait_for_server(timeout=rospy.Duration(5))
except:
rospy.logerr("Locomotion server timeout!")
self.isKilled = True
#Initializing controller service
if self.testing:
self.canPublish = True
controllerServer = rospy.ServiceProxy("/set_controller_srv", set_controller)
controllerServer(forward=True, sidemove=True, heading=True, depth=True, pitch=True, roll=True,
topside=False, navigation=False)
#TODO: Add histogram modes for debug
rospy.loginfo("Bucket ready")
def __init__(self, name):
rospy.loginfo("Starting %s" % name)
self.input = OnlineInput()
ppl_topic = rospy.get_param("~ppl_topic", "/people_tracker/positions")
robot_topic = rospy.get_param("~robot_topic", "/robot_pose")
self.target_frame = rospy.get_param("~target_frame", "/map")
self.dyn_srv = DynServer(OnlineQTCCreatorConfig, self.dyn_callback)
self.listener = tf.TransformListener()
self.pub = rospy.Publisher("~qtc_array", QTCArray, queue_size=10)
rospy.Subscriber(
ppl_topic,
PeopleTracker,
callback=self.ppl_callback,
queue_size=1
)
rospy.Subscriber(
robot_topic,
Pose,
callback=self.pose_callback,
queue_size=1
)
def __init__(self):
self.dyn_reconfigure_server=DynServer(Config,self.reconfigure)
#image= np.zeros((320,512,3), np.uint8)
self.imgData={'detected':False}
self.bridge=CvBridge()
self.camera_topic=rospy.get_param('~image', '/sedna/bottom/image_raw')
self.image_filter_pub=rospy.Publisher("/vision/image_filter",Image)
self.threshOut_pub=rospy.Publisher("/vision/lineFollower/threshOut",Image)
self.register()
self.previousCentroid=(-1,-1)
self.previousArea=0
def __init__(self, isAcoustic):
GenericComms.__init__(self, LaneMarkerVision(comms=self))
self.chosenLane = self.RIGHT
self.expectedLanes = 1
self.dynServer = DynServer(Config, self.reconfigure)
self.isAcoustic = isAcoustic
if self.isAcoustic:
self.visionFilter.isAcoustic = True
self.detectingBox = True
self.defaultDepth = 0.2
self.laneSearchDepth = 0.2
else:
self.visionFilter.isAcoustic = False
self.detectingBox = False
self.defaultDepth = 0.2
if not self.isAlone:
if self.isAcoustic:
self.initComms("laneAcoustic")
else:
self.initComms("lane")
def __init__(self, name):
rospy.loginfo("Starting {}.".format(name))
self.functions = {
"none": lambda y, _: y,
"power": self.reduce_noise_power,
"centroid_s": self.reduce_noise_centroid_s,
"centroid_mb": self.reduce_noise_centroid_mb,
"mfcc_down": self.reduce_noise_mfcc_down,
"mfcc_up": self.reduce_noise_mfcc_up,
"median": self.reduce_noise_median
}
self.dyn_srv = DynServer(NoiseFilterConfig, self.dyn_callback)
self.pub = rospy.Publisher("~result", AudioBuffer, queue_size=1)
# with open('/home/cd32/noise.yaml', 'r') as f:
# self.noise = np.array(yaml.load(f)).reshape(-1, 4)
# rospy.Subscriber("/mummer_ds_beamforming/result", AudioBuffer, self.callback)
rospy.Subscriber(
rospy.get_param("~topic", "mummer_ds_beamforming/result"),
AudioBuffer, self.callback)
# rospy.Subscriber("/naoqi_driver_node/audio", AudioBuffer, self.callback)
# print self.noise.shape
rospy.loginfo("done")
def __init__(self, name):
rospy.loginfo("Creating " + name + " ...")
self.simulator = rospy.get_param("~simulator", True)
if self.simulator:
rospy.loginfo("Just testing")
else:
rospy.loginfo("The games are over. This counts!")
self.dyn_srv = DynServer(SpotterConfig, self.dyn_callback)
self.bridge = CvBridge()
self.pub = rospy.Publisher("/predator_spotter/spotted",
Bool,
queue_size=10)
self.img_pub = rospy.Publisher("/predator_spotter/image",
Image,
queue_size=1)
rospy.Subscriber(
"/turtlebot_2/camera/rgb/image_raw" \
if self.simulator else \
"/camera/rgb/image_color",
Image,
self.callback,
queue_size=1
)
def __init__(self, name):
rospy.loginfo("Starting %s", name)
self.dynServer_name = rospy.get_param("~DWAPlannerROS_srv",
'/move_base/DWAPlannerROS')
self.moveBase_ac_name = rospy.get_param("~moveBase_ac", 'move_base')
self.simpleGoalTopic = rospy.get_param("~simple_goal_topic",
'/move_base_simple/goal')
self.cancel_topic = rospy.get_name() + '/cancel'
self.useGaze = rospy.get_param("~useGaze", True)
self._action_name = name
self.fast = True
self.gaze_type = DynamicVelocityReconfigure.GAZE
rospy.loginfo("Creating dynamic reconfigure client to server: " +
self.dynServer_name)
self.client = DynClient(self.dynServer_name)
rospy.loginfo(" ...done")
rospy.loginfo("Creating base movement client to server: " +
self.moveBase_ac_name)
self.baseClient = actionlib.SimpleActionClient(
self.moveBase_ac_name, move_base_msgs.msg.MoveBaseAction)
self.baseClient.wait_for_server()
rospy.loginfo(" ...done")
if self.useGaze:
rospy.loginfo("Creating gaze client.")
self.gazeClient = actionlib.SimpleActionClient(
'gaze_at_pose', strands_gazing.msg.GazeAtPoseAction)
self.gazeClient.wait_for_server()
rospy.loginfo("...done")
# Magic numbers overwritten in dyn_callback
self.threshold = 4.0
self.max_dist = 5.0
self.min_dist = 1.5
self.detection_angle = 90.0
self.dyn_srv = DynServer(HumanAwareNavigationConfig, self.dyn_callback)
current_time = rospy.get_time()
self.timeout = current_time + self.threshold
rospy.loginfo("Creating Human Aware Goal Action Server.")
self._as = actionlib.SimpleActionServer(
self._action_name,
move_base_msgs.msg.MoveBaseAction,
self.goalCallback,
auto_start=False)
rospy.loginfo(" ...starting Human Aware Goal Action Server")
self._as.start()
rospy.loginfo(" Human Aware Goal Action Server active")
self.sub_topic = rospy.get_param("~people_positions",
'/people_tracker/positions')
self.ppl_sub = None
self.last_cancel_time = rospy.Time(0)
rospy.loginfo(" Subscribing to GoalCancel messages at: " +
self.cancel_topic)
rospy.Subscriber(self.cancel_topic, actionlib_msgs.msg.GoalID,
self.cancel_time_checker_cb)
rospy.loginfo(" Creating simpleGoal interface at : " +
self.simpleGoalTopic)
self.ppl_sub = rospy.Subscriber(self.simpleGoalTopic, PoseStamped,
self.simpleGoalCallback, None, 1)
# yes, a client to myself...
self._as_client = actionlib.SimpleActionClient(
self._action_name, move_base_msgs.msg.MoveBaseAction)
self._as_client.wait_for_server()
rospy.loginfo(" Finished Start-up")