def __logfatal(self,msg):
"""Used for logging fatal error messages
Parameters
msg - string to log
"""
rospy.logfatal(self.log_prefix + msg)
def connect(self):
try:
self.dxl_io = dynamixel_io.DynamixelIO(self.port_name, self.baud_rate, self.readback_echo)
self.__find_motors()
except dynamixel_io.SerialOpenError, e:
rospy.logfatal(e.message)
sys.exit(1)
def loadPluginInstances(self, plugins, manager):
"""
plugins := {name: spec, name: spec, ...}
spec := {class: class, args: args}
"""
self.plugins = []
#for (instance_name, instance_spec) in plugins.items():
plugin_keys = plugins.keys()
plugin_keys.sort()
for instance_name in plugin_keys:
instance_spec = plugins[instance_name]
instance_class = instance_spec["class"]
instance_args = instance_spec["args"]
if not self.plugin_defs.has_key(instance_class):
rospy.logerr('cannot find %s in plugins for %s' % (instance_class,
self.package_name))
else:
try:
module_path = self.plugin_defs[instance_class]
module_name, class_from_class_type = module_path.rsplit('.', 1)
module = __builtin__.__import__(module_name,
fromlist=[class_from_class_type],
level=0)
class_ref = getattr(module, class_from_class_type, None)
if class_ref is None:
rospy.logfatal('cannot find %s' % (class_from_class_type))
else:
self.plugins.append(class_ref(instance_name, instance_args))
self.plugins[-1].registerManager(manager)
except:
rospy.logerr('failed to load %s' % (instance_class))
traceback.print_exc(file=sys.stdout)
return self.plugins
def callback(self, notification):
if 'log' in notification.destination:
if notification.level == 0:
rospy.loginfo(notification.text)
elif notification.level == 1:
rospy.logwarn(notification.text)
elif notification.level == 2:
rospy.logfatal(notification.text)
else:
rospy.logdebug(notification.text)
if 'voice' in notification.destination:
#todo check current voice notification level
# print notification.text
if not is_interval(self.silence_interval, datetime.now().time()) or notification.level == 2:
self.pub.publish(String(notification.text))
if 'mail' in notification.destination:
self.send_mail(notification)
if 'sms' in notification.destination:
#todo
pass
if 'display' in notification.destination:
#todo
pass
if 'audio' in notification.destination:
#todo
pass
def __init__(self):
# ROS initialization:
rospy.init_node('pose_manager')
self.poseLibrary = dict()
self.readInPoses()
self.poseServer = actionlib.SimpleActionServer("body_pose", BodyPoseAction,
execute_cb=self.executeBodyPose,
auto_start=False)
self.trajectoryClient = actionlib.SimpleActionClient("joint_trajectory", JointTrajectoryAction)
if self.trajectoryClient.wait_for_server(rospy.Duration(3.0)):
try:
rospy.wait_for_service("stop_walk_srv", timeout=2.0)
self.stopWalkSrv = rospy.ServiceProxy("stop_walk_srv", Empty)
except:
rospy.logwarn("stop_walk_srv not available, pose_manager will not stop the walker before executing a trajectory. "
+"This is normal if there is no nao_walker running.")
self.stopWalkSrv = None
self.poseServer.start()
rospy.loginfo("pose_manager running, offering poses: %s", self.poseLibrary.keys());
else:
rospy.logfatal("Could not connect to required \"joint_trajectory\" action server, is the nao_controller node running?");
rospy.signal_shutdown("Required component missing");
def main():
rospy.loginfo("Starting up...")
# command line args
mapfile = None
runmap = False
args = rospy.myargv()[1:]
while len(args) > 0:
opt = args.pop(0)
if opt == "-r":
runmap = True
else:
mapfile = opt
if not mapfile:
rospy.logfatal("Missing param 'mapfile'")
exit(1)
if runmap:
rospy.loginfo("Running file %s" % mapfile)
runner = MapRunner(mapfile)
runner.run()
else:
mapper = MapBuilder(mapfile)
rospy.Subscriber(SERVO_TOPIC, ServoCommand, mapper.on_servo_command)
rospy.Subscriber(STATE_TOPIC, FuriousState, mapper.on_state_update)
rospy.init_node(NODE_NAME)
rospy.loginfo("Ready.")
while not rospy.is_shutdown():
rospy.spin()
mapper.shutdown()
print "Shutting down..."
def run(self):
if self.group1.get_active_joints() != self.group2.get_active_joints():
rospy.logfatal(self.group1.get_name() + " and " + self.group2.get_name() + " do not represent the same group")
rospy.loginfo("Running IK reachability tests between " + self.group1.get_name() + " and " + self.group2.get_name())
while not rospy.is_shutdown():
self.test(self.group1.get_random_joint_values())
def evaulate_state(self):
# Check if we have data to evaluate state or not
if len(self.base_scan.ranges) is 0:
return
# We have data so we can evaluate state! Woo.
if self.state is RobotStates.MoveForward:
if self.is_blocked():
rospy.logdebug("Detected collision!")
self.state = RobotStates.Stop
self.full_stop()
else:
rospy.logdebug("Moving forward")
self.go_forward(self.get_new_velocity())
elif self.state is RobotStates.Stop:
rospy.logdebug("Stopping")
self.rotate(random.choice([-1, 1])*self.turn)
self.state = RobotStates.Rotate
elif self.state is RobotStates.Rotate:
if self.is_blocked():
pass
else:
self.state = RobotStates.MoveForward
self.full_stop()
rospy.logdebug("Rotating")
else:
# This should never happen. Panic and exit.
rospy.logfatal("Simplebot controller state is undefined. " +
"Are new states added but not managed? " +
"Killing the controller.")
self.shutdown()
rospy.shutdown()
def __init__(self):
self.seq = 0
self.frame_id = rospy.get_param('~frame_id')
if (rospy.has_param('~positions') or
rospy.has_param('~rotations') or
rospy.has_param('~dimensions')):
# Deprecated bounding box pose/dimension specification
rospy.logwarn("DEPRECATION WARNING: Rosparam '~positions', "
"'~rotations' and '~dimensions' are being "
"deprecated. Please use '~boxes' instead.")
positions = rospy.get_param('~positions')
rotations = rospy.get_param('~rotations')
dimensions = rospy.get_param('~dimensions')
if len(rotations) != len(positions):
rospy.logfatal('Number of ~rotations is expected as {}, but {}'
.format(len(positions), len(rotations)))
sys.exit(1)
if len(dimensions) != len(positions):
rospy.logfatal('Number of ~dimensions is expected as {}, but {}'
.format(len(positions), len(dimensions)))
sys.exit(1)
self.boxes = []
for pos, rot, dim in zip(positions, rotations, dimensions):
self.boxes.append({
'position': pos,
'rotation': rot,
'dimension': dim,
})
else:
self.boxes = rospy.get_param('~boxes')
self.pub = rospy.Publisher('~output', BoundingBoxArray, queue_size=1)
rate = rospy.get_param('~rate', 1)
self.timer = rospy.Timer(rospy.Duration(1. / rate), self.publish)
def test_log(self):
# we can't do anything fancy here like capture stdout as rospy
# grabs the streams before we do. Instead, just make sure the
# routines don't crash.
real_stdout = sys.stdout
real_stderr = sys.stderr
try:
from cStringIO import StringIO
sys.stdout = StringIO()
sys.stderr = StringIO()
import rospy
rospy.loginfo("test 1")
self.assert_("test 1" in sys.stdout.getvalue())
rospy.logwarn("test 2")
self.assert_("[WARN]" in sys.stderr.getvalue())
self.assert_("test 2" in sys.stderr.getvalue())
sys.stderr = StringIO()
rospy.logerr("test 3")
self.assert_("[ERROR]" in sys.stderr.getvalue())
self.assert_("test 3" in sys.stderr.getvalue())
sys.stderr = StringIO()
rospy.logfatal("test 4")
self.assert_("[FATAL]" in sys.stderr.getvalue())
self.assert_("test 4" in sys.stderr.getvalue())
finally:
sys.stdout = real_stdout
sys.stderr = real_stderr
def leader_check_for_chessboard(self, img_msg, cam):
# Convert img message to opencv img:
try:
cv_image = self.bridge.imgmsg_to_cv2(img_msg)
if cam == 2:
# Store cam img dimensions:
if self.cam2_size is None:
self.cam2_size = cv_image.shape[0:2]
elif self.cam1_size is None:
self.cam1_size = cv_image.shape[0:2]
except CvBridgeError as e:
rospy.logfatal("CV_Bridge failed to convert message from %s!"
% self.leader_topic_name)
return
# Check for chessboard calib pattern:
scrib, corners, downsampled_corners, scales = self.downsample_and_detect(cv_image)
# If chessboard found, save frame + chessboard data
# and set flag for follower's handler to find a corresponding frame:
if corners is not None:
print "LEADER found chessboard!"
print "Starting search for corresponding follower frame..."
self.leader_calib_frame_img = cv_image
self.leader_calib_frame_time = img_msg.header.stamp
self.leader_calib_frame_chessboard = corners
#cv2.drawChessboardCorners(scrib,
# (self.N_BOARD_ROWS, self.N_BOARD_COLS),
# downsampled_corners, True)
self.need_follower_calib_frame = True
def __init__(self):
#########################################################################
cur_val = 0
self.rolling_ave = 0.0
self.rolling_std = 0.0
self.rolling_meters = 0.0
rospy.init_node("range_filter")
rospy.loginfo("-I- range_filter started")
self.readParameters()
# check for valid parameters
# zero cannot be raised to a negative power
if self.min_valid == 0 and self.m < 0:
rospy.logerr("-E- range_filter: cannot have a min_valid of 0 with a negative exponent. Forcing min_valid to 1e-10")
min_valid = 1e-10
# negative number cannot be raised to a fractional power.
if self.min_valid < 0 and round(self.m) != self.m:
rospy.logerr("-E- range_filter: cannot have a negative min_valid with a fractional exponent. Forcing min_valid to 0")
self.min_valid = 0
if self.max_valid < self.min_valid:
rospy.logfatal("-E- range_filter: max_valid (%0.3f) cannot be less than min_vaid (%0.3f)" % (self.max_valid, self.min_valid))
return(-1)
self.prev = [0] * self.rolling_pts
rospy.Subscriber("range_int", Analog, self.inputCallback)
self.filtered_pub = rospy.Publisher("range_filtered", Float32)
self.std_pub = rospy.Publisher("range_std", Float32)
self.range_pub = rospy.Publisher("range", Range)
self.scan_pub = rospy.Publisher("scan", LaserScan)
def handle_dummy_node_action(self, req):
action = req.action
result = True
if action == "log debug":
rospy.logdebug("log debug")
elif action == "log info":
rospy.loginfo("log info")
elif action == "log warn":
rospy.logwarn("log warn")
elif action == "log err":
rospy.logerr("log err")
elif action == "log fatal":
rospy.logfatal("log fatal")
elif action == "init publisher":
self.pub = rospy.Publisher('test_topic', String, queue_size=10)
self.periodic_publish()
elif action == "init service":
self.start_dummy_service()
elif action == "shutdown":
self.shutdown = True
else:
result = False
print("unknown action")
print("handle node action called")
return DummyNodeActionResponse(result)
def start(self):
if len(self.plugin_instances) == 0:
rospy.logfatal('no valid plugins are loaded')
return
self.current_plugin = self.plugin_instances[0]
self.current_plugin.enable()
self.joy_subscriber = rospy.Subscriber('/joy', Joy, self.joyCB)
def state_callback(self, msg):
"""
callback to process state messages
:param msg:
:type msg: M3ControlStates
"""
for group_name, state in zip(msg.group_name, msg.state):
if group_name in self._enabled_groups:
if state > M3ControlState.DISABLE:
# print "group: ", group_name, " is enabled"
self._enabled_groups[group_name] = True
else:
# print "group: ", group_name, " is disabled"
self._enabled_groups[group_name] = False
if not self._service_ready:
# test reconnection only each 5 seconds
if (rospy.Time.now() - self._last_message_time) < rospy.Duration(5.0):
return
self._last_message_time = rospy.Time.now()
if self._actionclient.wait_for_server(timeout=rospy.Duration(1)) is False:
rospy.logfatal("Failed to connect to state_manager action server, trying again in 5 seconds")
else:
self._service_ready = True
def __init__(self):
rospy.init_node("motor_pid")
self.nodeName = rospy.get_name()
# ROS Parameter
self.Kp = rospy.get_param('~Kp', 100)
self.Ki = rospy.get_param('~Ki', 150)
self.Kd = rospy.get_param('~Kd', 0)
self.rate = rospy.get_param('~rate',20)
self.timeout = rospy.Duration(rospy.get_param("~timeout", 0.2))
self.pinNameBwd = rospy.get_param('~pinNameFwd','')
self.pinNameFwd = rospy.get_param('~pinNameBwd','')
self.pinNameCmd = rospy.get_param('~pinNameCmd','')
self.minPwm = rospy.get_param('~minPwm',40)
if self.pinNameBwd=='' or self.pinNameFwd=='' or self.pinNameCmd=='':
rospy.logfatal("Parameters pinNameFwd, pinNameBwd and pinNameCmd are required.")
return
self.timeoutTime = None
# BeagleBone pin setup
GPIO.setup(self.pinNameFwd,GPIO.OUT,initial=0)
GPIO.setup(self.pinNameBwd,GPIO.OUT,initial=0)
self.pwmCmd = PWM(self.pinNameCmd,frequency=100,dutyCycle=0,enable=1)
# ROS publisher/subscribers
rospy.Subscriber("wheel_vel",Float64,self.wheelVelUpdate)
rospy.Subscriber("cmd_vel" ,Float64,self.cmdVelUpdate)
self.initialized = True
rospy.loginfo("%s started",self.nodeName)
def __init__(self):
self.initialized = False
self.dry_run = rospy.get_param("~dry_run", False)
self.clingo_interface = ClingoWrapper(AtomKRR2014)
initial_file = rospy.get_param("~initial_file", None)
self.query_file = rospy.get_param("~query_file")
self.domain_costs_file = rospy.get_param("~domain_costs_file", None)
self.costs_file = rospy.get_param("~costs_file", None)
self.enable_learning = rospy.get_param("~enable_learning", False)
if self.dry_run and not initial_file:
rospy.logfatal("Dry run requested, but no initial state provided." +
" Please set the ~initial_file param.")
return
self.initial_file = "/tmp/initial"
if self.dry_run:
rospy.loginfo("Supplied initial state written to: " +
self.initial_file)
shutil.copyfile(initial_file, self.initial_file)
self.executor = ActionExecutorKRR2014(self.dry_run, self.initial_file)
if self.enable_learning:
rospy.wait_for_service('cost_learner/increment_episode')
rospy.wait_for_service('cost_learner/add_sample')
self.finalize_episode = \
rospy.ServiceProxy('cost_learner/increment_episode', Empty)
self.add_sample = \
rospy.ServiceProxy('cost_learner/add_sample',
CostLearnerInterface)
self.planning_times_file = rospy.get_param("~planning_times_file",
None)
self.initialized = True
def readParamFromFile(self):
# Check file existence
fname = self.getFilePath(self.veh_name)
# Use default.yaml if file doesn't exsit
if not os.path.isfile(fname):
rospy.logwarn("[%s] %s does not exist. Using default.yaml." % (self.node_name, fname))
fname = self.getFilePath("default")
with open(fname, "r") as in_file:
try:
yaml_dict = yaml.load(in_file)
except yaml.YAMLError as exc:
rospy.logfatal("[%s] YAML syntax error. File: %s fname. Exc: %s" % (self.node_name, fname, exc))
rospy.signal_shutdown()
return
# Set parameters using value in yaml file
if yaml_dict is None:
# Empty yaml file
return
for param_name in ["gain", "trim", "baseline", "k", "radius"]:
param_value = yaml_dict.get(param_name)
if param_name is not None:
rospy.set_param("~" + param_name, param_value)
else:
# Skip if not defined, use default value instead.
pass
def __init__(self):
if not rospy.has_param("teleop"):
rospy.logfatal("no configuration was found, taking node down")
raise JoyTeleopException("no config")
teleop_cfg = rospy.get_param("teleop")
rospy.Subscriber('joy', sensor_msgs.msg.Joy, self.joy_callback)
self.publishers = {}
self.al_clients = {}
self.message_types = {}
self.command_list = {}
self.offline_actions = []
self.old_buttons = []
for i in teleop_cfg:
if i in self.command_list:
rospy.logerr("command {} was duplicated".format(i))
continue
action_type = teleop_cfg[i]['type']
self.add_command(i, teleop_cfg[i])
if action_type == 'topic':
self.register_topic(i, teleop_cfg[i])
elif action_type == 'action':
self.register_action(i, teleop_cfg[i])
else:
rospy.logerr("unknown type '%s' for command '%s'", action_type, i)
def main():
rospy.init_node('new_node')
cm=smach.Concurrence(outcomes=['server_running','succeeded','time_out'], default_outcome='server_running',
outcome_map={'succeeded':{'SERVER_STATE':'succeeded'},
'time_out':{'SERVER_STATE':'preempted'},
'time_out':{'SERVER_STATE':'aborted'}
})
with cm:
goal=sequenceGoal()
goal.n=100
smach.Concurrence.add('SERVER_STATE', SimpleActionState("sequence", sequenceAction,goal))
# sis = smach_ros.IntrospectionServer('server_name', cm, '/SM_ROOT')
# sis.start()
outcome=cm.execute()
if(outcome=='succeeded'):
rospy.loginfo("hehe ")
elif outcome=='server_running':
rospy.logfatal("shit")
else:
rospy.logfatal('omg')
rospy.loginfo("done working")
def __find_motors(self):
rospy.loginfo('Pinging motor IDs %d through %d...' % (self.min_motor_id, self.max_motor_id))
self.motors = []
self.motor_static_info = {}
try:
for motor_id in range(self.min_motor_id, self.max_motor_id + 1):
result = self.dxl_io.ping(motor_id)
if result: self.motors.append(motor_id)
if self.motors:
rospy.loginfo('Found motors with IDs: %s.' % str(self.motors))
else:
rospy.logfatal('No motors found, aborting.')
sys.exit(1)
rospy.set_param('dynamixel/%s/connected_ids' % self.port_namespace, self.motors)
counts = {10: 0, 12: 0, 18: 0, 24: 0, 28: 0, 29: 0, 64: 0, 107: 0, 113: 0, 116: 0, 117: 0}
for motor_id in self.motors:
model_number = self.dxl_io.get_model_number(motor_id)
counts[model_number] += 1
self.__fill_motor_parameters(motor_id, model_number)
status_str = 'There are '
for model_number,count in counts.items():
if count: status_str += '%d %s, ' % (count, DXL_MODEL_TO_PARAMS[model_number]['name'])
rospy.loginfo('%s servos connected' % status_str[:-2])
rospy.loginfo('Dynamixel Manager on port %s initialized' % self.port_name)
except dynamixel_io.FatalErrorCodeError, fece:
rospy.logfatal(fece)
signal_shutdown(fece)
def rapp_print(var, verbosity='DEBUG'):
callerframerecord = inspect.stack()[1]
frame = callerframerecord[0]
info = inspect.getframeinfo(frame)
if verbosity == 'DEBUG':
rospy.logdebug('[ FILE: ' + os.path.basename(info.filename) + \
', FUNCTION: ' + info.function + ', LINE: ' + str(info.lineno) + \
' ]: ' + str(var) )
elif verbosity == 'INFO':
rospy.loginfo('[ FILE: ' + os.path.basename(info.filename) + \
', FUNCTION: ' + info.function + ', LINE: ' + str(info.lineno) + \
' ]: ' + str(var) )
elif verbosity == 'WARN':
rospy.logwarn('[ FILE: ' + os.path.basename(info.filename) + \
', FUNCTION: ' + info.function + ', LINE: ' + str(info.lineno) + \
' ]: ' + str(var) )
elif verbosity == 'ERROR':
rospy.logerr('[ FILE: ' + os.path.basename(info.filename) + \
', FUNCTION: ' + info.function + ', LINE: ' + str(info.lineno) + \
' ]: ' + str(var) )
elif verbosity == 'FATAL':
rospy.logfatal('[ FILE: ' + os.path.basename(info.filename) + \
', FUNCTION: ' + info.function + ', LINE: ' + str(info.lineno) + \
' ]: ' + str(var) )
def validateInputs(self):
valid_flag = True
for input_name, input_dict in self.inputs_dict.items():
if "topic" not in input_dict:
rospy.logfatal("[%s] topic not defined for input %s" %(self.node_name,input_name))
valid_flag = False
return valid_flag
def project(self, P_list, pts):
N = pts.shape[1]
# Reshape P_list into an actual matrix
P = reshape( matrix(P_list, float), (3,4) )
# Update the baseline by the "baseline_shift"
P[0,3] = P[0,3] + self._config['baseline_shift']
P[0,0] = P[0,0] + self._config['f_shift']
P[1,1] = P[1,1] + self._config['f_shift']
P[0,2] = P[0,2] + self._config['cx_shift']
P[1,2] = P[1,2] + self._config['cy_shift']
if (pts.shape[0] == 3):
rospy.logfatal('Got vector of points with only 3 rows. Was expecting at 4 rows (homogenous coordinates)')
# Apply projection matrix
pixel_pts_h = P * pts
# Strip out last row (3rd) and rescale
if self._rgbd:
pixel_pts = pixel_pts_h[0:3,:] / pixel_pts_h[2,:]
for i in range(N):
depth = pts[2,i]
pixel_pts[2,i] = self.get_disparity(P,depth)
else:
pixel_pts = pixel_pts_h[0:2,:] / pixel_pts_h[2,:]
return pixel_pts
def default_caller(command):
"""
BB Command callback which:
1. Attends a BB Command Invocation.
2. Parses the parameters list comming from BlackBoard to the Default_BB_ROS_Bridge format.
3. Invoke the correponding service (which has the same name of the bridged command) with the parsed parameters and waits for his response.
4. Send the response received from the ROS service and send it to Blackboard.
Receives:
command: A command (pyrobotics) object
Return:
The response to the command caller
"""
rospy.logdebug('Call to command "' + str(command) + '" received from BB.')
# send directly the parameters receved from BB to the ros service
try:
rospy.wait_for_service(command.name, timeout=4)
default_service = rospy.ServiceProxy(command.name, Default_BB_ROS_Bridge)
resp1 = default_service(command.params)
return Response.FromCommandObject(command, True, str(resp1.response))
except rospy.ServiceException:
rospy.logfatal('Call to service: "' + str(command.name) + '" failed.')
except rospy.ROSException:
rospy.logfatal("Timeout exceeded while waiting for the service " + command.name + " to be available.")
# send the response to BB
return Response.FromCommandObject(command, False, "")
def get_client(service_name, service_class):
rospy.wait_for_service(service_name)
try:
return rospy.ServiceProxy(service_name, service_class)
except rospy.ServiceException as se:
rospy.logfatal("ROSVendor.core.generator.client_proxy: cannot found service \"%s\".", service_name)
raise se
def main():
rospy.init_node('get_remote_map_node', anonymous=False)
if not (rospy.has_param('server_uri') and rospy.has_param('world')):
rospy.logfatal("no server_uri or world params provided")
return
server = rospy.get_param("server_uri", None)
world = rospy.get_param("world", None)
resolution = rospy.get_param("resolution", 5)
url = ("{server}/worlds/{world}/map.gmapping?resolution={resolution}"
.format(**locals()))
rospack = rospkg.RosPack()
# TODO: not nice
path = "{root}/worlds/{world}/map".format(
root=rospack.get_path('alma_remote_planner'),
world=world)
response = requests.get(url, stream=True)
rospy.loginfo(
"Sent request to {url}. Got response {response}".format(**locals()))
if response.status_code != 200:
rospy.logerror("No valid response")
else:
z = zipfile.ZipFile(StringIO.StringIO(response.content))
z.extractall(path=path)
def call_pddl_planner(self, problem, domain):
if self._planner_name == "ff":
# -f problem -o domain
output = commands.getoutput("rosrun ff ff -f %s -o %s" % (problem, domain))
return self.parse_pddl_result(output)
# ffha
elif self._planner_name == "ffha":
output = commands.getoutput("rosrun ffha ffha %s -f %s -o %s" % (self._search_option, problem, domain))
if re.search("final domain representation is:", output):
tmp = output.split("metric:")
if len(tmp) > 1:
output = tmp[1];
self._result.data = tmp;
return self.parse_pddl_result_ffha(output)
elif self._planner_name == "downward":
(fd, path_name) = tempfile.mkstemp(text=True, prefix='plan_')
(status, output) = commands.getstatusoutput("rosrun downward plan %s %s %s --plan-file %s" % (domain,
problem,
self._search_option,
path_name))
rospy.loginfo(output)
if status == 0:
self._result.data = output
return self.parse_pddl_result_downward(path_name)
else:
return False
else:
rospy.logfatal("set invalid planner: %s !" % self._planner_name)
return
def __init__(self):
rospy.init_node('dynamixel_joint_state_publisher', anonymous=True)
rate = rospy.get_param('~rate', 20)
r = rospy.Rate(rate)
joint_controllers = rospy.get_param('~joint_controllers', '')
if joint_controllers:
rospy.loginfo("Joints: {0}".format(joint_controllers))
else:
rospy.logfatal("No joint controllers configured. This node needs to have rosparam ~joint_controllers be set to a list of dynamixel joint controllers. Please see dynamixel_joint_state_publisher/launch/example.launch")
exit(1)
self.servos = list()
self.controllers = list()
self.joint_states = dict({})
for joint_controller in sorted(joint_controllers):
joint_name = rospy.get_param("/"+joint_controller+'/joint_name', '')
self.joint_states[joint_name] = JointStateMessage(joint_name, 0.0, 0.0, 0.0)
self.controllers.append(joint_controller)
# Start controller state subscribers
[rospy.Subscriber(c + '/state', JointStateDynamixel, self.controller_state_handler) for c in self.controllers]
# Start publisher
self.joint_states_pub = rospy.Publisher('/joint_states', JointState)
rospy.loginfo("Starting Dynamixel Joint State Publisher at " + str(rate) + "Hz")
while not rospy.is_shutdown():
self.publish_joint_states()
r.sleep()
def __init__(self):
self.pub = tf2_ros.TransformBroadcaster()
pose = rospy.get_param('~transform')
self.pose_msg = TransformStamped()
self.pose_msg.header.frame_id = pose['parent_id']
self.pose_msg.child_frame_id = pose['child_id']
self.pose_msg.transform.translation.x = pose['translation'][0]
self.pose_msg.transform.translation.y = pose['translation'][1]
self.pose_msg.transform.translation.z = pose['translation'][2]
q = self.pose_msg.transform.rotation
if 'rotation' in pose and 'quaternion' in pose:
rospy.logfatal("'quaterion' and 'rotation' specified. Please remove one")
raise
if 'rotation' in pose:
(q.x, q.y, q.z, q.w) = PyKDL.Rotation(*pose['rotation']).GetQuaternion()
elif 'quaternion' in pose:
r = pose['quaternion']
q.x = r['x']
q.y = r['y']
q.z = r['z']
q.w = r['w']
else:
rospy.logfatal("No rotation specified. Use 'rotation' or 'quaternion'.")
raise
def callback(self, img, pose2d, pose3d):
header = img.header
try:
img = self.cv_bridge.imgmsg_to_cv2(img, "bgr8")
except cv_bridge.CvBridgeError as e:
rospy.logerr("Failed to convert image: %s" % str(e))
rospy.logerr(traceback.format_exc())
return
faces = []
rects = []
face_origins = []
for p2d, p3d in zip(pose2d.poses, pose3d.poses):
try:
score = p2d.scores[p2d.limb_names.index("Nose")]
if score < self.face_threshold:
continue
nose = p2d.poses[p2d.limb_names.index("Nose")]
neck = p2d.poses[p2d.limb_names.index("Neck")]
width_half = np.sqrt((neck.position.x - nose.position.x) ** 2 +
(neck.position.y - nose.position.y) ** 2)
width_half *= 1.0 + self.face_padding
rect = Rect(x=int(nose.position.x-width_half),
y=int(nose.position.y-width_half),
width=int(width_half*2),
height=int(width_half*2))
face_origin = p3d.poses[p3d.limb_names.index("Nose")]
except ValueError:
continue
try:
face = img[rect.y:rect.y+rect.height, rect.x:rect.x+rect.width]
if face.size <= 0:
continue
except Exception as e:
rospy.logerr("Failed to crop image: %s" % str(e))
rospy.logerr(traceback.format_exc())
continue
rects.append(rect)
face_origins.append(face_origin)
faces.append(face)
if not faces:
rospy.logdebug("No face found")
return
try:
results = self.predictor(faces)
except OverflowError:
rospy.logfatal(traceback.format_exc())
rospy.signal_shutdown("shutdown")
except Exception as e:
rospy.logerr("Failed to predict: %s" % str(e))
rospy.logerr(traceback.format_exc())
return
for i in range(len(results)):
results[i].update({
"face_origin": face_origins[i],
"rect": rects[i],
})
self.publish_face_rects(header, results)
self.publish_gender(header, results)
self.publish_pose(header, results)
def critical(self, msg):
rospy.logfatal(msg) # print(msg) #
cam_path = rospy.get_param(nodename + '/camPath')
res_w = rospy.get_param(nodename + '/resolution' + '/width')
res_h = rospy.get_param(nodename + '/resolution' + '/height')
os.system('v4l2-ctl -d ' + cam_path + ' -v width=' + str(res_w) +
',height=' + str(res_h))
os.system('v4l2-ctl -d ' + cam_path + ' -c focus_auto=0')
os.system('v4l2-ctl -d ' + cam_path + ' -c exposure_auto=50')
cam = cv2.VideoCapture() #create cam object
cam.open(cam_path) #start cam based on cam_path
#cam.open('/dev/video6')
ip = ImagePublisher() #create ImagePublisher object
try:
while not rospy.is_shutdown():
ip.get_img(cam, image_mode) #get image
rate.sleep() #sleep for rest of time
except KeyboardInterrupt:
#print("shutting down ros")
rospy.logfatal("Keyboard Interrupt. Shutting down cam_bridge node")
if __name__ == '__main__':
try:
main()
except rospy.ROSInterruptException:
rospy.logfatal("ROS Interrupt. Shutting down cam_bridge node")
pass
def __on_packet(self, buf):
state = RobotState.unpack(buf)
self.last_state = state
#import deserialize; deserialize.pstate(self.last_state)
#log("Packet. Mode=%s" % state.robot_mode_data.robot_mode)
if not state.robot_mode_data.real_robot_enabled:
rospy.logfatal(
"Real robot is no longer enabled. Driver is fuxored")
time.sleep(2)
sys.exit(1)
###
# IO-Support is EXPERIMENTAL
#
# Notes:
# - Where are the flags coming from? Do we need flags? No, as 'prog' does not use them and other scripts are not running!
# - analog_input2 and analog_input3 are within ToolData
# - What to do with the different analog_input/output_range/domain?
# - Shall we have appropriate ur_msgs definitions in order to reflect MasterboardData, ToolData,...?
###
# Use information from the robot state packet to publish IOStates
msg = IOStates()
#gets digital in states
for i in range(0, 10):
msg.digital_in_states.append(
DigitalIn(i, (state.masterboard_data.digital_input_bits &
(1 << i)) >> i))
#gets digital out states
for i in range(0, 10):
msg.digital_out_states.append(
DigitalOut(i, (state.masterboard_data.digital_output_bits &
(1 << i)) >> i))
#gets analog_in[0] state
inp = state.masterboard_data.analog_input0 / MULT_analog_robotstate
msg.analog_in_states.append(Analog(0, inp))
#gets analog_in[1] state
inp = state.masterboard_data.analog_input1 / MULT_analog_robotstate
msg.analog_in_states.append(Analog(1, inp))
#gets analog_out[0] state
inp = state.masterboard_data.analog_output0 / MULT_analog_robotstate
msg.analog_out_states.append(Analog(0, inp))
#gets analog_out[1] state
inp = state.masterboard_data.analog_output1 / MULT_analog_robotstate
msg.analog_out_states.append(Analog(1, inp))
#print "Publish IO-Data from robot state data"
pub_io_states.publish(msg)
# Updates the state machine that determines whether we can program the robot.
can_execute = (state.robot_mode_data.robot_mode
in [RobotMode.READY, RobotMode.RUNNING])
if self.robot_state == self.CONNECTED:
if can_execute:
self.__trigger_ready_to_program()
self.robot_state = self.READY_TO_PROGRAM
elif self.robot_state == self.READY_TO_PROGRAM:
if not can_execute:
self.robot_state = self.CONNECTED
elif self.robot_state == self.EXECUTING:
if not can_execute:
self.__trigger_halted()
self.robot_state = self.CONNECTED
# Report on any unknown packet types that were received
if len(state.unknown_ptypes) > 0:
state.unknown_ptypes.sort()
s_unknown_ptypes = [str(ptype) for ptype in state.unknown_ptypes]
self.throttle_warn_unknown(
1.0, "Ignoring unknown pkt type(s): %s. "
"Please report." % ", ".join(s_unknown_ptypes))
def vel_callback(self, data):
# Record times
recv_time = rospy.Time.now().to_sec()
msg_time = data.header.stamp.to_sec()
# Filter old/delayed messages
if (msg_time < (recv_time - 0.25)):
# And if the time difference is significantly greater than the average offset
# Ignore this msg
rospy.logwarn("Ignoring {} second old message".format(recv_time - msg_time))
return
# Filter messages at frequency
if (msg_time < (self.last_msg_time + 1.0/MSG_PER_SECOND)):
# Ignore this message
return
else:
rospy.logdebug("Time since last message received: {} seconds".format(recv_time - self.last_recv_time))
self.last_recv_time = recv_time
self.last_msg_time = msg_time
# Notify of reconnection
if (self.watchdog_fired == True):
self.watchdog_fired = False
self.conn_lost_dur = rospy.Time.now() - self.conn_lost_time
rospy.logwarn("Connection to controller reestablished! Lost connection for {} seconds.".format(self.conn_lost_dur.to_sec()))
# # If one disconnects, stop all.
# try:
# --- Time BEGIN here
odrv_com_time_start = rospy.Time.now().to_sec()
# Read errors and feed watchdog at slower rate
if (recv_time > self.next_wd_feed_time):
self.next_wd_feed_time = recv_time + 1.0/WD_FEED_PER_SECOND
# Do stuff for all axes
for ax in self.axes:
ax.watchdog_feed()
# TODO
# # ODrive watchdog error clear
# if(ax.error == errors.axis.ERROR_WATCHDOG_TIMER_EXPIRED):
# ax.error = errors.axis.ERROR_NONE
# rospy.logwarn("Cleared ODrive watchdog error")
# For other errors
if (ax.error != errors.axis.ERROR_NONE):
rospy.logfatal("Received axis error: {} {}".format(self.axes.index(ax), ax.error))
# -- Time STOP: Calculate time taken to reset ODrive
rospy.logdebug("Reseting each ODrive watchdog took {} seconds".format(rospy.Time.now().to_sec() - odrv_com_time_start))
# Emergency brake - 4 & 5 are bumpers
if (data.buttons[4] and data.buttons[5]):
# Stop motors
rospy.logdebug("Applying E-brake")
for ax in (self.leftAxes + self.rightAxes):
ax.controller.vel_ramp_target = 0
ax.controller.vel_setpoint = 0
else:
# # Control motors as tank drive
for ax in self.leftAxes:
ax.controller.vel_ramp_target = data.axes[1] * SPEED_LIMIT
for ax in self.rightAxes:
ax.controller.vel_ramp_target = data.axes[4] * SPEED_LIMIT * -1
# Control motors in arcade mode
# left_speed = data.axes[1] * SPEED_LIMIT
# right_speed = left_speed * -1
# turning = data.axes[5]
# if(turning > 0):
# #turn right by increasing left speed - mult by between 1 and 2
# left_speed = left_speed * (abs(turning) + 1)
# if(turning < 0):
# right_speed = right_speed * (abs(turning) + 1)
# for ax in self.leftAxes:
# ax.controller.vel_ramp_target = left_speed
# for ax in self.rightAxes:
# ax.controller.vel_ramp_target = right_speed
# -- Time STOP: Calculate time taken to reset ODrive
rospy.logdebug("Communication with odrives took {} seconds".format(rospy.Time.now().to_sec() - odrv_com_time_start))
# except ChannelBrokenException:
# # Stop motors (This could be a problem if the disconnected Odrive continues to run...)
# for ax in (self.leftAxes + self.rightAxes):
# ax.controller.vel_ramp_target = 0
# # ax.controller.vel_setpoint = 0
rospy.loginfo(rospy.get_caller_id() + "Left: %s", data.axes[1] * SPEED_LIMIT)
rospy.loginfo(rospy.get_caller_id() + "Right: %s", data.axes[4] * SPEED_LIMIT)
# Received mesg so reset watchdog
self.timer.shutdown()
self.timer = rospy.Timer(rospy.Duration(self.timeout), self.watchdog_callback, oneshot=True)
tot_time = rospy.Time.now().to_sec() - recv_time
# --- Time STOP: Calculate time taken to reset ODrive
rospy.logdebug("Callback execution took {} seconds".format(tot_time))
def __init__(self):
''' Initialize ros node and read params '''
# Parse parameters
self.ns_planner = rospy.get_param('~ns_planner',
"/firefly/planner_node")
self.planner_delay = rospy.get_param(
'~delay', 0.0) # Waiting time until the planner is launched
self.evaluate = rospy.get_param(
'~evaluate', False) # Periodically save the voxblox state
self.startup_timeout = rospy.get_param(
'~startup_timeout', 0.0) # Max allowed time for startup, 0 for inf
self.eval_frequency = rospy.get_param('~eval_frequency',
5.0) # Save rate in seconds
self.time_limit = rospy.get_param(
'~time_limit', 0.0) # Maximum sim duration in minutes, 0 for inf
self.reset_unreal_cv_ros = rospy.get_param(
'~reset_unreal_cv_ros', True) # On shutdown reset pose to 0
self.ns_unreal_cv_ros = rospy.get_param('~ns_unreal_cv_ros',
"/unreal/unreal_ros_client")
self.eval_walltime_0 = None
self.eval_rostime_0 = None
if self.evaluate:
# Setup parameters
self.eval_directory = rospy.get_param(
'~eval_directory',
'DirParamNotSet') # Periodically save voxblox map
if not os.path.isdir(self.eval_directory):
rospy.logfatal("Invalid target directory '%s'.",
self.eval_directory)
sys.exit(-1)
self.ns_voxblox = rospy.get_param('~ns_voxblox',
"/voxblox/voxblox_node")
# Statistics
self.eval_n_maps = 0
self.eval_n_pointclouds = 0
# Setup data directory
if not os.path.isdir(os.path.join(self.eval_directory,
"tmp_bags")):
os.mkdir(os.path.join(self.eval_directory, "tmp_bags"))
self.eval_directory = os.path.join(
self.eval_directory,
datetime.datetime.now().strftime("%Y%m%d_%H%M%S"))
os.mkdir(self.eval_directory)
rospy.set_param(self.ns_planner + "/performance_log_dir",
self.eval_directory)
os.mkdir(os.path.join(self.eval_directory, "voxblox_maps"))
self.eval_data_file = open(
os.path.join(self.eval_directory, "voxblox_data.csv"), 'wb')
self.eval_writer = csv.writer(self.eval_data_file,
delimiter=',',
quotechar='|',
quoting=csv.QUOTE_MINIMAL,
lineterminator='\n')
self.eval_writer.writerow(
['MapName', 'RosTime', 'WallTime', 'NPointclouds', 'CPUTime'])
self.eval_writer.writerow(
['Unit', 'seconds', 'seconds', '-', 'seconds'])
self.eval_log_file = open(
os.path.join(self.eval_directory, "data_log.txt"), 'a')
# Subscribers, Services
self.ue_out_sub = rospy.Subscriber("ue_out_in",
PointCloud2,
self.ue_out_callback,
queue_size=10)
self.collision_sub = rospy.Subscriber("collision",
String,
self.collision_callback,
queue_size=10)
self.cpu_time_srv = rospy.ServiceProxy(
self.ns_planner + "/get_cpu_time", SetBool)
# Finish
self.writelog("Data folder created at '%s'." % self.eval_directory)
rospy.loginfo("Data folder created at '%s'." % self.eval_directory)
self.eval_voxblox_service = rospy.ServiceProxy(
self.ns_voxblox + "/save_map", FilePath)
rospy.on_shutdown(self.eval_finish)
self.collided = False
self.launch_simulation()
def run(self):
'''
Run node, spawning model and doing other actions as configured in program arguments.
Returns exit code, 1 for failure, 0 for success
'''
# Wait for model to exist if wait flag is enabled
if self.args.wait:
self.model_exists = False
def models_cb(models):
self.model_exists = self.args.wait in models.name
rospy.Subscriber("%s/model_states" % self.args.gazebo_namespace,
ModelStates, models_cb)
r = rospy.Rate(10)
rospy.loginfo('Waiting for model {} before proceeding.'.format(
self.args.wait))
while not rospy.is_shutdown() and not self.model_exists:
r.sleep()
if rospy.is_shutdown():
return 0
# Load model XML from file
if self.args.file:
rospy.loginfo("Loading model XML from file %s" % self.args.file)
if not os.path.exists(self.args.file):
rospy.logfatal("Error: specified file %s does not exist",
self.args.file)
return 1
if not os.path.isfile(self.args.file):
rospy.logfatal("Error: specified file %s is not a file",
self.args.file)
return 1
# load file
try:
f = open(self.args.file, 'r')
model_xml = f.read()
except IOError as e:
rospy.logerr("Error reading file {}: {}".format(
self.args.file, e))
return 1
if model_xml == "":
rospy.logerr("Error: file %s is empty", self.args.file)
return 1
# Load model XML from ROS param
elif self.args.param:
rospy.loginfo("Loading model XML from ros parameter %s" %
self.args.param)
model_xml = rospy.get_param(self.args.param)
if model_xml == "":
rospy.logerr("Error: param does not exist or is empty")
return 1
# Generate model XML by putting requested model name into request template
elif self.args.database:
rospy.loginfo("Loading model XML from Gazebo Model Database")
model_xml = self.MODEL_DATABASE_TEMPLATE.format(self.args.database)
elif self.args.stdin:
rospy.loginfo("Loading model XML from stdin")
model_xml = sys.stdin.read()
if model_xml == "":
rospy.logerr("Error: stdin buffer was empty")
return 1
# Parse xml to detect invalid xml before sending to gazebo
try:
xml_parsed = xml.etree.ElementTree.fromstring(model_xml)
except xml.etree.ElementTree.ParseError as e:
rospy.logerr('Invalid XML: {}'.format(e))
return 1
# Replace package:// with model:// for mesh tags if flag is set
if self.args.package_to_model:
for element in xml_parsed.iterfind('.//mesh'):
filename_tag = element.get('filename')
if filename_tag is None:
continue
url = urlsplit(filename_tag)
if url.scheme == 'package':
url = SplitResult('model', *url[1:])
element.set('filename', url.geturl())
# Encode xml object back into string for service call
model_xml = xml.etree.ElementTree.tostring(xml_parsed)
# For Python 3
if not isinstance(model_xml, str):
model_xml = model_xml.decode(encoding='ascii')
# Form requested Pose from arguments
initial_pose = Pose()
initial_pose.position.x = rospy.get_param('~x_pos')
initial_pose.position.y = rospy.get_param('~y_pos')
initial_pose.position.z = self.args.z
q = quaternion_from_euler(self.args.R, self.args.P, self.args.Y)
initial_pose.orientation = Quaternion(*q)
# Spawn model using urdf or sdf service based on arguments
success = False
if self.args.urdf:
success = gazebo_interface.spawn_urdf_model_client(
self.args.model, model_xml, self.args.robot_namespace,
initial_pose, self.args.reference_frame,
self.args.gazebo_namespace)
elif self.args.sdf:
success = gazebo_interface.spawn_sdf_model_client(
self.args.model, model_xml, self.args.robot_namespace,
initial_pose, self.args.reference_frame,
self.args.gazebo_namespace)
if not success:
rospy.logerr('Spawn service failed. Exiting.')
return 1
# Apply joint positions if any specified
if len(self.args.joints) != 0:
joint_names = [joint[0] for joint in self.args.joints]
joint_positions = [joint[1] for joint in self.args.joints]
success = gazebo_interface.set_model_configuration_client(
self.args.model, "", joint_names, joint_positions,
self.args.gazebo_namespace)
if not success:
rospy.logerr('SetModelConfiguration service failed. Exiting.')
return 1
# Unpause physics if user requested
if self.args.unpause:
rospy.loginfo('Unpausing physics')
rospy.wait_for_service('%s/unpause_physics' %
self.args.gazebo_namespace)
try:
unpause_physics = rospy.ServiceProxy(
'%s/unpause_physics' % self.args.gazebo_namespace, Empty)
unpause_physics()
except rospy.ServiceException as e:
rospy.logerr(
"Unpause physics service call failed: {}".format(e))
return 1
# If bond enabled, setup shutdown callback and wait for shutdown
if self.args.bond:
rospy.on_shutdown(self._delete_model)
rospy.loginfo('Waiting for shutdown to delete model {}'.format(
self.args.model))
rospy.spin()
return 0
#! /usr/bin/env python
import rospy
import random
import time
#rospy.init_node('log_demo', log_level=rospy.DEBUG)
#rospy.init_node('log_demo', log_level=rospy.INFO)
#rospy.init_node('log_demo', log_level=rospy.WARN)
rospy.init_node('log_demo', log_level=rospy.ERROR)
#rospy.init_node('log_demo', log_level=rospy.FATAL)
rate = rospy.Rate(0.5)
# In Kinetic ROS you can use this periodic log publication.
#rospy.loginfo_throttle(120, "DeathStars Minute info: "+str(time.time()))
while not rospy.is_shutdown():
rospy.logdebug("There is a missing droid")
rospy.loginfo("The Emperors Capuchino is done")
rospy.logwarn("The Revels are coming time " + str(time.time()))
exhaust_number = random.randint(1, 100)
port_number = random.randint(1, 100)
rospy.logerr("The thermal exhaust port %s, right below the main port %s",
exhaust_number, port_number)
rospy.logfatal("The DeathStar Is EXPLODING")
rate.sleep()
rospy.logfatal("END")
import tempfile
from diagnostic_msgs.msg import DiagnosticStatus, KeyValue, DiagnosticArray
from sound_play.msg import SoundRequest, SoundRequestAction, SoundRequestResult, SoundRequestFeedback
import actionlib
try:
import gi
gi.require_version('Gst', '1.0')
from gi.repository import Gst as Gst
except:
str = """
**************************************************************
Error opening pygst. Is gstreamer installed?
**************************************************************
"""
rospy.logfatal(str)
print str
exit(1)
def sleep(t):
try:
rospy.sleep(t)
except:
pass
class soundtype:
STOPPED = 0
LOOPING = 1
COUNTING = 2
def createMaster(self):
file = open(self.file_name, 'w')
file.write('<?xml version="1.0"?>\n')
file.write('<!-- SESSION ID: ' + str(self.session_id) + ' -->\n')
file.write('<launch>\n\n')
# Calibration parameters
file.write(' <!-- Calibration parameters -->\n')
file.write(' <arg name="lock_world_frame" default="false" />\n')
file.write(' <group if="$(arg lock_world_frame)">\n')
file.write(' <arg name="fixed_sensor_id" />\n')
file.write(
' <rosparam command="load" file="$(find opt_calibration)/conf/camera_poses.yaml" />\n'
)
file.write(' </group>\n\n')
# Network parameters
file.write(' <!-- Network parameters -->\n')
file.write(' <arg name="num_sensors" default="' +
str(len(self.sensor_list)) + '" />\n\n')
index = 0
for sensor in self.sensor_list:
# if sensor['type'] == 'sr4500':
# sensor_name = "SR_" + sensor['id'].replace('.', '_');
# file.write(' <arg name="sensor_' + str(index) + '_id" default="' + sensor_name + '" />\n')
# else:
# file.write(' <arg name="sensor_' + str(index) + '_id" default="' + sensor['id'] + '" />\n')
file.write(' <arg name="sensor_' + str(index) +
'_id" default="' + sensor['id'] + '" />\n')
file.write(' <arg name="sensor_' + str(index) +
'_name" default="$(arg sensor_' + str(index) +
'_id)" />\n\n')
index = index + 1
# Checkerboard parameters
file.write(' <arg name="rows" default="' +
str(self.checkerboard['rows']) + '" />\n')
file.write(' <arg name="cols" default="' +
str(self.checkerboard['cols']) + '" />\n')
file.write(' <arg name="cell_width" default="' +
str(self.checkerboard['cell_width']) + '" />\n')
file.write(' <arg name="cell_height" default="' +
str(self.checkerboard['cell_height']) + '" />\n\n')
# Rviz and calibration node
file.write(' <!-- Opening Rviz for visualization -->\n')
file.write(
' <node name="rviz" pkg="rviz" type="rviz" args="-d $(find opt_calibration)/conf/opt_calibration.rviz" />\n\n'
)
file.write(' <!-- Plot calibration status -->\n')
file.write(
' <node name="opt_calibration_status_plot" pkg="opt_calibration" type="status_plot.py" output="screen">\n'
)
file.write(
' <remap from="~calibration_status" to="/opt_calibration/status" />\n'
)
file.write(' </node>\n\n')
file.write(' <!-- Launching calibration -->\n')
file.write(
' <node pkg="opt_calibration" type="opt_calibration" name="opt_calibration" output="screen">\n\n'
)
file.write(
' <param unless="$(arg lock_world_frame)" name="world_computation" value="last_checkerboard" />\n'
)
file.write(
' <param if="$(arg lock_world_frame)" name="world_computation" value="update" />\n'
)
file.write(
' <param if="$(arg lock_world_frame)" name="fixed_sensor/name" value="/$(arg fixed_sensor_id)" />\n\n'
)
file.write(
' <param name="num_sensors" value="$(arg num_sensors)" />\n\n'
)
file.write(
' <param name="rows" value="$(arg rows)" />\n')
file.write(
' <param name="cols" value="$(arg cols)" />\n')
file.write(
' <param name="cell_width" value="$(arg cell_width)" />\n'
)
file.write(
' <param name="cell_height" value="$(arg cell_height)" />\n\n'
)
#if (calibration_with_serials)??
# file.write(' <param name="calibration_with_serials" value="true" />\n\n')
index = 0
for sensor in self.sensor_list:
file.write(' <param name="sensor_' + str(index) +
'/name" value="/$(arg sensor_' + str(index) +
'_name)" />\n')
if sensor['type'] == 'sr4500':
file.write(' <param name="sensor_' + str(index) +
'/type" value="pinhole_rgb" />\n')
file.write(' <remap from="~sensor_' + str(index) +
'/image" to="/$(arg sensor_' + str(index) +
'_name)/intensity/image_resized" />\n')
file.write(' <remap from="~sensor_' + str(index) +
'/camera_info" to="/$(arg sensor_' + str(index) +
'_name)/intensity/camera_info" />\n\n')
elif sensor['type'] == 'kinect1':
file.write(' <param name="sensor_' + str(index) +
'/type" value="pinhole_rgb" />\n')
file.write(' <remap from="~sensor_' + str(index) +
'/image" to="/$(arg sensor_' + str(index) +
'_name)/rgb/image_color" />\n')
file.write(' <remap from="~sensor_' + str(index) +
'/camera_info" to="/$(arg sensor_' + str(index) +
'_name)/rgb/camera_info" />\n\n')
elif sensor['type'] == 'kinect2':
file.write(' <param name="sensor_' + str(index) +
'/type" value="pinhole_rgb" />\n')
file.write(' <remap from="~sensor_' + str(index) +
'/image" to="/$(arg sensor_' + str(index) +
'_name)/hd/image_color" />\n')
file.write(' <remap from="~sensor_' + str(index) +
'/camera_info" to="/$(arg sensor_' + str(index) +
'_name)/hd/camera_info" />\n\n')
elif sensor['type'] == 'stereo_pg':
file.write(' <param name="sensor_' + str(index) +
'/type" value="pinhole_rgb" />\n')
file.write(' <remap from="~sensor_' + str(index) +
'/image" to="/$(arg sensor_' + str(index) +
'_name)/left/image_color" />\n')
file.write(' <remap from="~sensor_' + str(index) +
'/camera_info" to="/$(arg sensor_' + str(index) +
'_name)/left/camera_info" />\n\n')
else:
rospy.logfatal('Sensor type "' + sensor['type'] +
'" not supported yet!')
index = index + 1
file.write(' </node>\n\n')
file.write('</launch>\n')
file.close()
# Create launch file for defining user reference frame:
file = open(self.frame_file_name, 'w')
file.write('<?xml version="1.0"?>\n')
file.write('<!-- SESSION ID: ' + str(self.session_id) + ' -->\n')
file.write('<launch>\n\n')
# Calibration parameters
file.write(' <!-- Calibration parameters -->\n')
file.write(
' <rosparam command="load" file="$(find opt_calibration)/conf/camera_poses.yaml" />\n\n'
)
# Network parameters
file.write(' <!-- Network parameters -->\n')
file.write(' <arg name="num_sensors" default="' +
str(len(self.sensor_list)) + '" />\n\n')
index = 0
for sensor in self.sensor_list:
# if sensor['type'] == 'sr4500':
# sensor_name = "SR_" + sensor['id'].replace('.', '_');
# file.write(' <arg name="sensor_' + str(index) + '_id" default="' + sensor_name + '" />\n')
# else:
# file.write(' <arg name="sensor_' + str(index) + '_id" default="' + sensor['id'] + '" />\n')
file.write(' <arg name="sensor_' + str(index) +
'_id" default="' + sensor['id'] + '" />\n')
file.write(' <arg name="sensor_' + str(index) +
'_name" default="$(arg sensor_' + str(index) +
'_id)" />\n\n')
index = index + 1
# Calibration node
file.write(' <!-- Launching calibration -->\n')
file.write(
' <node pkg="opt_calibration" type="opt_define_reference_frame" name="opt_define_reference_frame" output="screen">\n'
)
file.write(
' <rosparam command="load" file="$(find opt_calibration)/conf/camera_network.yaml" />\n\n'
)
file.write(
' <param name="num_sensors" value="$(arg num_sensors)" />\n\n'
)
index = 0
for sensor in self.sensor_list:
file.write(' <param name="sensor_' + str(index) +
'/name" value="/$(arg sensor_' + str(index) +
'_name)" />\n')
if sensor['type'] == 'sr4500':
file.write(' <param name="sensor_' + str(index) +
'/type" value="pinhole_rgb" />\n')
file.write(' <remap from="~sensor_' + str(index) +
'/image" to="/$(arg sensor_' + str(index) +
'_name)/intensity/image_resized" />\n')
file.write(' <remap from="~sensor_' + str(index) +
'/camera_info" to="/$(arg sensor_' + str(index) +
'_name)/intensity/camera_info" />\n\n')
elif sensor['type'] == 'kinect1':
file.write(' <param name="sensor_' + str(index) +
'/type" value="pinhole_rgb" />\n')
file.write(' <remap from="~sensor_' + str(index) +
'/image" to="/$(arg sensor_' + str(index) +
'_name)/rgb/image_color" />\n')
file.write(' <remap from="~sensor_' + str(index) +
'/camera_info" to="/$(arg sensor_' + str(index) +
'_name)/rgb/camera_info" />\n\n')
elif sensor['type'] == 'kinect2':
file.write(' <param name="sensor_' + str(index) +
'/type" value="pinhole_rgb" />\n')
file.write(' <remap from="~sensor_' + str(index) +
'/image" to="/$(arg sensor_' + str(index) +
'_name)/hd/image_color" />\n')
file.write(' <remap from="~sensor_' + str(index) +
'/camera_info" to="/$(arg sensor_' + str(index) +
'_name)/hd/camera_info" />\n\n')
elif sensor['type'] == 'stereo_pg':
file.write(' <param name="sensor_' + str(index) +
'/type" value="pinhole_rgb" />\n')
file.write(' <remap from="~sensor_' + str(index) +
'/image" to="/$(arg sensor_' + str(index) +
'_name)/left/image_color" />\n')
file.write(' <remap from="~sensor_' + str(index) +
'/camera_info" to="/$(arg sensor_' + str(index) +
'_name)/left/camera_info" />\n\n')
else:
rospy.logfatal('Sensor type "' + sensor['type'] +
'" not supported yet!')
index = index + 1
file.write(' </node>\n\n')
file.write('</launch>\n')
file.close()
def __init__(self):
self.ERRORS = {
0x0000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "Normal"),
0x0001:
(diagnostic_msgs.msg.DiagnosticStatus.WARN, "M1 over current"),
0x0002:
(diagnostic_msgs.msg.DiagnosticStatus.WARN, "M2 over current"),
0x0004:
(diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Emergency Stop"),
0x0008:
(diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Temperature1"),
0x0010: (diagnostic_msgs.msg.DiagnosticStatus.ERROR,
"Temperature2"),
0x0020: (diagnostic_msgs.msg.DiagnosticStatus.ERROR,
"Main batt voltage high"),
0x0040: (diagnostic_msgs.msg.DiagnosticStatus.ERROR,
"Logic batt voltage high"),
0x0080: (diagnostic_msgs.msg.DiagnosticStatus.ERROR,
"Logic batt voltage low"),
0x0100: (diagnostic_msgs.msg.DiagnosticStatus.WARN,
"M1 driver fault"),
0x0200: (diagnostic_msgs.msg.DiagnosticStatus.WARN,
"M2 driver fault"),
0x0400: (diagnostic_msgs.msg.DiagnosticStatus.WARN,
"Main batt voltage high"),
0x0800: (diagnostic_msgs.msg.DiagnosticStatus.WARN,
"Main batt voltage low"),
0x1000: (diagnostic_msgs.msg.DiagnosticStatus.WARN,
"Temperature1"),
0x2000: (diagnostic_msgs.msg.DiagnosticStatus.WARN,
"Temperature2"),
0x4000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "M1 home"),
0x8000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "M2 home")
}
rospy.init_node("roboclaw_node")
rospy.on_shutdown(self.shutdown)
rospy.loginfo("Connecting to roboclaw")
dev_name = rospy.get_param("~dev", "/dev/ttyACM0")
baud_rate = int(rospy.get_param("~baud", "115200"))
self.topic_timeout = int(rospy.get_param("~topic_timeout", "1"))
self.TICKS_PER_RADIAN = float(
rospy.get_param("~ticks_per_rotation", "663")) / (2 * pi)
self.MAX_SPEED = float(rospy.get_param("~max_speed", "0"))
addresses = str(rospy.get_param("~addresses", "128")).split(",")
self.addresses = []
for addr in addresses:
addr = int(addr)
if addr > 0x87 or addr < 0x80:
rospy.logfatal("Address out of range")
rospy.signal_shutdown("Address out of range")
self.addresses.append(addr)
rospy.loginfo("Addresses: %s" % str(self.addresses))
# TODO need some way to check if address is correct
try:
roboclaw.Open(dev_name, baud_rate)
except Exception as e:
rospy.logfatal("Could not connect to Roboclaw serial device")
rospy.logdebug(e)
rospy.signal_shutdown("Could not connect to Roboclaw")
return
self.updater = diagnostic_updater.Updater()
self.updater.setHardwareID("Roboclaw")
self.updater.add(
diagnostic_updater.FunctionDiagnosticTask("Vitals",
self.check_vitals))
for addr in self.addresses:
version = None
try:
version = roboclaw.ReadVersion(addr)
except Exception as e:
rospy.logwarn(
"Problem getting roboclaw version from address %d" % addr)
rospy.logdebug(e)
if version is None or not version[0]:
rospy.logwarn(
"Could not get version from roboclaw address %d" % addr)
rospy.signal_shutdown(
"Could not get version from roboclaw address %d" % addr)
return
rospy.logdebug("Controller %d version is %s" %
(addr, repr(version[1])))
roboclaw.SpeedM1M2(addr, 0, 0)
roboclaw.ResetEncoders(addr)
self.last_set_speed_time = rospy.get_rostime()
rospy.Subscriber("/cmd_motors",
MotorSpeeds,
self.cmd_motors_callback,
queue_size=1)
rospy.logdebug("dev %s", dev_name)
rospy.logdebug("baud %d", baud_rate)
rospy.logdebug("addresses %s", str(self.addresses))
rospy.logdebug("max_speed %f", self.MAX_SPEED)
rospy.logdebug("ticks_per_radian: %f", self.TICKS_PER_RADIAN)
def __init__(self):
# initilializing node
rospy.init_node('espeleo_locomotion', anonymous=True)
# sending a message
rospy.loginfo('Espeleo_Locomotion 2.0 node initiated.')
# subscribing to cmd_vel
rospy.Subscriber("cmd_vel", Twist, self.callback_command)
# subscribing to joints positions
rospy.Subscriber("ros_eposmcd/joints_positions", EspeleoJoints, self.callback_jointsPos)
# creating the three command topics publisher
self.pub_posAbs = rospy.Publisher("ros_eposmcd/position_movement_absolute", MovementArray, queue_size=1)
self.pub_posRel = rospy.Publisher("ros_eposmcd/position_movement_relative", MovementArray, queue_size=1)
self.pub_vel1 = rospy.Publisher('/device1/set_joint_state', JointState, queue_size=1)
self.pub_vel2 = rospy.Publisher('/device2/set_joint_state', JointState, queue_size=1)
self.pub_vel3 = rospy.Publisher('/device3/set_joint_state', JointState, queue_size=1)
self.pub_vel4 = rospy.Publisher('/device4/set_joint_state', JointState, queue_size=1)
self.pub_vel5 = rospy.Publisher('/device5/set_joint_state', JointState, queue_size=1)
self.pub_vel6 = rospy.Publisher('/device6/set_joint_state', JointState, queue_size=1)
# trying to get robot parameters from ROS parameter server
try:
locomotionParamDictionary = rospy.get_param('/espeleo_locomotion/')
except:
# sends a fatal message to the user
rospy.logfatal("Espeleo default parameters not found on ROS param server. Check if ./espeleo_locomotion/config/locomotion_parameters.yaml file exists, and if it is being correctly loaded into a launch file. Shutting down the node")
# shuts down the node
rospy.signal_shutdown('Shutting down the node.')
return None
# instantiating the locomotion library with the locomotion dictionary
self.esp_loc = bib_espeleo_locomotion(locomotionParamDictionary)
# defining the loop rate [Hz]cld
rate = rospy.Rate(locomotionParamDictionary['control_rate'])
# infinite loop
while not rospy.is_shutdown():
# rospy.loginfo('Num entrance: %s. Ancient: %s and new %s. The flag is %s' % (auxi, self.espeleo_input_last['linear'], self.espeleo_input['linear'], flag_dif))
# retrieves ROS time
ros_time = rospy.Time.now()
# sends the actual command to the library, obtaining the desired control
flag_sendControl,command_list = self.esp_loc.control_update(ros_time)
# tests if there is need for a command publish
if flag_sendControl:
# publishes the control command
state = JointState()
state.velocity = [command_list.movement_command[0].velocity]
self.pub_vel1.publish(state)
state.velocity = [command_list.movement_command[1].velocity]
self.pub_vel2.publish(state)
state.velocity = [command_list.movement_command[2].velocity]
self.pub_vel3.publish(state)
state.velocity = [command_list.movement_command[3].velocity]
self.pub_vel4.publish(state)
state.velocity = [command_list.movement_command[4].velocity]
self.pub_vel5.publish(state)
state.velocity = [command_list.movement_command[5].velocity]
self.pub_vel6.publish(state)
# sleeping
rate.sleep()
mp.process_frame_data(data)
time_keeper.sleep()
# Close the connection if the node is shut down
conn.close()
if __name__ == "__main__":
rospy.init_node("frame_connection_point")
# Get the port
port = rospy.get_param("~port", 0)
if port == 0:
rospy.logfatal("Failed to get param 'port'!")
sys.exit(1)
# Get the communication frequency
rate = rospy.get_param("~communication_frequency", 0)
if rate == 0:
rospy.logwarn(
"Failed to get param 'communication_frequency', using default of 10 Hz."
)
rate = 10
# Get the timeout value
timeout = rospy.get_param("~connection_timeout", 0)
if timeout == 0:
rospy.logwarn(
"Failed to get param 'connection_timeout', using default of 0.5s.")
def main():
rospy.init_node('vive_device_publisher')
vive_frame = rospy.get_param("~vive_frame")
topic_prefix = rospy.get_param("~topic_prefix", "/vive")
try:
vr = VRWrapper.VRWrapper()
except openvr.OpenVRError:
rospy.logfatal("Could not start OpenVR. Is SteamVR running?")
return
vibrate_continuous_service = rospy.Service(
topic_prefix + "/vibrate_controller_continuous",
VibrateControllerContinuous,
lambda req: vibrate_controller_continuous(vr, req))
vibrate_pulse_service = rospy.Service(
topic_prefix + "/vibrate_controller_pulse", VibrateControllerPulse,
lambda req: vibrate_controller_pulse(vr, req))
broadcaster = tf2_ros.TransformBroadcaster()
device_pose_publishers = {}
device_twist_publishers = {}
device_connected_publishers = {}
device_tracking_result_publishers = {}
device_valid_pose_publishers = {}
controller_state_publishers = {}
broadcasted_tracking_references = set()
try:
while not rospy.is_shutdown():
vr.find_new_devices()
vr.wait_update_poses()
for device in vr.get_devices():
full_device_name = topic_prefix + "/" + device.name
if device not in vr.get_tracking_references():
transform_msg = publish_device_frame(
device, broadcaster, full_device_name, vive_frame)
publish_device_pose(device, device_pose_publishers,
full_device_name, vive_frame)
publish_device_twist(device, device_twist_publishers,
full_device_name, transform_msg)
publish_is_connected(device, device_connected_publishers,
full_device_name)
publish_is_valid_pose(device, device_valid_pose_publishers,
full_device_name)
publish_tracking_result(device,
device_tracking_result_publishers,
full_device_name)
if device in vr.get_controllers():
publish_controller_state(device,
controller_state_publishers,
full_device_name)
if device in vr.get_tracking_references():
if device not in broadcasted_tracking_references:
publish_tracking_reference_static_frame(
device, broadcasted_tracking_references,
full_device_name, vive_frame)
finally:
vibrate_continuous_service.shutdown()
vibrate_pulse_service.shutdown()
vr.shutdown()
def start(self):
if not self.initialized:
return
self.executor.sense_initial_state()
while True:
if self.domain_costs_file != None:
rospy.loginfo("Planning with cost optimization!!")
start_time = time.time()
plan_available, optimization, plan, states = \
self.clingo_interface.get_plan_costs([self.initial_file,
self.query_file,
self.costs_file,
self.domain_costs_file])
duration = time.time() - start_time
if self.enable_learning and self.planning_times_file:
ptf = open(self.planning_times_file, "a")
ptf.write(str(duration) + "\n")
ptf.close()
else:
plan_available, optimization, plan, states = \
self.clingo_interface.get_plan([self.initial_file,
self.query_file])
if not plan_available:
rospy.logfatal("No plan found to complete task!")
break
rospy.loginfo("Found plan (optimization = %i): %s" %
(optimization, plan))
need_replan = False
for action in plan:
current_state = [
state for state in states if state.time == action.time
]
next_state = [
state for state in states if state.time == action.time + 1
]
start_time = rospy.get_time()
success, observations = \
self.executor.execute_action(action, next_state,
action.time+1)
duration = rospy.get_time() - start_time
if self.enable_learning and action.name == "approach":
if success:
# Check to see if we were beside some door in start
# state and the location
req = CostLearnerInterfaceRequest()
req.location = get_location(current_state)
req.door_from = get_adjacent_door(current_state)
if req.door_from:
req.door_to = str(action.value)
req.cost = duration
self.add_sample(req)
else:
rospy.loginfo("Executed unsuccessful approach action.")
for observation in observations:
if observation not in next_state:
rospy.logwarn(" Unexpected observation: " +
str(observation))
need_replan = True
break
if need_replan:
break
if need_replan:
self._construct_initial_state(current_state, observations)
continue
rospy.loginfo("Plan successful!")
self._send_finish()
break
motor, servo = line_follower.get_ECU_data()
servo_reg = servo / 180 * pi
servo_reg = min(max(servo_reg, -0.34), 0.34)
motor_reg = min(max(motor, -2.0), 2.0)
print("Servo", servo)
print("Servo_reg", servo_reg)
print("Motor", motor)
print("Motor_reg", motor_reg)
print("t", timestamp)
ads_msg = ADS()
ads_msg.header.seq += 1
ads_msg.header.stamp.secs = timestamp
ads_msg.drive.steering_angle = servo_reg
ads_msg.drive.speed = motor_reg
publisher.pub_vesc_msg(ads_msg)
rate.sleep()
if __name__ == '__main__':
try:
main()
except rospy.ROSInterruptException:
rospy.logfatal("ROS Interrupt. Shutting down line_follower node")
pass
def RunOneExtraction(fishEntries,
negEntries,
blobDir,
experimentDir,
outputPrefix,
videoId=None,
shapeDictFilename=None,
useSurfDescriptor=False,
useSiftDescriptor=False,
useOpponentSurf=False,
useCInvariantSurf=False,
saveIndex=False,
hessThresh=400.0,
numNegBlobs=5.0,
options=None):
# Split up the training and test entries
if videoId is not None:
trainEntries = filter(lambda x: x[2] <> videoId, fishEntries)
trainNegEntries = filter(lambda x: x[1] <> videoId, negEntries)
testEntries = filter(lambda x: x[2] == videoId, fishEntries)
testBlobFiles = sorted(set([x[0] for x in testEntries]))
rospy.loginfo('Testing with entries of video id ' + videoId)
else:
rospy.logfatal('Must have a video id. We are done')
return
runName = GetRandomRunName()
# Now build up a list of training and negative entries entries to be
# used by the BuildFishIndex program.
labelFilename = os.path.join(experimentDir, 'train_%s.label' % runName)
labelFile = open(labelFilename, 'w')
try:
for blobFile, blobId, trainVideoId, label in trainEntries:
labelFile.write('%s,%i,%s\n' % (blobFile, blobId, label))
WriteNegativeExamples(labelFile, trainNegEntries,
numNegBlobs * len(trainEntries), blobDir)
finally:
labelFile.close()
# Setup the parameters used by the routines
paramList = [('color_dict_filename', ''), ('color_converter', ''),
('color_frac', 0), ('shape_dict_filename', shapeDictFilename),
('surf_detector', True),
('surf_hessian_threshold', hessThresh), ('surf_octaves', 3),
('surf_octave_layers', 4), ('surf_extended', False),
('surf_descriptor', useSurfDescriptor),
('sift_descriptor', useSiftDescriptor), ('shape_weight', 1.0),
('min_shape_val', 0.01), ('min_color_val', 0.01),
('min_score', 0.005),
('opponent_color_surf', useOpponentSurf),
('cinvariant_color_surf', useCInvariantSurf),
('use_bounding_box', True), ('bounding_box_expansion', 1.0),
('geometric_rerank', False),
('geo_rerank_inlier_thresh', 3.0)]
rospy.loginfo('Using parameters: ' + str(paramList))
indexFilename = os.path.join(experimentDir, 'train_%s.index' % runName)
# Build the search index for the fish
BuildFishIndex(labelFilename, indexFilename, blobDir, paramList)
# Open up the bag which we will output stuff to
resultFile = '%s_%s.bag' % (outputPrefix, videoId)
outBag = rosbag.Bag(os.path.join(experimentDir, resultFile), 'w', 'bz2')
# Start a SpeciesIDNode to serve the requests and grab the service
# hook to it
speciesIdProc, speciesIdParams, ClassifyGrid = StartSpeciesIDNode(
experimentDir, runName, outBag, rospy.Time.from_sec(0.0), paramList,
indexFilename)
try:
blobSerializer = Blob.BlobSerializer()
cvBridge = cv_bridge.CvBridge()
npBridge = cv_bridge.NumpyBridge()
curFrameNum = 0
for blobFile in testBlobFiles:
# The result message to store
resultMsg = SpeciesIDScoring()
# Read the blob file
blobStream = open(os.path.join(blobDir, blobFile), 'r')
try:
blobs, imageFile = blobSerializer.Deserialize(
blobStream, blobDir)
finally:
blobStream.close()
rospy.loginfo('Processing %s' % imageFile)
# Fill out the ground truth in the result message
curEntries = filter(lambda x: x[0] == blobFile, testEntries)
curEntries = sorted(curEntries, key=lambda x: x[1])
curBlobId = 0
for blob in blobs:
curEntry = [x for x in curEntries if x[1] == curBlobId]
if len(curEntry) == 1 and int(curEntry[0][3]) > 3:
# The human label says it's a fish so add the label to the results
resultMsg.labels.append(int(curEntries[curBlobId][3]))
bbox = RegionOfInterest()
bbox.x_offset = blob.minX
bbox.y_offset = blob.minY
bbox.height = blob.maxY - blob.minY
bbox.width = blob.maxX - blob.minX
resultMsg.regions.append(bbox)
curBlobId += 1
# Open up the image and package it into a message
cvImage = cv2.imread(imageFile)
imgMsg = cvBridge.cv_to_imgmsg(cvImage, 'bgr8')
imgMsg.header.stamp = rospy.Time.from_sec(curFrameNum)
imgMsg.header.seq = curFrameNum
# Build up the request
request = DetectObjectGrid()
request.header = imgMsg.header
request.image = imgMsg
request.grid.minX = 0
request.grid.minY = 0
request.grid.minH = options.min_region_height
request.grid.minW = options.min_region_width
request.grid.strideX = options.win_stride
request.grid.strideY = options.win_stride
request.grid.strideH = options.scale_stride
request.grid.strideW = options.scale_stride
request.grid.fixAspect = False
request.mask.encoding = "8U"
# Process this image
response = ClassifyGrid(request)
# Build up the result message to store
resultMsg.image = imageFile
resultMsg.grid = response.grid
resultMsg.confidence = response.confidence
resultMsg.top_label = response.top_label
resultMsg.not_fish_confidence = response.not_fish_confidence
resultMsg.processing_time = response.processing_time
# Record the result in the bag file
outBag.write('results', resultMsg, request.header.stamp)
outBag.flush()
curFrameNum += 1
finally:
outBag.close()
speciesIdParams.RemoveParameters()
ClassifyGrid.close()
speciesIdProc.send_signal(2)
tries = 0
while speciesIdProc.returncode == None and tries < 10:
speciesIdProc.poll()
tries = tries + 1
time.sleep(1)
if speciesIdProc.returncode == None:
speciesIdProc.kill()
if not options.save_index:
os.remove(indexFilename)
gc.collect()
mask_out=False,
update_parameter_server=args.update)
tmp = []
[tmp.extend(_) for _ in args.calibration]
cal_files = []
[cal_files.extend(glob.glob(_)) for _ in tmp]
rospy.loginfo('cal_files: %r' % cal_files)
if len(cal_files):
#multiple bag files
for c in cal_files:
fn = decode_url(c)
assert os.path.exists(fn)
cal.load(fn, args.no_wait_display_server)
else:
rospy.logfatal('No data. Specify inputs with --calibration <file.bag>')
rospy.signal_shutdown('no data')
sys.exit(1)
if args.smooth:
cal.smooth(args.smooth)
cal.do_exr(args.interpolation, args.luminance, args.gamma,
args.blend_curve)
if cal.visualize:
plt.show()
rospy.loginfo('finished')
rospy.spin()
def __init__(self):
# Parse parameters
target_dir = rospy.get_param('~target_directory')
self.method = rospy.get_param('~method', 'single')
self.ns_voxblox = rospy.get_param('~ns_eval_voxblox_node',
'/eval_voxblox_node')
self.evaluate = rospy.get_param('~evaluate', True)
self.evaluate_volume = rospy.get_param('~evaluate_volume', False)
self.create_plots = rospy.get_param('~create_plots', True)
self.show_plots = rospy.get_param(
'~show_plots', False) # Auxiliary param, prob removed later
self.create_meshes = rospy.get_param('~create_meshes', True)
self.series = rospy.get_param(
'~series', False) # True: skip single evaluation and create
# series evaluation data and plots for all runs in the target directory
self.clear_voxblox_maps = rospy.get_param(
'~clear_voxblox_maps',
False) # rm all maps after eval (disk space!)
self.unobservable_points_pct = rospy.get_param(
'~unobservable_points_pct', 0.0) # Exlude unobservable points
# from the plots (in percent of total)
# Check for valid params
methods = {
'single': 'single',
'recent': 'recent',
'all': 'all'
} # Dictionary of implemented models
selected = methods.get(self.method, 'NotFound')
if selected == 'NotFound':
warning = "Unknown method '" + self.method + \
"'. Implemented are: " + \
"".join(["'" + m + "', " for m in methods])
rospy.logfatal(warning[:-2])
sys.exit(-1)
else:
self.method = selected
# Setup
self.eval_log_file = None
rospy.wait_for_service(self.ns_voxblox + "/evaluate")
self.eval_voxblox_srv = rospy.ServiceProxy(
self.ns_voxblox + "/evaluate", Empty)
# Evaluate
if self.series:
self.evaluate_series(target_dir)
elif self.method == 'single':
self.run_single_evaluation(target_dir)
elif self.method == 'recent':
dir_expression = re.compile(
r'\d{8}_\d{6}') # Only check the default names
subdirs = [
o for o in os.listdir(target_dir)
if os.path.isdir(os.path.join(target_dir, o))
and dir_expression.match(o)
]
subdirs.sort(reverse=True)
if len(subdirs) == 0:
rospy.loginfo(
"No recent directories in target dir '%s' to evaluate.",
target_dir)
sys.exit(-1)
self.run_single_evaluation(os.path.join(target_dir, subdirs[0]))
elif self.method == 'all':
subdirs = [
o for o in os.listdir(target_dir)
if os.path.isdir(os.path.join(target_dir, o))
]
for subdir in subdirs:
self.run_single_evaluation(os.path.join(target_dir, subdir))
rospy.loginfo(
"\n" + "*" * 53 +
"\n* Evaluation completed successfully, shutting down. *\n" +
"*" * 53)
def __init__(self):
self.lock = threading.Lock()
self.ERRORS = {0x0000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "Normal"),
0x0001: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M1 over current"),
0x0002: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M2 over current"),
0x0004: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Emergency Stop"),
0x0008: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Temperature1"),
0x0010: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Temperature2"),
0x0020: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Main batt voltage high"),
0x0040: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Logic batt voltage high"),
0x0080: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Logic batt voltage low"),
0x0100: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M1 driver fault"),
0x0200: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M2 driver fault"),
0x0400: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Main batt voltage high"),
0x0800: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Main batt voltage low"),
0x1000: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Temperature1"),
0x2000: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Temperature2"),
0x4000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "M1 home"),
0x8000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "M2 home")}
rospy.init_node("roboclaw_node",log_level=rospy.DEBUG)
rospy.on_shutdown(self.shutdown)
rospy.loginfo("Connecting to roboclaw")
dev_name = rospy.get_param("~dev", "/dev/roboclaw")
baud_rate = int(rospy.get_param("~baud", "460800"))
self.wheels_speeds_pub = rospy.Publisher('/motors/commanded_speeds', Wheels_speeds, queue_size=1)
self.motors_currents_pub = rospy.Publisher('/motors/read_currents', Motors_currents, queue_size=1)
self.address = int(rospy.get_param("~address", "128"))
if self.address > 0x87 or self.address < 0x80:
rospy.logfatal("Address out of range")
rospy.signal_shutdown("Address out of range")
# TODO need someway to check if address is correct
try:
roboclaw.Open(dev_name, baud_rate)
except Exception as e:
rospy.logfatal("Could not connect to Roboclaw")
rospy.logdebug(e)
rospy.signal_shutdown("Could not connect to Roboclaw")
self.updater = diagnostic_updater.Updater()
self.updater.setHardwareID("Roboclaw")
self.updater.add(diagnostic_updater.
FunctionDiagnosticTask("Vitals", self.check_vitals))
try:
version = roboclaw.ReadVersion(self.address)
except Exception as e:
rospy.logwarn("Problem getting roboclaw version")
rospy.logdebug(e)
pass
if not version[0]:
rospy.logwarn("Could not get version from roboclaw")
else:
rospy.logdebug(repr(version[1]))
roboclaw.SpeedM1M2(self.address, 0, 0)
roboclaw.ResetEncoders(self.address)
self.MAX_ABS_LINEAR_SPEED = float(rospy.get_param("~max_abs_linear_speed", "1.0"))
self.MAX_ABS_ANGULAR_SPEED = float(rospy.get_param("~max_abs_angular_speed", "1.0"))
self.TICKS_PER_METER = float(rospy.get_param("~ticks_per_meter", "4342.2"))
self.BASE_WIDTH = float(rospy.get_param("~base_width", "0.315"))
self.ACC_LIM = float(rospy.get_param("~acc_lim", "0.1"))
self.encodm = EncoderOdom(self.TICKS_PER_METER, self.BASE_WIDTH)
self.last_set_speed_time = rospy.get_rostime()
rospy.Subscriber("cmd_vel", Twist, self.cmd_vel_callback, queue_size=1)
rospy.sleep(1)
rospy.logdebug("dev %s", dev_name)
rospy.logdebug("baud %d", baud_rate)
rospy.logdebug("address %d", self.address)
rospy.logdebug("max_abs_linear_speed %f", self.MAX_ABS_LINEAR_SPEED)
rospy.logdebug("max_abs_angular_speed %f", self.MAX_ABS_ANGULAR_SPEED)
rospy.logdebug("ticks_per_meter %f", self.TICKS_PER_METER)
rospy.logdebug("base_width %f", self.BASE_WIDTH)
def create_map(self, random_map: bool, x_size: int, y_size: int):
self.remove_models()
if random_map:
normal_prob = 1 - self._chargers_prob - self._wheels_lubrication_prob
map_fields = np.random.choice(list(
range(len(FieldType.get_field_names()))), (y_size, x_size),
p=[
normal_prob, self._chargers_prob,
self._wheels_lubrication_prob
])
else:
pkg_path = rospkg.RosPack().get_path('environment')
try:
map_fields: np.ndarray = np.loadtxt(os.path.join(
pkg_path, 'data', 'map_fields.txt'),
dtype=int)
except Exception as e:
rospy.logfatal(
'File with map fields types not available. Please provide map or request random generated map. Exiting...'
)
rospy.signal_shutdown('There is no global map available')
sys.exit(1)
cnts = {
getattr(FieldType, field_type): 0
for field_type in FieldType.get_field_names()
}
field_names = {
field_type: field_name
for field_type, field_name in enumerate(
FieldType.get_field_names())
}
for i in range(map_fields.shape[0]):
for j in range(map_fields.shape[1]):
field_type = map_fields[i, j]
if i == map_fields.shape[0] // 2 and j == map_fields.shape[
1] // 2:
field_type = FieldType.NORMAL
field = Field(
x=int(i - map_fields.shape[0] // 2),
y=int(j - map_fields.shape[1] // 2),
field_type=field_type,
name=f'{field_names[field_type]}-{cnts[field_type]}')
self._world_map.append(field)
# Spawning in gazebo part
if field.field_type == FieldType.NORMAL:
# Spawn model only for non normal fields (to save time)
cnts[FieldType.NORMAL] += 1
continue
# pose = Pose()
# pose.position.x = field.x
# pose.position.y = field.y
# req = SpawnModelRequest()
# req.model_name = f'{GlobalMapManager.FIELD_NAMES[field.field_type]}-{cnts[field.field_type]}'
# req.model_xml = models_sdf[field.field_type]
# req.initial_pose = pose
# req.reference_frame = 'world'
# spawn_sdf_model.call(req)
cnts[field.field_type] += 1
self.publish()
self.publish_marker_array()
def manual_override(self, msg):
if msg.axes[4] != 1:
rospy.logfatal('Manual Override')
rospy.signal_shutdown('Manual Override')
os.system("rosnode kill /reach_target")
rospy.init_node('evaluator_node')
rospy.loginfo("BUM Evaluator ROS node started!")
# Initialize subscribers
rospy.Subscriber("bum/tuple", Tuple, tuple_callback)
#rospy.Subscriber("bum/evidence", Evidence, evidence_callback)
gcd_filename = ""
gt_log_file = ""
exec_log_file = ""
# Get parameters
try:
gcd_filename = rospy.get_param("bum_ros/gcd_file")
except KeyError:
rospy.logfatal("Could not get GCD file name parameter")
exit()
try:
gt_log_file = rospy.get_param("bum_ros/gt_log_file")
except KeyError:
rospy.logfatal("Could not get gt log file name parameter")
exit()
try:
exec_log_file = rospy.get_param("bum_ros/exec_log_file")
except KeyError:
rospy.logfatal("Could not get exec log file name parameter")
exit()
# Read GCD file
with open(gcd_filename, "r") as gcd_file:
GCD = yaml.load(gcd_file)
def __init__(self):
'''
Default image_topic for:
Basler ace cameras with camera_aravis driver: camera/image_raw
Pt Grey Firefly cameras with pt grey driver : camera/image_mono
'''
# default parameters (parameter server overides them)
self.params = {
'image_topic': 'camera/image_mono',
'min_persistence_to_draw': 10,
'max_frames_to_draw': 50,
'camera_encoding':
'mono8', # fireflies are bgr8, basler gige cams are mono8
'roi_l': 0,
'roi_r': -1,
'roi_b': 0,
'roi_t': -1,
'~circular_mask_x': None,
'~circular_mask_y': None,
'~circular_mask_r': None,
'~roi_points': None,
'~detect_tracking_pipelines': False, # rename?
'save_demo': False
}
for parameter, default_value in self.params.items():
use_default = False
try:
if parameter[0] == '~':
value = rospy.get_param(parameter)
else:
try:
value = rospy.get_param('multi_tracker/liveviewer/' +
parameter)
except KeyError:
value = rospy.get_param('multi_tracker/tracker/' +
parameter)
# for maintaining backwards compatibility w/ Floris' config files that
# would use 'none' to signal default should be used.
# may break some future use cases.
if self.params[parameter] is None:
if isinstance(value, str):
use_default = True
except KeyError:
use_default = True
if use_default:
rospy.loginfo(rospy.get_name() + ' using default parameter: ' + \
parameter + ' = ' + str(default_value))
value = default_value
if parameter[0] == '~':
del self.params[parameter]
parameter = parameter[1:]
self.params[parameter] = value
# If we are tracking an experiment that is being played back
# ("retracking"), we don't want to further restrict roi, and we will
# always use the same camera topic.
if rospy.get_param('/use_sim_time', False):
self.params['image_topic'] = 'camera/image_raw'
self.params['roi_l'] = 0
self.params['roi_r'] = -1
self.params['roi_b'] = 0
self.params['roi_t'] = -1
self.videowriter = None
if self.params['save_demo']:
# TODO include timestamp?
self.video_filename = 'tracking_demo.avi'
self.desired_frame_rate = 30.0
self.mode = 'color'
rospy.init_node('liveviewer')
self.tracked_trajectories = {}
self.clear_rois()
# should i allow both roi_* and detect_tracking_pipelines?
roi_params = [
'circular_mask_x', 'circular_mask_y', 'circular_mask_r',
'roi_points'
]
if self.params['detect_tracking_pipelines']:
if any(map(lambda x: self.params[x] != None, roi_params)):
rospy.logfatal('liveviewer: roi parameters other than rectangular roi_[l/r/b/t] ' + \
'are not supported when detect_tracking_pipelines is set to True')
# add single rois defined in params to instance variables for consistency later
# roi_* are still handled differently, and can be set alongside tracker roi_*'s
# which are now private (node specific) parameters
else:
n = rospy.get_name()
try:
node_num = int(n.split('_')[-1])
except ValueError:
node_num = 1
circle_param_names = [
'circular_mask_x', 'circular_mask_y', 'circular_mask_r'
]
self.add_roi(node_num, circle_param_names)
poly_param_names = ['roi_points']
# TODO break roi stuff and subscription initiation
# into one function?
self.add_roi(node_num, poly_param_names)
# otherwise, we need to start subscriptions as we
# detect other pipelines in our namespace
self.start_subscribers(node_num)
# TODO put in dict above? (& similar lines in other files)
# displays extra information about trajectory predictions / associations if True
self.debug = rospy.get_param('multi_tracker/liveviewer/debug', False)
# initialize display
self.window_name = 'liveviewer'
self.cvbridge = CvBridge()
if self.params['detect_tracking_pipelines']:
self.contours = dict()
else:
self.contours = None
self.window_initiated = False
self.image_mask = None
# Subscriptions - subscribe to images, and tracked objects
sizeImage = 128 + 1024 * 1024 * 3 # Size of header + data.
self.subImage = rospy.Subscriber(self.params['image_topic'],
Image,
self.image_callback,
queue_size=5,
buff_size=2 * sizeImage,
tcp_nodelay=True)
# for adding images to background
add_image_to_background_service_name = 'multi_tracker/tracker/add_image_to_background'
rospy.wait_for_service(add_image_to_background_service_name)
try:
self.add_image_to_background = rospy.ServiceProxy(
add_image_to_background_service_name,
addImageToBackgroundService)
except:
rospy.logerr(
'could not connect to add image to background service - is tracker running?'
)
imagePath = rospy.get_param("dataset_path")
self.benchmarkTest(PKG_PATH + imagePath, publisherTopic,
subscriberTopic, benchmarkFlag)
if __name__ == "__main__":
# Initialize the node
rospy.init_node(NAME, anonymous=True, log_level=rospy.DEBUG)
publisherTopic = None
publisherMessagePackage = None
publisherMessageType = None
if rospy.has_param("kinect/topic_name"):
publisherTopic = rospy.get_param("kinect/topic_name")
else:
rospy.logfatal("Could not find the Image Topic name parameter!")
if rospy.has_param("publisherMessagePackage"):
publisherMessagePackage = rospy.get_param("publisherMessagePackage")
else:
rospy.logfatal("Could not find the Message Package parameter!")
if rospy.has_param("publisherMessageType"):
publisherMessageType = rospy.get_param("publisherMessageType")
else:
rospy.logfatal("Could not find the Message Type parameter!")
subscriberTopic = None
subscriberMessagePackage = None
subscriberMessageType = None
if rospy.has_param("subscriberTopic"):
subscriberTopic = rospy.get_param("subscriberTopic")
n_episodes_running = rospy.get_param('/moving_cube/episodes_running')
n_win_ticks = rospy.get_param('/moving_cube/n_win_ticks')
min_episodes = rospy.get_param('/moving_cube/min_episodes')
max_env_steps = None
gamma = rospy.get_param('/moving_cube/gamma')
epsilon = rospy.get_param('/moving_cube/epsilon')
epsilon_min = rospy.get_param('/moving_cube/epsilon_min')
epsilon_log_decay = rospy.get_param('/moving_cube/epsilon_decay')
alpha = rospy.get_param('/moving_cube/alpha')
alpha_decay = rospy.get_param('/moving_cube/alpha_decay')
batch_size = rospy.get_param('/moving_cube/batch_size')
monitor = rospy.get_param('/moving_cube/monitor')
quiet = rospy.get_param('/moving_cube/quiet')
agent = DQNRobotSolver(environment_name, n_observations, n_actions,
n_win_ticks, min_episodes, max_env_steps, gamma,
epsilon, epsilon_min, epsilon_log_decay, alpha,
alpha_decay, batch_size, monitor, quiet)
#agent.run(num_episodes=n_episodes_training, do_train=True)
rospack = rospkg.RosPack()
pkg_path = rospack.get_path(rospackage_name)
outdir = pkg_path + '/models.1'
if not os.path.exists(outdir):
os.makedirs(outdir)
rospy.logfatal("Created folder=" + str(outdir))
#agent.save(model_name, outdir)
agent.load(model_name, outdir)
agent.run(num_episodes=n_episodes_running, do_train=False)
def run(self):
rospy.loginfo(self.logname + 'Initializing...')
# Get parameters from launch file
keyphrase_dir = rospy.get_param('key_phrase_dir', "")
keyphrase_1 = rospy.get_param('key_phrase_1', "")
keyphrase_1_path = keyphrase_dir + '/' + keyphrase_1
keyphrase_2 = rospy.get_param('key_phrase_2', "")
keyphrase_2_path = keyphrase_dir + '/' + keyphrase_2
# Hotword Sensitivity: larger value is more sensitive (good for quiet room)
hotword_sensitivity = 0.50 # rospy.get_param('hotword_sensitivity', 0.80) # 0.38?
apply_frontend = rospy.get_param('apply_frontend',
True) # Frontend filtering
proxyUrl = rospy.get_param('proxyUrl', "") # default to no proxy
rospy.loginfo(self.logname + "KEYPHRASE INFO: ")
rospy.loginfo(" keyphrase_dir: " + keyphrase_dir)
rospy.loginfo(" keyphrase_1: " + keyphrase_1)
rospy.loginfo(" keyphrase_2: " + keyphrase_2)
rospy.loginfo("========================================")
if (keyphrase_1 == "") or (keyphrase_2 == ""):
rospy.logfatal("========================================")
rospy.logfatal("MISSING KEYPHRASE! SHUTTING DOWN!")
rospy.logfatal("========================================")
return
# Check for Internet connection (fail early unstead of first time we try to use a cloud service)
if not self.internet_available():
rospy.logfatal("========================================")
rospy.logfatal("INTERNET NOT AVAILABLE, SHUTTING DOWN!")
rospy.logfatal("========================================")
return
#=====================================================================================
# Google Assistant Setup
rospy.loginfo('Initializing Google Assistant...')
# initialize parameters (these are usually passed on command line in sample)
verbose = False
credentials = '/home/system/.config/google-oauthlib-tool/credentials.json'
device_config = '/home/system/.config/googlesamples-assistant/device_config.json'
# Setup logging.
#logging.basicConfig(level=logging.DEBUG if verbose else logging.INFO)
# Load OAuth 2.0 credentials.
rospy.loginfo('Loading OAuth 2.0 credentials...')
try:
with open(credentials, 'r') as f:
credentials = google.oauth2.credentials.Credentials(
token=None, **json.load(f))
http_request = google.auth.transport.requests.Request()
credentials.refresh(http_request)
except Exception as e:
rospy.logfatal('Error loading credentials: %s', e)
rospy.logfatal('Run google-oauthlib-tool to initialize '
'new OAuth 2.0 credentials.')
sys.exit(-1)
# Create an authorized gRPC channel.
self.grpc_channel = google.auth.transport.grpc.secure_authorized_channel(
credentials, http_request, ASSISTANT_API_ENDPOINT)
rospy.loginfo('Connecting to %s', ASSISTANT_API_ENDPOINT)
# get device info from the config file
try:
with open(device_config) as f:
device = json.load(f)
self.device_id = device['id']
self.device_model_id = device['model_id']
rospy.loginfo("Using device model %s and device id %s",
self.device_model_id, self.device_id)
except Exception as e:
rospy.logfatal('Device config not found: %s' % e)
sys.exit(-1)
#===================================================================
# DEVICE HANDLERS. See resources/actions .json to add more actions
rospy.loginfo('Setting up Google Assistant device handlers...')
self.device_handler = device_helpers.DeviceRequestHandler(
self.device_id)
@self.device_handler.command('com.shinselrobots.commands.move')
def move(move_direction, amount):
if amount == '$amount':
amount = ''
rospy.loginfo('******> Got Move Command [%s] [%s] ',
move_direction, amount)
move_speed = '0.5'
move_command = '0.75' # Normal Move (meters)
if move_direction == 'BACKWARD':
move_command = '-0.5' # Normal Backward Move (meters)
if amount == 'SMALL':
move_command = '-0.25' # Small Move
elif amount == 'LARGE':
move_command = '-0.75' # Large Move
else:
move_command = '0.75' # Normal Forward Move (meters)
if amount == 'SMALL':
move_command = '0.25' # Small Move
elif amount == 'LARGE':
move_command = '1.0' # Large Move
self.handle_behavior_command('MOVE', move_command, move_speed,
('ok, moving ' + move_direction))
@self.device_handler.command('com.shinselrobots.commands.turn')
def turn(turn_direction, amount):
if amount == '$amount':
amount = ''
rospy.loginfo('******> Got Turn Command [%s] [%s] ',
turn_direction, amount)
turn_speed = '2.0'
turn_command = '45' # Normal Turn
if amount == 'SMALL':
turn_command = '30' # Small Turn
elif amount == 'LARGE':
turn_command = '90' # Large Turn
if turn_direction == 'RIGHT':
turn_command = '-' + turn_command # Negative Turn
self.handle_behavior_command('TURN', turn_command, turn_speed,
('turning ' + turn_direction))
@self.device_handler.command('com.shinselrobots.commands.spin_left')
def spin_left(param1):
turn_speed = '2.0'
self.handle_behavior_command('TURN', '180', turn_speed,
'spinning left')
@self.device_handler.command('com.shinselrobots.commands.spin_right')
def spin_right(param1):
turn_speed = '2.0'
self.handle_behavior_command('TURN', '-180', turn_speed,
'spinning right')
@self.device_handler.command('com.shinselrobots.commands.stop')
def stop(param1):
self.handle_behavior_command('STOP', '', '', 'stopping')
@self.device_handler.command('com.shinselrobots.commands.sleep')
def sleep(param1):
self.handle_behavior_command('SLEEP', '', '', 'ok')
@self.device_handler.command('com.shinselrobots.commands.wake')
def wake(param1):
self.handle_behavior_command('WAKEUP', '', '', 'ok, waking up')
@self.device_handler.command('com.shinselrobots.commands.intro')
def intro(param1):
self.handle_behavior_command('INTRO', '', '', 'ok, sure')
@self.device_handler.command('com.shinselrobots.commands.hands_up')
def hands_up(param1):
self.handle_behavior_command('HANDS_UP', '', '', 'ok')
@self.device_handler.command('com.shinselrobots.commands.arms_home')
def arms_home(param1):
self.handle_behavior_command('ARMS_HOME', '', '', 'ok')
@self.device_handler.command('com.shinselrobots.commands.follow')
def follow(param1):
self.handle_behavior_command('FOLLOW_ME', '', '',
'ok, I will follow you')
@self.device_handler.command(
'com.shinselrobots.commands.microphone_off')
def microphone_off(param1):
rospy.loginfo('**********************************************')
rospy.loginfo(' Google Assistant Command: MICROPHONE OFF')
rospy.loginfo('**********************************************')
if not use_google_assistant_voice:
# assistant not acknowledging the command, so we do it
self.local_voice_say_text("Ok, I will stop listening")
self.mic_user_enable_pub.publish(False)
@self.device_handler.command('com.shinselrobots.commands.microphone_on'
)
def microphone_on(param1):
rospy.loginfo('**********************************************')
rospy.loginfo(' Google Assistant Command: MICROPHONE ON')
rospy.loginfo('**********************************************')
# no action needed, but send just in case...
self.mic_user_enable_pub.publish(True)
@self.device_handler.command('com.shinselrobots.commands.toggle_lights'
)
def toggle_lights(param1):
rospy.loginfo('**********************************************')
rospy.loginfo(' Google Assistant Command: toggle lights')
rospy.loginfo('**********************************************')
# TODO Fix this temp Kludge, to use some global state (param server, or messaging?)
text_to_say = ""
light_mode = 0
if self.arm_lights_on:
rospy.loginfo('SPEECH: TURN LIGHTS OFF (Toggle)')
light_mode = 0
text_to_say = "entering stealth mode"
self.arm_lights_on = False
else:
rospy.loginfo('SPEECH: TURN LIGHTS ON (Toggle)')
light_mode = 1
text_to_say = "Doesnt this look cool?"
self.arm_lights_on = True
rospy.loginfo('DEBUG2: *********')
self.pub_light_mode.publish(light_mode)
rospy.loginfo('DEBUG3: *********')
if not use_google_assistant_voice:
# assistant not acknowledging the command, so we do it
self.local_voice_say_text(text_to_say)
rospy.loginfo('DEBUG: DONE WITH TOGGLE LIGHTS *********')
@self.device_handler.command('com.shinselrobots.commands.sing_believer'
)
def sing_believer(param1):
self.handle_behavior_command('RUN_SCRIPT', 'believer', '',
'all right')
@self.device_handler.command('com.shinselrobots.commands.bow')
def bow(param1):
self.handle_behavior_command('BOW', '', '', 'ok')
@self.device_handler.command(
'com.shinselrobots.commands.who_is_president')
def who_is_president(param1):
rospy.loginfo('**********************************************')
rospy.loginfo(' Google Assistant Command: Who is President')
rospy.loginfo('**********************************************')
if not use_google_assistant_voice:
# assistant not acknowledging the command, so we do it
self.local_voice_say_text(
"According to the internet, the president is Donald Trump. but, that might just be fake news"
)
@self.device_handler.command('com.shinselrobots.commands.wave')
def wave(param1):
self.handle_behavior_command('WAVE', '', '', '')
@self.device_handler.command('com.shinselrobots.commands.head_center')
def head_center(param1):
self.handle_behavior_command('HEAD_CENTER', '', '', 'ok')
@self.device_handler.command('com.shinselrobots.commands.tell_time')
def tell_time(param1):
self.handle_behavior_command('TELL_TIME', '', '', 'let me check')
@self.device_handler.command('com.shinselrobots.commands.joke')
def joke(param1):
self.handle_behavior_command('TELL_JOKE', param1, '', 'ok')
@self.device_handler.command('com.shinselrobots.commands.tell_age')
def tell_age(param1):
rospy.loginfo('**********************************************')
rospy.loginfo(' Google Assistant Command: TELL AGE')
rospy.loginfo(' Param1 = %s', param1)
rospy.loginfo('**********************************************')
if not use_google_assistant_voice:
# assistant not acknowledging the command, so we do it
self.local_voice_say_text(
"I have been under construction for about a year. My hardware is nearly complete, but my software is constantly evolving"
)
@self.device_handler.command('com.shinselrobots.commands.tell_function'
)
def tell_function(param1):
rospy.loginfo('**********************************************')
rospy.loginfo(' Google Assistant Command: TELL FUNCTION')
rospy.loginfo(' Param1 = %s', param1)
rospy.loginfo('**********************************************')
if not use_google_assistant_voice:
# assistant not acknowledging the command, so we do it
self.local_voice_say_text(
"i am currently focused on human interaction. but I hope to gain object manipulation capabilities soon. because the ultimate goal of any robot is to fetch beer"
)
@self.device_handler.command('com.shinselrobots.commands.tell_size')
def tell_size(param1):
rospy.loginfo('**********************************************')
rospy.loginfo(' Google Assistant Command: TELL SIZE')
rospy.loginfo(' Param1 = %s', param1)
rospy.loginfo('**********************************************')
if not use_google_assistant_voice:
# assistant not acknowledging the command, so we do it
self.local_voice_say_text(
"I am 4 foot 3 inches tall, and I weigh about 75 pounds")
@self.device_handler.command('com.shinselrobots.commands.tell_sex')
def tell_sex(param1):
rospy.loginfo('**********************************************')
rospy.loginfo(' Google Assistant Command: TELL SEX')
rospy.loginfo(' Param1 = %s', param1)
rospy.loginfo('**********************************************')
if not use_google_assistant_voice:
# assistant not acknowledging the command, so we do it
self.local_voice_say_text("i feel like i am a boy robot")
rospy.loginfo('Google Assistant *** Initialization complete ***')
self.TTS_client = None
if not use_google_assistant_voice:
# check for service as late as possible to give service time to start up
rospy.loginfo(self.logname +
"Initializing LOCAL Text to Speech...")
client = actionlib.SimpleActionClient(
"/speech_service", audio_and_speech_common.msg.speechAction)
if (False == client.wait_for_server(rospy.Duration(10, 0))):
rospy.logerr(
self.logname +
"WARNING!!! Text to Speech server is not available, skipping"
)
else:
self.TTS_client = client
rospy.loginfo(self.logname + "LOCAL Text to speech server ready")
#=====================================================================================
rospy.loginfo(self.logname + "Starting detector...")
keyphrase_models = [keyphrase_1_path, keyphrase_2_path]
detector = snowboydecoder.HotwordDetector(
keyphrase_models,
sensitivity=hotword_sensitivity,
apply_frontend=False)
rospy.loginfo(self.logname + "Listening for keyphrase...")
# main loop - This funciton will block until ros shutdown
keyword_detected_callbacks = [
self.detectedCallback1, self.detectedCallback2
]
detector.start(detected_callback=keyword_detected_callbacks,
audio_recorder_callback=self.audioRecorderCallback,
interrupt_check=interrupt_callback,
mic_pause=mic_pause_callback,
sleep_time=0.01,
silent_count_threshold=15,
recording_timeout=10) # (blocks? 10 = about 4 seconds)
# Tune recording_timeout for max expected command. Default of 100 is a LONG time!
detector.terminate()
def __init__(self, file):
super(BeltParser, self).__init__()
rospy.logdebug("Parsing belt definition...")
root = ET.parse(file).getroot()
required = ["max_range", "angle", "precision", "scale_responsive"]
# parse params
if required and root.find("params") is None:
msg = "Can't parse belt definition file: a 'params' element is required. Shutting down."
rospy.logfatal(msg)
raise rospy.ROSInitException(msg)
self.Params = {
p.attrib["type"]: {c.tag: float(c.text)
for c in p}
for p in root.find("params")
}
rospy.loginfo(self.Params)
for p in required:
for s in self.Params:
if p not in self.Params[s]:
msg = "Can't parse belt definition: a '{}' element is required in the parameters. Shutting down."\
.format(p)
rospy.logfatal(msg)
raise rospy.ROSInitException(msg)
# parse sensors
if root.find("sensors") is None:
msg = "Can't parse belt definition: a 'sensors' element is required. Shutting down."
rospy.logfatal(msg)
raise rospy.ROSInitException(msg)
sensors = []
for sensor in root.find("sensors"):
if "id" not in sensor.attrib:
rospy.logerr(
"Can't parse sensor definition: a 'id' attribute is required. Skipping this sensor."
)
continue
required = ["x", "y", "a"]
for p in required:
if sensor.find(p) is None:
rospy.logerr(
"Can't parse sensor definition: a '{}' element is required. Skipping this sensor."
.format(p))
if "type" not in sensor.attrib:
rospy.logerr(
"Can't parse sensor definition: a 'type' attribute is required. Skipping this sensor."
)
continue
if "layer" not in sensor.attrib:
rospy.logerr(
"Can't parse sensor definition: a 'layer' attribute is required. Skipping this sensor."
)
continue
if sensor.attrib["type"] not in self.Params:
rospy.logerr(
"Can't parse sensor definition: {} sensor type is not defined. Skipping this sensor."
.format(sensor.attrib["type"]))
continue
sensors.append({
"id": sensor.attrib["id"],
"x": float(sensor.find("x").text),
"y": float(sensor.find("y").text),
"a": float(sensor.find("a").text),
"type": sensor.attrib["type"],
"layer": sensor.attrib["layer"]
})
if not sensors:
rospy.logwarn("No sensor found in belt definition.")
rospy.logdebug("{} sensors found in belt definition".format(
len(sensors)))
self.Sensors = {s["id"]: s for s in sensors}
def is_grpc_error_unavailable(e):
is_grpc_error = isinstance(e, grpc.RpcError)
if is_grpc_error and (e.code() == grpc.StatusCode.UNAVAILABLE):
rospy.logfatal('grpc unavailable error: %s', e)
return True
return False
def poseCB(self, data):
self.gazebo_state.model_name = self.name
self.gazebo_state.pose = data.pose
self.gazebo_state.twist = data.twist
self.gazebo_state.reference_frame = "world"
self.pubState.publish(self.gazebo_state)
def startNode():
c = QuadJoy()
rospy.Subscriber("vicon", ViconState, c.poseCB)
rospy.spin()
if __name__ == '__main__':
ns = rospy.get_namespace()
try:
rospy.init_node('relay')
if str(ns) == '/':
rospy.logfatal("Need to specify namespace as vehicle name.")
rospy.logfatal("This is tyipcally accomplished in a launch file.")
rospy.logfatal(
"Command line: ROS_NAMESPACE=mQ01 $ rosrun quad_control joy.py"
)
else:
print "Starting joystick teleop node for: " + ns
startNode()
except rospy.ROSInterruptException:
pass
