def __init__(self):
self.ns_global_prefix = "/light_controller"
self.pub_marker = rospy.Publisher("marker", Marker)
# set default color to green rgba = [0,1,0,1]
self.color = ColorRGBA()
self.color.r = 0
self.color.g = 1
self.color.b = 0
self.color.a = 1
self.sim_mode = False
# get parameter from parameter server
if not self.sim_mode:
if not rospy.has_param(self.ns_global_prefix + "/devicestring"):
rospy.logwarn("parameter %s does not exist on ROS Parameter Server, aborting... (running in simulated mode)",self.ns_global_prefix + "/devicestring")
self.sim_mode = True
devicestring_param = rospy.get_param(self.ns_global_prefix + "/devicestring")
if not self.sim_mode:
if not rospy.has_param(self.ns_global_prefix + "/baudrate"):
rospy.logwarn("parameter %s does not exist on ROS Parameter Server, aborting... (running in simulated mode)",self.ns_global_prefix + "/baudrate")
self.sim_mode = True
baudrate_param = rospy.get_param(self.ns_global_prefix + "/baudrate")
if not self.sim_mode:
# open serial communication
rospy.loginfo("trying to initializing serial connection")
try:
self.ser = serial.Serial(devicestring_param, baudrate_param)
except serial.serialutil.SerialException:
rospy.logwarn("Could not initialize serial connection on %s, aborting... (running in simulated mode)",devicestring_param)
self.sim_mode = True
rospy.loginfo("serial connection initialized successfully")
def start_recognizer(self):
rospy.loginfo("Starting recognizer... ")
self.pipeline = gst.parse_launch(self.launch_config)
self.asr = self.pipeline.get_by_name('asr')
self.asr.connect('partial_result', self.asr_partial_result)
self.asr.connect('result', self.asr_result)
self.asr.set_property('configured', True)
self.asr.set_property('dsratio', 1)
# Configure language model
if rospy.has_param(self._lm_param):
lm = rospy.get_param(self._lm_param)
else:
rospy.logerr('Recognizer not started. Please specify a language model file.')
return
if rospy.has_param(self._dic_param):
dic = rospy.get_param(self._dic_param)
else:
rospy.logerr('Recognizer not started. Please specify a dictionary.')
return
self.asr.set_property('lm', lm)
self.asr.set_property('dict', dic)
self.bus = self.pipeline.get_bus()
self.bus.add_signal_watch()
self.bus_id = self.bus.connect('message::application', self.application_message)
self.pipeline.set_state(gst.STATE_PLAYING)
self.started = True
def execute(self, userdata):
userdata.base_pose_to_approach = userdata.task_list[userdata.current_task_index].location
#get location position
if (not rospy.has_param("script_server/base/" + userdata.base_pose_to_approach)):
rospy.logerr("location <<" + userdata.base_pose_to_approach + ">> is not on the parameter server")
return 'location_not_known'
position = rospy.get_param("script_server/base/" + userdata.base_pose_to_approach)
#get orientation angle
if (not rospy.has_param("script_server/base_orientations/" + userdata.task_list[userdata.current_task_index].orientation)):
rospy.logerr("orientation <<" + userdata.task_list[userdata.current_task_index].orientation + ">> is not on the parameter server")
return 'location_not_known'
orientation = rospy.get_param("script_server/base_orientations/" + userdata.task_list[userdata.current_task_index].orientation)
#establish new pose
new_pose = [0]*3
new_pose[0] = position[0]
new_pose[1] = position[1]
new_pose[2] = orientation
#set parameter
rospy.set_param("script_server/base/" + userdata.base_pose_to_approach, new_pose)
rospy.loginfo('selected pose: ' + userdata.base_pose_to_approach + " with orientation: " + userdata.task_list[userdata.current_task_index].orientation)
return 'pose_selected'
def node():
global server_ip
global qgc_port
rospy.init_node("mavlink_qgc_node",anonymous=True)
# if not rospy.has_param("/mav_raw_udp/server_ip"):
if not rospy.has_param("~server_ip"):
rospy.logwarn("no server ip spicified")
else:
server_ip = rospy.get_param("~server_ip")
rospy.logwarn("%s", server_ip)
if not rospy.has_param("~qgc_port"):
rospy.logwarn("no port spicified")
else:
qgc_port = rospy.get_param("~qgc_port")
rospy.logwarn("%d", qgc_port)
pub=rospy.Publisher('/from_qgc_mav_raw_data',MAV_RAW_DATA,queue_size=10)
rospy.Subscriber("/to_qgc_mav_raw_data",MAV_RAW_DATA,to_qgc_mav_raw_data_callback,queue_size=10)
r=rospy.Rate(100)
m=MAV_RAW_DATA()
m.channel=0
while not rospy.is_shutdown():
m.data,ADDR=locAddr.recvfrom(255)
rospy.loginfo(m.data)
pub.publish(m)
r.sleep()
def __init__(self):
self.received_state = False
if (not rospy.has_param("joints")):
rospy.logerr("No arm joints given.")
exit(0)
else:
self.joint_names = sorted(rospy.get_param("joints"))
rospy.loginfo("arm joints: %s", self.joint_names)
# read joint limits
self.joint_limits = []
for joint in self.joint_names:
if ((not rospy.has_param("limits/" + joint + "/min")) or (not rospy.has_param("limits/" + joint + "/min"))):
rospy.logerr("No arm joint limits given.")
exit(0)
else:
limit = arm_navigation_msgs.msg.JointLimits()
limit.joint_name = joint
limit.min_position = rospy.get_param("limits/" + joint + "/min")
limit.max_position = rospy.get_param("limits/" + joint + "/max")
self.joint_limits.append(limit)
self.current_joint_configuration = [0 for i in range(len(self.joint_names))]
self.unit = "rad"
# subscriptions
rospy.Subscriber("joint_states", sensor_msgs.msg.JointState, self.joint_states_callback)
# publications
self.pub_joint_positions = rospy.Publisher("position_command", brics_actuator.msg.JointPositions)
# action server
self.as_move_joint_direct = actionlib.SimpleActionServer("MoveToJointConfigurationDirect", raw_arm_navigation.msg.MoveToJointConfigurationAction, execute_cb = self.execute_cb_move_joint_config_direct)
self.as_move_cart_direct = actionlib.SimpleActionServer("MoveToCartesianPoseDirect", raw_arm_navigation.msg.MoveToCartesianPoseAction, execute_cb = self.execute_cb_move_cartesian_direct)
self.as_move_cart_rpy_sampled_direct = actionlib.SimpleActionServer("MoveToCartesianRPYSampledDirect", raw_arm_navigation.msg.MoveToCartesianPoseAction, execute_cb = self.execute_cb_move_cartesian_rpy_sampled_direct)
# additional classes
self.iks = SimpleIkSolver()
def __init__(self):
# Start node
rospy.init_node("recognizer_en")
self._device_name_param = "~mic_name" # Find the name of your microphone by typing pacmd list-sources in the terminal
self._lm_param = "~lm"
self._dic_param = "~dict"
# Configure mics with gstreamer launch config
if rospy.has_param(self._device_name_param):
self.device_name = rospy.get_param(self._device_name_param)
self.device_index = self.pulse_index_from_name(self.device_name)
self.launch_config = "pulsesrc device=" + str(self.device_index)
rospy.loginfo("Using: pulsesrc device=%s name=%s", self.device_index, self.device_name)
else:
self.launch_config = 'gconfaudiosrc'
rospy.loginfo("Launch config: %s", self.launch_config)
self.launch_config += " ! audioconvert ! audioresample " \
+ '! vader name=vad auto-threshold=true ' \
+ '! pocketsphinx name=asr ! fakesink'
# + '! pocketsphinx hmm=en_US/hub4wsj_sc_8k name=asr ! fakesink'
# Configure ROS settings
self.started = False
rospy.on_shutdown(self.shutdown)
self.pub = rospy.Publisher('~output', String)
rospy.Service("~start", Empty, self.start)
rospy.Service("~stop", Empty, self.stop)
if rospy.has_param(self._lm_param) and rospy.has_param(self._dic_param):
self.start_recognizer()
else:
rospy.logwarn("lm dic and hmm parameters need to be set to start recognizer.")
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 __init__(self, *args):
super(UnitTest, self).__init__(*args)
rospy.init_node('component_test')
self.message_received = False
self.sss = simple_script_server()
self.command_traj = JointTrajectory()
# get parameters
try:
# component
if not rospy.has_param('~component'):
self.fail('Parameter component does not exist on ROS Parameter Server')
self.component = rospy.get_param('~component')
# movement command
if not rospy.has_param('~test_target'):
self.fail('Parameter test_target does not exist on ROS Parameter Server')
self.test_target = rospy.get_param('~test_target')
# movement command
if not rospy.has_param('~default_target'):
self.fail('Parameter default_target does not exist on ROS Parameter Server')
self.default_target = rospy.get_param('~default_target')
# time to wait before
self.wait_time = rospy.get_param('~wait_time', 5)
# error range
if not rospy.has_param('~error_range'):
self.fail('Parameter error_range does not exist on ROS Parameter Server')
self.error_range = rospy.get_param('~error_range')
except KeyError, e:
self.fail('Parameters not set properly')
def listener(self):
# setup call back for lgging
print "Starting listener..."
rospy.init_node('log_listener', anonymous=True)
print "Looking for ros params..."
print rospy.search_param('logging_location')
print rospy.search_param('to_log')
if rospy.has_param('~logger_name'):
print("This logger is named")
self.logger_name = rospy.get_param('~logger_name')
print self.logger_name
if rospy.has_param('~to_log'):
print "logging the following topics..."
self.toLog = rospy.get_param('~to_log')
print self.toLog
else:
print "Failed to find topics to log."
if rospy.has_param('~enable_Log_grabbing'):
self.enableBDILogging = rospy.get_param('~enable_Log_grabbing')
else:
print "Not setting up log grabbing"
if rospy.has_param("~logging_location"):
print "logging to the following location..."
self.logLocation = rospy.get_param('~logging_location')
print self.logLocation
else:
print "Using default logging location"
rospy.Subscriber('/vigir_logging', OCSLogging, self.callback)
rospy.Subscriber('/vigir_logging_query', String, self.state)
self.pub = rospy.Publisher('/vigir_logging_responce', String, queue_size=1)
rospy.spin()
def __init__(self, *args):
super(UnitTest, self).__init__(*args)
self.sss = simple_script_server()
rospy.init_node('tactile_test')
self.message_received = False
self.actual_values = []
try:
#tactile topic
if not rospy.has_param('~topic'):
self.fail('Parameter topic does not exist on ROS Parameter Server')
self.topic = rospy.get_param('~topic')
# minimum required value
if not rospy.has_param('~min_tactile_value'):
self.fail('Parameter min_tactile_value does not exist on ROS Parameter Server')
self.min_tactile_value = rospy.get_param('~min_tactile_value')
#test time after confirm
if not rospy.has_param('~user_time'):
self.fail('Parameter user_time does not exist on ROS Parameter Server')
self.user_time = rospy.get_param('~user_time')
#message at confirm
if not rospy.has_param('~user_message'):
self.fail('Parameter user_message does not exist on ROS Parameter Server')
self.user_message = rospy.get_param('~user_message')
except KeyError, e:
self.fail('Parameters not set properly')
def __init__(self):
# Start the super class
DPControllerBase.__init__(self, is_model_based=True)
self._logger.info('Initializing: ' + self._LABEL)
# Proportional gains
self._Kp = np.zeros(shape=(6, 6))
# Derivative gains
self._Kd = np.zeros(shape=(6, 6))
# Error for the vehicle pose
self._error_pose = np.zeros(6)
self._tau = np.zeros(6)
if rospy.has_param('~Kp'):
Kp_diag = rospy.get_param('~Kp')
if len(Kp_diag) == 6:
self._Kp = np.diag(Kp_diag)
else:
raise rospy.ROSException('Kp matrix error: 6 coefficients '
'needed')
self._logger.info('Kp=' + str([self._Kp[i, i] for i in range(6)]))
if rospy.has_param('~Kd'):
Kd_diag = rospy.get_param('~Kd')
if len(Kd_diag) == 6:
self._Kd = np.diag(Kd_diag)
else:
raise rospy.ROSException('Kd matrix error: 6 coefficients '
'needed')
self._logger.info('Kd=' + str([self._Kd[i, i] for i in range(6)]))
self._is_init = True
self._logger.info(self._LABEL + ' ready')
def __init__ (self, inQueue, outQueue, planStr, startEvent, stopEvent, agent = None, pddlFiles=None):
if agent is None:
logger.error("Cannot repair with ROS without an agent name")
sys.exit(1)
self.repair_sub = rospy.Subscriber("hidden/repair/in", RepairMsg, self.repairCallback)
self.repair_pub = rospy.Publisher("hidden/repair/out", RepairMsg, queue_size=10)
self.stnvisu_pub = rospy.Publisher('/hidden/stnvisu', StnVisu, queue_size=10)
self.mastn_pub = rospy.Publisher('hidden/mastnUpdate/out', MaSTNUpdate, queue_size=10)
self.mastn_sub = rospy.Subscriber('hidden/mastnUpdate/in', MaSTNUpdate, self.mastnUpdate)
self.stnupdate_pub = rospy.Publisher('hidden/stnupdate', String, queue_size=10)
self.stnupdate_pub_latched = rospy.Publisher('hidden/stnupdate', String, queue_size=10, latch=True)
self.alea_srv = rospy.Service("alea", AleaAction, self.aleaReceived)
Supervisor.__init__(self, inQueue, outQueue, planStr, startEvent, stopEvent, agent, pddlFiles)
self.repairRos = True
self.lastPlanSyncRequest = time.time()
self.lastPlanSyncResponse = time.time()
if rospy.has_param("/hidden/ubForCom"):
f = float(rospy.get_param("/hidden/ubForCom"))
logger.info("Setting ubForCom to %s from the parameter server" % f)
self.ubForCom = f
if rospy.has_param("/hidden/ubForTrack"):
f = float(rospy.get_param("/hidden/ubForTrack"))
logger.info("Setting ubForTrack to %s from the parameter server" % f)
self.ubForTrack = f
def define(self):
self.pose=Pose()
self.oriation_angle=0
self.map=OccupancyGrid()
self.coor=[]
self.static_area=[]
#self.points_marker=Marker()
self.cut_points=[]#中分线的上下左右终点
self.centre_points=[]
self.feedback=''
#self.now=Time()
if not rospy.has_param('~detector_resolution'):
rospy.set_param('~detector_resolution',20)
self.mim_space = rospy.get_param('~detector_resolution')
if not rospy.has_param('~detector_radius'):
rospy.set_param('~detector_radius',0.2)
self.radius = rospy.get_param('~detector_radius')
if not rospy.has_param('~stop_flag_topic'):
rospy.set_param('~stop_flag_topic','/stop_flag')
self.stop_flag_topic = rospy.get_param('~stop_flag_topic')
self.stop_flag=rospy.Publisher("%s"%self.stop_flag_topic, String ,queue_size=1)
self.marker_pub=rospy.Publisher("detector_marker", Marker ,queue_size=1)
print 'radiu',self.radius
def _init_params(self):
self.connect_color_red = 255
if rospy.has_param('/sphero_swarm/connect_red'):
self.connect_color_red = rospy.get_param('/sphero_swarm/connect_red')
rospy.set_param('/sphero_swarm/connect_red', self.connect_color_red)
self.connect_color_blue = 255
if rospy.has_param('/sphero_swarm/connect_blue'):
self.connect_color_blue = rospy.get_param('/sphero_swarm/connect_blue')
rospy.set_param('/sphero_swarm/connect_blue', self.connect_color_blue)
self.connect_color_green = 255
if rospy.has_param('/sphero_swarm/connect_green'):
self.connect_color_green = rospy.get_param('/sphero_swarm/connect_green')
rospy.set_param('/sphero_swarm/connect_green', self.connect_color_green)
self.connect_back_led = 255
if rospy.has_param('/sphero_swarm/connect_back_led'):
self.connect_back_led = rospy.get_param('/sphero_swarm/connect_back_led',255)
rospy.set_param('/sphero_swarm/connect_back_led', self.connect_back_led)
self.cmd_vel_timeout = rospy.Duration(rospy.get_param('~cmd_vel_timeout', 0.6))
self.diag_update_rate = rospy.Duration(rospy.get_param('~diag_update_rate', 1.0))
self.team_info = rospy.get_param('/sphero_swarm/team')
def main():
global publishers
if rospy.has_param('/asmo/allow_periodic_competition'):
allow_periodic_competition = rospy.get_param('/asmo/allow_periodic_competition')
else:
allow_periodic_competition = True
if rospy.has_param('/asmo/periodic_milliseconds'):
periodic_milliseconds = rospy.get_param('/asmo/periodic_milliseconds')
else:
periodic_milliseconds = 110
# periodic_rate is in Hz
periodic_rate = 1000 / periodic_milliseconds
rospy.init_node(_process_name)
publishers['/asmo/winners'] = rospy.Publisher('/asmo/winners', asmo.msg.Winners, queue_size=10)
rospy.Subscriber('/asmo/non_reflex', asmo.msg.NonReflex, handle_process)
rospy.Subscriber('/asmo/reflex', asmo.msg.Reflex, handle_process)
rospy.Subscriber('/asmo/message_non_reflex', asmo.msg.MessageNonReflex, handle_message_process)
rospy.Subscriber('/asmo/message_reflex', asmo.msg.MessageReflex, handle_message_process)
rospy.Subscriber('/asmo/attention_value', asmo.msg.NameValue, handle_name_value)
rospy.Subscriber('/asmo/boost_value', asmo.msg.NameValue, handle_name_value)
rospy.Subscriber('/asmo/priority_level', asmo.msg.NameValue, handle_name_value)
rospy.Subscriber('/asmo/remove_process', std_msgs.msg.String, handle_remove_process)
rospy.Subscriber('/asmo/compete', std_msgs.msg.Empty, handle_compete)
print('[ OK ] Start {process_name}'.format(process_name=_process_name))
if allow_periodic_competition:
while not rospy.is_shutdown():
handle_compete([])
rospy.Rate(periodic_rate).sleep()
else:
rospy.spin()
def init_gains(self, kp, ki, kd):
# initialize gains from various sources
# just start by using given ones
self.kp = kp
self.ki = ki
self.kd = kd
rospy.loginfo('PID %s given gains kp=%f, ki=%f, kd=%f', self.namespace, self.kp, self.ki, self.kd)
# check for parameter overrides
gain_override_flag=False
# proportional
if rospy.has_param(self.namespace + "/init_gains/kp"):
self.kp = rospy.get_param(self.namespace + "/init_gains/kp")
gain_override_flag=True
# integral
if rospy.has_param(self.namespace + "/init_gains/ki"):
self.ki = rospy.get_param(self.namespace + "/init_gains/ki")
gain_override_flag=True
# derivative
if rospy.has_param(self.namespace + "/init_gains/kd"):
self.kd = rospy.get_param(self.namespace + "/init_gains/kd")
gain_override_flag=True
# report override
if gain_override_flag:
rospy.logwarn('PID %s got gains from parameters kp=%f, ki=%f, kd=%f', self.namespace, self.kp, self.ki, self.kd)
def init_config( self ):
# mandatory configurations to be set
self.config['frame_rate'] = rospy.get_param('~frame_rate')
self.config['source'] = rospy.get_param('~source')
self.config['resolution'] = rospy.get_param('~resolution')
self.config['color_space'] = rospy.get_param('~color_space')
# optional for camera frames
# top camera with default
if rospy.has_param('~camera_top_frame'):
self.config['camera_top_frame'] = rospy.get_param('~camera_top_frame')
else:
self.config['camera_top_frame'] = "/CameraTop_frame"
# bottom camera with default
if rospy.has_param('~camera_bottom_frame'):
self.config['camera_bottom_frame'] = rospy.get_param('~camera_bottom_frame')
else:
self.config['camera_bottom_frame'] = "/CameraBottom_frame"
# depth camera with default
if rospy.has_param('~camera_depth_frame'):
self.config['camera_depth_frame'] = rospy.get_param('~camera_depth_frame')
else:
self.config['camera_depth_frame'] = "/DepthCamera_frame"
#load calibration files
if rospy.has_param('~calibration_file_top'):
self.config['calibration_file_top'] = rospy.get_param('~calibration_file_top')
if rospy.has_param('~calibration_file_bottom'):
self.config['calibration_file_bottom'] = rospy.get_param('~calibration_file_bottom')
def __init__(self):
self.JOINT_TWO_LAYING_GRASP = 2.44389
self.JOINT_FOUR_LAYING_GRASP = 3.10522
#2.47758 old value
self.received_state = False
self.joint_namespace = "/arm_1/arm_controller/"
if (not rospy.has_param(self.joint_namespace+"joints")):
rospy.logerr("No arm joints given.")
exit(0)
else:
self.joint_names = sorted(rospy.get_param(self.joint_namespace+"joints"))
rospy.loginfo("arm joints: %s", self.joint_names)
# read joint limits
self.joint_limits = []
for joint in self.joint_names:
if ((not rospy.has_param(self.joint_namespace+"limits/" + joint + "/min")) or (not rospy.has_param(self.joint_namespace+"limits/" + joint + "/min"))):
rospy.logerr("No arm joint limits given.")
exit(0)
else:
limit = arm_navigation_msgs.msg.JointLimits()
limit.joint_name = joint
limit.min_position = rospy.get_param(self.joint_namespace+"limits/" + joint + "/min")
limit.max_position = rospy.get_param(self.joint_namespace+"limits/" + joint + "/max")
self.joint_limits.append(limit)
self.current_joint_configuration = [0 for i in range(len(self.joint_names))]
self.unit = "rad"
# subscriptions
rospy.Subscriber("joint_states", sensor_msgs.msg.JointState, self.joint_states_callback)
def __init__( self ):
# Create critique service proxy
critique_topic = rospy.get_param( '~critic_service' )
rospy.wait_for_service( critique_topic )
self.critique_service = rospy.ServiceProxy( critique_topic, GetCritique, True )
# Read initialization
self.grid_mins = rospy.get_param( '~grid_lower_bounds' )
self.grid_max = rospy.get_param( '~grid_upper_bounds' )
if len(self.grid_mins) != len(self.grid_max):
raise ValueError( 'Grid mins and max must have same number of elements' )
# Set up the grid
grid_values = []
if rospy.has_param( '~grid_step_sizes' ):
step_sizes = rospy.get_param( '~grid_step_sizes' )
for (lower,upper,step) in izip(self.grid_mins, self.grid_max, step_sizes):
grid_values.append( np.arange( lower, upper, step ) )
elif rospy.has_param( '~grid_num_points' ):
num_points = rospy.get_param( '~grid_num_points' )
for (lower,upper,num) in izip(self.grid_mins, self.grid_max, num_points):
grid_values.append( np.linspace( lower, upper, num ) )
else:
raise ValueError( 'Must specify step sizes or number of points in grid dimensions.' )
self.param_iterator = product( *grid_values )
self.num_samples = rospy.get_param( '~num_samples', 1 )
# I/O initialization
output_path = rospy.get_param( '~output_log_path' )
self.output_log = open( output_path, 'w' )
if self.output_log is None:
raise RuntimeError( 'Could not open output log at path: ' + output_path )
def handle_auction_server_callback(auction_req):
global role_assigned, actual_role
global master_server, buyer_server
# update number of messages in parameter server
if rospy.has_param('/num_messages'):
num_messages = rospy.get_param('/num_messages')
num_messages += 2
rospy.set_param('/num_messages', num_messages)
# Clean up
if rospy.has_param('/auction_closed'):
if rospy.get_param('/auction_closed') == True:
role_assigned = False
rospy.loginfo(rospy.get_name()+' '+str(role_assigned))
# avoid node to take another role
if not role_assigned:
role_assigned = True
if auction_req.role == 'be_auctioneer':
return auctioneer.handle_auction_server_callback(auction_req)
elif auction_req.role == 'be_buyer':
return buyer_k_saap.handle_auction_server_callback(auction_req)
else:
return {'response_info':'invalid role requested'}
else:
return {'response_info':'node already have a role'}
def test_set_param(self):
try: rospy.set_param(None, 'foo'); self.fail("set_param(None) succeeded")
except: pass
try: rospy.set_param('', 'foo'); self.fail("set_param('') succeeded")
except: pass
rostest_tests = {
'spstring': "string",
'spint0': 0,
'spint10': 10,
'spfloat0': 0.0,
'spfloat10': 10.0,
"spnamespace/string": "namespaced string",
}
initial_param_names = rospy.get_param_names()
param_names = [rospy.resolve_name(k) for k in rostest_tests.keys()]
for k, v in rostest_tests.iteritems():
self.failIf(rospy.has_param(k))
self.failIf(rospy.has_param(rospy.resolve_name(k)))
rospy.set_param(k, v)
self.assert_(rospy.has_param(k))
self.assert_(rospy.has_param(rospy.resolve_name(k)))
self.assertEquals(v, rospy.get_param(k))
self.assertEquals(v, rospy.get_param(rospy.resolve_name(k)))
correct_state = set(initial_param_names + param_names)
self.failIf(correct_state ^ set(rospy.get_param_names()))
def listener(self):
# setup call back for lgging
print "Starting listener..."
rospy.init_node('log_listener', anonymous=True)
print "Looking for ros params..."
print rospy.search_param('logging_location')
if rospy.has_param("~logging_location"):
print "logging to the following location..."
self.logLocation = rospy.get_param('~logging_location')
print self.logLocation
else:
print "Using default logging location"
if rospy.has_param("~onboard"):
print "logging instance is onboard?"
self.onboard = rospy.get_param("~onboard")
print self.onboard
print "Subscribe to logging messages ..."
rospy.Subscriber('/vigir_logging', OCSLogging, self.callback)
rospy.Subscriber('/vigir_logging_query', String, self.state)
print "Publish topic ..."
self.pub = rospy.Publisher('/vigir_logging_responce', String, queue_size=1)
self.query = "desktop_"
print "Now spin ..."
rospy.spin()
print "Shutdown the desktop logger!"
def __init__(self, *args):
super(UnitTest, self).__init__(*args)
rospy.init_node('teleop_test')
self.message_received = False
try:
# topic
if not rospy.has_param('~topic'):
self.fail('Parameter topic does not exist on ROS Parameter Server')
self.topic = rospy.get_param('~topic')
# desired movement commands
if not rospy.has_param('~min_target_x'):
self.fail('Parameter min_target_x does not exist on ROS Parameter Server')
self.min_target_x = rospy.get_param('~min_target_x')
if not rospy.has_param('~min_target_y'):
self.fail('Parameter min_test_target_y does not exist on ROS Parameter Server')
self.min_target_y = rospy.get_param('~min_target_y')
#test time after confirm
if not rospy.has_param('~user_time'):
self.fail('Parameter user_time does not exist on ROS Parameter Server')
self.user_time = rospy.get_param('~user_time')
#message at confirm
if not rospy.has_param('~user_message'):
self.fail('Parameter user_message does not exist on ROS Parameter Server')
self.user_message = rospy.get_param('~user_message')
except KeyError, e:
self.fail('Parameters not set properly')
def joint_configuration_check_ss(self, params, userdata): # get names and states from script server
rospy.loginfo("<<joint_configuration_check_ss>>")
for item in params.values()[0]:
component = item['component']
configuration = item['configuration']
ss_names_path = "/script_server/" + component + "/joint_names"
ss_values_path = "/script_server/" + component + "/" + configuration
if rospy.has_param(ss_names_path):
joint_names = rospy.get_param(ss_names_path)
else:
self.result ="failed"
rospy.logerr("There is no " + ss_names_path + " on parameters server." )
return
if rospy.has_param(ss_values_path):
joint_states = rospy.get_param(ss_values_path)[0]
else:
self.result = "failed"
rospy.logerr("There is no " + ss_values_path + " on parameters server." )
return
aw_error = item['allowed_error']
self.joint_configuration_check(joint_names,joint_states, aw_error)
def __init__(self):
self.received_state = False
if (not rospy.has_param("~position_command")):
rospy.logerr("No parameter set for ~position_command")
exit(-1)
else:
self.position_cmd_topic = rospy.get_param("~position_command", "POSITION_COMMAND_NOT_SET")
self.server = actionlib.SimpleActionServer('gripper_controller', GripperCommandAction, self.execute, False)
self.pub_joint_positions = rospy.Publisher(self.position_cmd_topic, JointPositions)
rospy.loginfo("using " + self.position_cmd_topic + " topic for publishing gripper commands")
self.joint_names = []
self.current_joint_configuration = [0, 0]
if (not rospy.has_param("~joints")):
rospy.logerr("No parameter set for ~joints")
exit(-1)
else:
self.joint_names = sorted(rospy.get_param("~joints"))
rospy.loginfo("gripper joints: %s", self.joint_names)
rospy.Subscriber("joint_states", JointState, self.joint_states_callback)
self.server.start()
def __init__(self):
# Start node
rospy.init_node("recognizer_cn")
self._device_name_param = "~mic_name" # Find the name of your microphone by typing pacmd list-sources in the terminal
self._lm_param = "~lm"
self._dic_param = "~dict"
# Configure mics with gstreamer launch config
if rospy.has_param(self._device_name_param):
self.device_name = rospy.get_param(self._device_name_param)
self.device_index = self.pulse_index_from_name(self.device_name)
self.launch_config = "pulsesrc device=" + str(self.device_index)
rospy.loginfo("Using: pulsesrc device=%s name=%s", self.device_index, self.device_name)
else:
self.launch_config = 'filesrc location=/home/ppeix/wav/foo%05d.wav ! wavparse '
# rospy.loginfo("Launch config: %s", self.launch_config)
#rospy.loginfo("Launch config: %s", self.launch_config)
# Configure ROS settings
self.started = False
rospy.on_shutdown(self.shutdown)
self.pub = rospy.Publisher('~output', String)
rospy.Service("~start", Empty, self.start)
rospy.Service("~stop", Empty, self.stop)
if rospy.has_param(self._lm_param) and rospy.has_param(self._dic_param):
self.start_loop()
else:
rospy.logwarn("lm and dic parameters need to be set to start recognizer.")
def dummy_image_source():
pub = rospy.Publisher('dummy_images/compressed', CompressedImage)
rospy.init_node('dummy_image_source')
while not rospy.is_shutdown():
if rospy.has_param('dummy_image_source/image'):
try:
image_handle = load_image(rospy.get_param('dummy_image_source/image'))
except:
rospy.logwarn("Problem loading image")
else:
publish_image(pub, image_handle)
print "Image Sent"
elif rospy.has_param('dummy_image_source/image_dir'):
try:
image_handle = random_image_from_dir(rospy.get_param('dummy_image_source/image_dir'))
except:
rospy.logwarn("Problem loading image from directory")
else:
publish_image(pub, image_handle)
print "Image Sent"
else:
rospy.logwarn("No image source definied in 'image' or 'image_dir' params")
rospy.sleep(rospy.get_param('dummy_image_source/sleep_time', config.DEFAULT_SLEEP_TIME))
def test_delete_param(self):
try: rospy.delete_param(None); self.fail("delete_param(None) succeeded")
except: pass
try: rospy.delete_param(''); self.fail("delete_param('') succeeded")
except: pass
rostest_tests = {
'dpstring': "string",
'dpint0': 0,
'dpint10': 10,
'dpfloat0': 0.0,
'dpfloat10': 10.0,
"dpnamespace/string": "namespaced string",
}
initial_params = rospy.get_param_names()
# implicitly depends on set param
for k, v in rostest_tests.iteritems():
rospy.set_param(k, v)
# test delete
for k, v in rostest_tests.iteritems():
self.assert_(rospy.has_param(k))
rospy.delete_param(k)
self.failIf(rospy.has_param(k))
self.failIf(rospy.has_param(rospy.resolve_name(k)))
try:
rospy.get_param(k)
self.fail("get_param should fail on deleted key")
except KeyError: pass
# make sure no new state
self.failIf(set(initial_params) ^ set(rospy.get_param_names()))
def __init__(self, path_name="path"):
smach.StateMachine.__init__(self, outcomes=['succeeded','not_reached','failed'])
with self:
self.add('SAFE_MODE', Turtlebot_SetMode(2),
transitions={'succeeded':'DELIVERY0', 'failed':'failed'})
no = 0
#for i in range(0,3):
while rospy.has_param("/script_server/base/"+path_name+str(no)):
#last = i>1n
last = not rospy.has_param("/script_server/base/"+path_name+str(no+1))
next = 'DELIVERY'+str(no+1)
if last:
next = 'succeeded'
self.add('DELIVERY'+str(no),DeliveryTask(path_name+str(no)),
transitions={'succeeded':next,
'failed':'failed'})
no += 1
self.add('MOVE_BACK',ApproachPose("charge_pose"),
transitions={'reached':'succeeded',
'not_reached':'not_reached',
'failed':'failed'})
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 handle_buyer_server_callback(auction_req):
# update number of messages in parameter server
if rospy.has_param('/num_messages'):
num_messages = rospy.get_param('/num_messages')
num_messages += 2
rospy.set_param('/num_messages', num_messages)
# Graph parameters
graph_parameters = eval(rospy.get_param("graph_info"))
N = int(graph_parameters[0])
l = int(graph_parameters[1])
d = int(graph_parameters[2])
r = math.sqrt((d*l*l)/(math.pi*(N-1)))
print N, l, d, r
# Calculate k (number of hop)
node_position = eval(rospy.get_param('~position'))
auctioneer_position = eval(auction_req._connection_header['auctioneer_position'])
x = float(node_position[0])-float(auctioneer_position[0])
y = float(node_position[1])-float(auctioneer_position[1])
z = float(node_position[2])-float(auctioneer_position[2])
distance_from_node_to_auctioneer = math.sqrt(x*x+y*y+z*z)
k = int(distance_from_node_to_auctioneer/r)
print distance_from_node_to_auctioneer, k
# Create a bid messsage to put an offer for the item in auction_req!
bid = auction_msgs.msg.Bid()
bid.header.frame_id = 'base_link' # to be rechecked
bid.header.stamp = rospy.Time.now()
bid.buyer_id = rospy.get_name()
if auction_req.auction_data.metrics == "distance":
# to be given by the cost to go to position of the ocurring event
# the cost for the metrics==distance is calculated using the euclidean
# distance between the parameter position of the node and the task_position
# given in the auction_req
x = float(node_position[0])-auction_req.auction_data.task_location.x
y = float(node_position[1])-auction_req.auction_data.task_location.y
z = float(node_position[2])-auction_req.auction_data.task_location.z
bid.cost_distance = float(math.sqrt(x*x+y*y+z*z))
else:
rospy.loginfo("Metrics unkown")
bid.cost_distance = 999999;
# put bid to auctioneer
service_path = auction_req.auctioneer_node+'/auctioneer_bid_reception_server'
rospy.wait_for_service(service_path)
auctioneer_bid_reception_service = rospy.ServiceProxy(service_path, auction_srvs.srv.AuctioneerBidReceptionService)
try:
sending_node = rospy.get_name()
auctioneer_bid_reception_server_resp = auctioneer_bid_reception_service(sending_node,bid)
except rospy.ServiceException, e:
print "Service did not process request: %s"%str(e)
def __init__(self):
print "MobileBase.-> INITIALIZING THE AMAZING MOBILE BASE NODE BY REY..."
rospy.init_node("mobile_base");
rospy.on_shutdown(self.shutdown)
rospy.loginfo("Trying to Connect to roboclaws")
port_name_frontal = "/dev/ttyACM0";
port_name_lateral = "/dev/ttyACM1";
self.simul = False
self.ControlP = 1.0
self.ControlI = 1.0
self.ControlD = 1.0
if rospy.has_param('~simul'):
self.simul = rospy.get_param('~simul')
if rospy.has_param('~port1'):
port_name_frontal = rospy.get_param('~port1')
elif not self.simul:
print_help();
sys.exit();
if rospy.has_param('~port2'):
port_name_lateral = rospy.get_param('~port2')
elif not self.simul:
#TODO put the correct config ros param help
print_help();
sys.exit();
if rospy.has_param('~ControlP'):
self.ControlP = int(rospy.get_param('~ControlP'))
if rospy.has_param('~ControlI'):
self.ControlI = int(rospy.get_param('~ControlI'))
if rospy.has_param('~ControlD'):
self.ControlD = int(rospy.get_param('~ControlD'))
#TODO If is necessary add the ros param address roboclaw
self.rc_address_frontal = 0x80
self.rc_address_lateral = 0x80
#self.BASE_WIDTH = 0.6
self.BASE_WIDTH = 0.46
#self.TICKS_PER_METER_LATERAL = 103072.0 #Ticks per meter for the slow motors (front and rear)
#self.TICKS_PER_METER_FRONTAL = 103072.0 #Ticks per meter for the fast motors (left and right)
self.TICKS_PER_METER_LATERAL = 139071.0 #Ticks per meter for the slow motors (front and rear)
self.TICKS_PER_METER_FRONTAL = 139071.0 #Ticks per meter for the fast motors (left and right)
#baud_rate_frontal = 38400
#baud_rate_lateral = 38400
baud_rate_frontal = 115200
baud_rate_lateral = 115200
self.speed_left_f = 0
self.speed_right_r = 0
self.speed_right_f = 0
self.speed_left_r = 0
if not self.simul:
self.rc_frontal = roboclaw.Roboclaw(port_name_frontal, baud_rate_frontal); #Roboclaw controling motors for frontal movement (left and right)
self.rc_lateral = roboclaw.Roboclaw(port_name_lateral, baud_rate_lateral); #Roboclaw controling motors for lateral movement (front and rear)
if self.rc_address_frontal > 0x87 or self.rc_address_frontal < 0x80:
rospy.logfatal("Address frontal out of range")
rospy.signal_shutdown("Address lateral out of range")
if self.rc_address_frontal > 0x87 or self.rc_address_frontal < 0x80:
rospy.logfatal("Address frontal out of range")
rospy.signal_shutdown("Address lateral out of range")
try:
self.rc_frontal.Open()
except Exception as e:
rospy.logfatal("Could not connect to frontal Roboclaw")
rospy.logdebug(e)
rospy.signal_shutdown("Could not connect to frontal Roboclaw")
try:
self.rc_lateral.Open()
except Exception as e:
rospy.logfatal("Could not connect to lateral Roboclaw")
rospy.logdebug(e)
rospy.signal_shutdown("Could not connect to lateral Roboclaw")
#TODO Config the diagnostic config
try:
frontal_version = self.rc_frontal.ReadVersion(self.rc_address_frontal)
except Exception as e:
rospy.logwarn("Problem getting roboclaw frontal version")
rospy.logdebug(e)
pass
if not frontal_version[0]:
rospy.logwarn("Could not get version from frontal roboclaw")
else:
rospy.logdebug(repr(frontal_version[1]))
try:
lateral_version = self.rc_frontal.ReadVersion(self.rc_address_lateral)
except Exception as e:
rospy.logwarn("Problem getting roboclaw lateral version")
rospy.logdebug(e)
pass
if not lateral_version[0]:
rospy.logwarn("Could not get version from lateral roboclaw")
else:
rospy.logdebug(repr(lateral_version[1]))
self.rc_frontal.SpeedM1M2(self.rc_address_frontal, 0, 0)
self.rc_frontal.ResetEncoders(self.rc_address_frontal)
self.rc_lateral.SpeedM1M2(self.rc_address_lateral, 0, 0)
self.rc_lateral.ResetEncoders(self.rc_address_lateral)
#ROBOCLAW CONFIGURATION CONSTANT
print "MobileBase.->Control PID constants for Velocity passed by parameter:"
print "MobileBase.->P = " + str(self.ControlP)
print "MobileBase.->I = " + str(self.ControlI)
print "MobileBase.->D = " + str(self.ControlD)
#pos_PID_left = self.rc_frontal.ReadM1PositionPID(self.rc_address_frontal)
#pos_PID_right = self.rc_frontal.ReadM2PositionPID(self.rc_address_frontal)
#pos_PID_front = self.rc_lateral.ReadM1PositionPID(self.rc_address_lateral)
#pos_PID_rear = self.rc_lateral.ReadM2PositionPID(self.rc_address_lateral)
vel_PID_left = self.rc_frontal.ReadM1VelocityPID(self.rc_address_frontal)
vel_PID_right = self.rc_frontal.ReadM2VelocityPID(self.rc_address_frontal)
vel_PID_front = self.rc_lateral.ReadM1VelocityPID(self.rc_address_lateral)
vel_PID_rear = self.rc_lateral.ReadM2VelocityPID(self.rc_address_lateral)
#print "MobileBase.->Position PID constants: Success P I D MaxI Deadzone MinPos MaxPos"
#print "MobileBase.->Left Motor: " + str(pos_PID_left )
#print "MobileBase.->Right Motor: " + str(pos_PID_right)
#print "MobileBase.->Front Motor: " + str(pos_PID_front)
#print "MobileBase.->Rear Motor: " + str(pos_PID_rear )
print "MobileBase.->Velocity PID constants: Success P I D QPPS" #QPPS = speed in ticks/s when motor is at full speed
print "MobileBase.->Left Motor: " + str(vel_PID_left )
print "MobileBase.->Right Motor: " + str(vel_PID_right)
print "MobileBase.->Front Motor: " + str(vel_PID_front)
print "MobileBase.->Left Motor: " + str(vel_PID_rear )
self.QPPS_LEFT_F = vel_PID_left[4]
self.QPPS_RIGHT_R = vel_PID_right[4]
self.QPPS_RIGHT_F = vel_PID_front[4]
self.QPPS_LEFT_R = vel_PID_rear[4]
if self.QPPS_LEFT_F != self.QPPS_RIGHT_R or self.QPPS_RIGHT_F != self.QPPS_LEFT_R:
print "MobileBase.->WARNING: Motors have different max speeds!! Stall condition may occur."
if self.rc_frontal.ReadPWMMode(self.rc_address_frontal)[1] == 1:
print "MobileBase.->PWM Mode for left and right motors: Sign magnitude"
else:
print "MobileBase.->PWM Mode for left and right motors: Locked antiphase"
if self.rc_lateral.ReadPWMMode(self.rc_address_lateral)[1] == 1:
print "MobileBase.->PWM Mode for front and rear motors: Sign magnitude"
else:
print "MobileBase.->PWM Mode for front and rear motors: Locked antiphase"
else:
self.QPPS_LEFT_F = self.TICKS_PER_METER_FRONTAL
self.QPPS_RIGHT_R = self.TICKS_PER_METER_FRONTAL
self.QPPS_RIGHT_F = self.TICKS_PER_METER_LATERAL
self.QPPS_LEFT_R = self.TICKS_PER_METER_LATERAL
self.encodm = EncoderOdom(self.TICKS_PER_METER_FRONTAL, self.TICKS_PER_METER_LATERAL, self.BASE_WIDTH)
self.last_set_speed_time = rospy.get_rostime()
rospy.sleep(1)
self.pubBattery = rospy.Publisher("mobile_base/base_battery", Float32, queue_size = 1);
self.pubJointStates = rospy.Publisher("/joint_states", JointState, queue_size = 1)
#subStop = rospy.Subscriber("robot_state/stop", Empty, callback_stop, queue_size=1);
self.subSpeeds = rospy.Subscriber("/hardware/mobile_base/speeds", Float32MultiArray, self.cmd_speeds_callback, queue_size=1);
self.subCmdVel = rospy.Subscriber("/hardware/mobile_base/cmd_vel", Twist, self.cmd_vel_callback, queue_size=1);
self.subSimul = rospy.Subscriber("/simulated", Bool, self.callback_simulated, queue_size = 1);
self.dev = open(dev_path,"rb") #TODO: handle exception.
rospy.loginfo("[MouseEncoder] Use %s" %(dev_path))
self.pub_tick = rospy.Publisher('~tick',Point, queue_size=1)
def getMouseTick(self,event):
buf = self.dev.read(3);
button = ord( buf[0] );
bLeft = button & 0x1;
bMiddle = ( button & 0x4 ) > 0;
bRight = ( button & 0x2 ) > 0;
x,y = struct.unpack( "bb", buf[1:] );
# print ("L:%d, M: %d, R: %d, x: %d, y: %d\n" % (bLeft,bMiddle,bRight, x, y) );
point = Point()
point.x = x
point.y = y
self.pub_tick.publish(point)
# rospy.loginfo("[x,y] = %s" %([x,y]))
if __name__ == '__main__':
rospy.init_node('mouse_encoder',anonymous=False)
# Set default parameters
dev_path = "/dev/input/mice"
# Read from paramter
if rospy.has_param("~dev_path"):
dev_path = rospy.get_param("~dev_path")
mouse_odo = MouseEncoder(dev_path)
moust_timer = rospy.Timer(rospy.Duration.from_sec(0.01),mouse_odo.getMouseTick)
rospy.spin()
def test_urdf(self):
self.assertTrue(rospy.has_param("robot_description"))
rd = rospy.get_param("robot_description")
self.assertNotEqual("", rd)
current_point.pose.position.z = data_to_rviz.pose.position.z
list_posestamped.append(current_point)
else:
rospy.logerr('[RVIZ] The requested body is not available')
data_to_rviz = draw_body_error(body)
#publish the data to rviz
path.poses = list_posestamped
#position and orientation of the quad
position.publish(data_to_rviz)
#trajectory of the quad
trajectory.publish(path)
loop_rate.sleep()
if __name__ == '__main__':
rospy.init_node('rviz_mocap')
#get the body id parameter from the parameters space
body_id = int(rospy.get_param('/rviz_mocap/qualysis_id', 7))
if rospy.has_param('/rviz_mocap/qualysis_id'):
rospy.loginfo('Found body id: ' + str(body_id))
else:
rospy.logwarn('No body id found - trying with default: ' +
str(body_id))
talk(body_id)
#EOF
def __init__(self):
self.action_clients = []
self.joint_names = []
while not rospy.has_param('~state_topics'):
logging.loginfo('waiting for param ' + '~state_topics')
rospy.sleep(1)
while not rospy.has_param('~client_topics'):
logging.loginfo('waiting for param ' + '~client_topics')
rospy.sleep(1)
self.state_topics = rospy.get_param('~state_topics', [])
self.client_topics = rospy.get_param('~client_topics', [])
self.number_of_clients = len(self.state_topics)
if self.number_of_clients != len(self.client_topics):
logging.logerr('Joint Trajector Splitter: number of state and action topics do not match')
exit()
if self.number_of_clients == 0:
logging.logerr('Joint Trajector Splitter: the state_topic or client_topic parameter is empty')
exit()
self.client_type = []
for i in range(self.number_of_clients):
try:
rospy.wait_for_message(self.state_topics[i], AnyMsg)
type = rostopic.get_info_text(self.client_topics[i] + '/goal').split('\n')[0][6:]
self.client_type.append(type)
except rostopic.ROSTopicException:
logging.logerr('Joint Trajector Splitter: unknown topic \'{}/goal\' \nmissing / in front of topic name?'.format(self.client_topics[i]))
exit()
self.state_type = []
for i in range(self.number_of_clients):
try:
type = rostopic.get_info_text(self.state_topics[i]).split('\n')[0][6:]
self.state_type.append(type)
except rostopic.ROSTopicException:
logging.logerr(
'Joint Trajector Splitter: unknown topic \'{}/goal\' \nmissing / in front of topic name?'.format(
self.state_topics[i]))
exit()
for i in range(self.number_of_clients):
if self.client_type[i] == 'control_msgs/FollowJointTrajectoryActionGoal':
self.action_clients.append(actionlib.SimpleActionClient(self.client_topics[i], control_msgs.msg.FollowJointTrajectoryAction))
elif self.client_type[i] == 'pr2_controllers_msgs/JointTrajectoryActionGoal':
self.action_clients.append(actionlib.SimpleActionClient(self.client_topics[i], pr2_controllers_msgs.msg.JointTrajectoryAction))
else:
logging.logerr('Joint Trajector Splitter: wrong client topic type:' + self.client_type[i] + '\nmust be either control_msgs/FollowJointTrajectoryActionGoal or pr2_controllers_msgs/JointTrajectoryActionGoal')
exit()
self.joint_names.append(rospy.wait_for_message(self.state_topics[i], control_msgs.msg.JointTrajectoryControllerState).joint_names)
logging.loginfo('Joint Trajector Splitter: connected to {}'.format(self.client_topics[i]))
self.current_controller_state = control_msgs.msg.JointTrajectoryControllerState()
total_number_joints = 0
for joint_name_list in self.joint_names:
total_number_joints += len(joint_name_list)
self.current_controller_state.joint_names.extend(joint_name_list)
self.current_controller_state.desired.positions = [0 for i in range(total_number_joints)]
self.current_controller_state.desired.accelerations = [0 for i in range(total_number_joints)]
self.current_controller_state.desired.effort = [0 for i in range(total_number_joints)]
self.current_controller_state.desired.velocities = [0 for i in range(total_number_joints)]
self.current_controller_state.actual = copy.deepcopy(self.current_controller_state.desired)
self.current_controller_state.error = copy.deepcopy(self.current_controller_state.desired)
self.state_pub = rospy.Publisher('/whole_body_controller/state', control_msgs.msg.JointTrajectoryControllerState, queue_size=10)
for i in range(self.number_of_clients):
if self.state_type[i] == 'control_msgs/JointTrajectoryControllerState':
rospy.Subscriber(self.state_topics[i], control_msgs.msg.JointTrajectoryControllerState, self.state_cb_update)
elif self.state_type[i] == 'pr2_controllers_msgs/JointTrajectoryControllerState':
rospy.Subscriber(self.state_topics[i], pr2_controllers_msgs.msg.JointTrajectoryControllerState, self.state_cb_update)
else:
logging.logerr('Joint Trajector Splitter: wrong state topic type ' + self.state_type[i] + '\nmust be either control_msgs/JointTrajectoryControllerState or pr2_controllers_msgs/JointTrajectoryControllerState')
exit()
rospy.Subscriber(self.state_topics[0], control_msgs.msg.JointTrajectoryControllerState, self.state_cb_publish)
self._as = actionlib.SimpleActionServer('/whole_body_controller/follow_joint_trajectory', control_msgs.msg.FollowJointTrajectoryAction,
execute_cb=self.callback, auto_start=False)
self._as.register_preempt_callback(self.preempt_cb)
self._as.start()
logging.loginfo(u'Joint Trajector Splitter: running')
rospy.spin()
from numpy import pi
from geometry_msgs.msg import Point
from std_msgs.msg import Time
if __name__ == '__main__':
print 'Starting the helical trajectory creator'
rospy.init_node('start_circular_trajectory')
if rospy.is_shutdown():
print 'ROS master not running!'
sys.exit(-1)
# If no start time is provided: start *now*.
start_time = rospy.Time.now().to_sec()
start_now = False
if rospy.has_param('~start_time'):
start_time = rospy.get_param('~start_time')
if start_time < 0.0:
print 'Negative start time, setting it to 0.0'
start_time = 0.0
start_now = True
else:
start_now = True
param_labels = ['radius', 'center', 'n_points', 'heading_offset',
'duration', 'n_turns', 'delta_z', 'max_forward_speed']
params = dict()
for label in param_labels:
if not rospy.has_param('~' + label):
print '%s must be provided for the trajectory generation!' % label
# limitations under the License.
import rospy
import sys
from uuv_control_msgs.srv import *
from std_msgs.msg import String, Time
if __name__ == '__main__':
print 'Send a waypoint file, namespace=', rospy.get_namespace()
rospy.init_node('send_waypoint_file')
if rospy.is_shutdown():
print 'ROS master not running!'
sys.exit(-1)
if rospy.has_param('~filename'):
filename = rospy.get_param('~filename')
else:
raise rospy.ROSException('No filename found')
# If no start time is provided: start *now*.
start_time = rospy.Time.now().to_sec()
start_now = True
if rospy.has_param('~start_time'):
start_time = rospy.get_param('~start_time')
if start_time < 0.0:
print 'Negative start time, setting it to 0.0'
start_time = 0.0
start_now = True
else:
start_now = True
def __init__(self):
# Initializing publisher with buffer size of 10 messages
self.pub_ = rospy.Publisher("grammar_data", String, queue_size=10)
# initialize node
rospy.init_node("jsgf_control")
# Call custom function on node shutdown
rospy.on_shutdown(self.shutdown)
# Initializing rospack to find package location
rospack = rospkg.RosPack()
# Params
# Location of external files
self.location = rospack.get_path('pocketsphinx') + '/demo/'
# File containing language model
self._lm_param = "~lm"
# Dictionary
self._dict_param = "~dict"
# Hidden markov model. Default has been provided below
self._hmm_param = "~hmm"
# Gram file contains the rules and grammar
self._gram = "~gram"
# Name of rule within the grammar
self._rule = "~rule"
# check if lm or grammar mode. Default = grammar
self._use_lm = 0
# Setting param values
if rospy.has_param(self._hmm_param):
self.hmm = self.location + rospy.get_param(self._hmm_param)
if rospy.get_param(self._hmm_param) == ":default":
if os.path.isdir("/usr/local/share/pocketsphinx/model"):
rospy.loginfo("Loading the default acoustic model")
self.hmm = "/usr/local/share/pocketsphinx/model/en-us/en-us"
rospy.loginfo("Done loading the default acoustic model")
else:
rospy.logerr(
"No language model specified. Couldn't find default model."
)
return
else:
rospy.logerr(
"No language model specified. Couldn't find default model.")
return
if rospy.has_param(self._dict_param) and rospy.get_param(
self._dict_param) != ":default":
self.dict = self.location + rospy.get_param(self._dict_param)
else:
rospy.logerr(
"No dictionary found. Please add an appropriate dictionary argument."
)
return
if rospy.has_param(self._lm_param) and rospy.get_param(
self._lm_param) != ':default':
self._use_lm = 1
self.lm = self.location + rospy.get_param(self._lm_param)
elif rospy.has_param(self._gram) and rospy.has_param(self._rule):
self._use_lm = 0
self.gram = rospy.get_param(self._gram)
self.rule = rospy.get_param(self._rule)
else:
rospy.logerr(
"Couldn't find suitable parameters. Please take a look at the documentation"
)
return
# All params satisfied. Starting recognizer
self.start_recognizer()
def load_parameters(self):
if not rospy.has_param("~topics/y"):
return False
self._output_derivs_topic = rospy.get_param("~topics/y")
if not rospy.has_param("~topics/x"):
return False
self._state_topic = rospy.get_param("~topics/x")
if not rospy.has_param("~topics/params"):
return False
self._params_topic = rospy.get_param("~topics/params")
if not rospy.has_param("~topics/u"):
return False
self._control_topic = rospy.get_param("~topics/u")
if not rospy.has_param("~topics/ref"):
return False
self._ref_topic = rospy.get_param("~topics/ref")
if not rospy.has_param("~topics/transitions"):
return False
self._transitions_topic = rospy.get_param("~topics/transitions")
if not rospy.has_param("~topics/linear_system_reset"):
return False
self._reset_topic = rospy.get_param("~topics/linear_system_reset")
if not rospy.has_param("~frames/fixed"):
return False
self._fixed_frame = rospy.get_param("~frames/fixed")
if not rospy.has_param("~viz/reference"):
return False
self._ref_viz_topic = rospy.get_param("~viz/reference")
if not rospy.has_param("~dynamics/m"):
return False
m = rospy.get_param("~dynamics/m")
if not rospy.has_param("~dynamics/Ix"):
return False
Ix = rospy.get_param("~dynamics/Ix")
if not rospy.has_param("~dynamics/Iy"):
return False
Iy = rospy.get_param("~dynamics/Iy")
if not rospy.has_param("~dynamics/Iz"):
return False
Iz = rospy.get_param("~dynamics/Iz")
self._dynamics = Quadrotor14D(m, Ix, Iy, Iz)
return True
if not data.success:
cls.alertEvent.set()
return None
# Notify the program that an alert has been received.
if "alert" in output_topic:
cls.alertEvent.set()
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("publisherTopic"):
publisherTopic = rospy.get_param("publisherTopic")
else:
rospy.logfatal("Could not find the Point Cloud 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
tf.child_frame_id = self.child_frame
tf_msg.transforms.append(tf)
self.tf_pub.publish(tf_msg)
def normalize_quaternion_msg(self, quaternion):
q = Quaternion()
rotation = np.array(
[quaternion.x, quaternion.y, quaternion.z, quaternion.w])
normalized_rotation = rotation / np.linalg.norm(rotation)
q.x = normalized_rotation[0]
q.y = normalized_rotation[1]
q.z = normalized_rotation[2]
q.w = normalized_rotation[3]
return q
if __name__ == "__main__":
rospy.init_node("map_odom_transform_publisher")
while not rospy.has_param('~child_frame') or not rospy.has_param(
'~parent_frame'):
if (rospy.is_shutdown()):
sys.exit(0)
print(
'waiting for param {0}/child_frame and/or {0}/parent_frame'.format(
rospy.get_name()))
rospy.sleep(1)
child_frame = rospy.get_param('~child_frame', "")
parent_frame = rospy.get_param('~parent_frame', "")
TransformPublisher(child_frame, parent_frame, '~update_map_odom_transform')
rospy.spin()
class ThrusterManager:
"""
The thruster manager generates the thruster allocation matrix using the
TF information and publishes the thruster forces assuming the the thruster
topics are named in the following pattern
<thruster_topic_prefix>/<index>/<thruster_topic_suffix>
Thruster frames should also be named as follows
<thruster_frame_base>_<index>
"""
MAX_THRUSTERS = 16
def __init__(self):
"""Class constructor."""
# This flag will be set to true once the thruster allocation matrix is
# available
self._ready = False
# Acquiring the namespace of the vehicle
self.namespace = rospy.get_namespace()
if self.namespace[-1] != '/':
self.namespace += '/'
if self.namespace[0] != '/':
self.namespace = '/' + self.namespace
if not rospy.has_param('thruster_manager'):
raise rospy.ROSException('Thruster manager parameters not '
'initialized for uuv_name=' +
self.namespace)
# Load all parameters
self.config = rospy.get_param('thruster_manager')
if self.config['update_rate'] < 0:
self.config['update_rate'] = 50
self.base_link_ned_to_enu = None
tf_buffer = tf2_ros.Buffer()
listener = tf2_ros.TransformListener(tf_buffer)
tf_trans_ned_to_enu = None
try:
target = 'base_link'
source = 'base_link_ned'
tf_trans_ned_to_enu = tf_buffer.lookup_transform(
target, source, rospy.Time(), rospy.Duration(1))
except Exception, e:
rospy.loginfo('No transform found between base_link and base_link_ned'
' for vehicle ' + self.namespace)
rospy.loginfo(str(e))
self.base_link_ned_to_enu = None
if tf_trans_ned_to_enu is not None:
self.base_link_ned_to_enu = trans.quaternion_matrix(
(tf_trans_ned_to_enu.transform.rotation.x,
tf_trans_ned_to_enu.transform.rotation.y,
tf_trans_ned_to_enu.transform.rotation.z,
tf_trans_ned_to_enu.transform.rotation.w))[0:3, 0:3]
rospy.loginfo('base_link transform NED to ENU=\n' + str(self.base_link_ned_to_enu))
rospy.loginfo(
'ThrusterManager::update_rate=' + str(self.config['update_rate']))
# Retrieve the output file path to store the TAM
# matrix for future use
self.output_dir = None
if rospy.has_param('~output_dir'):
self.output_dir = rospy.get_param('~output_dir')
if not isdir(self.output_dir):
raise rospy.ROSException(
'Invalid output directory, output_dir=' + self.output_dir)
rospy.loginfo('output_dir=' + self.output_dir)
# Number of thrusters
self.n_thrusters = 0
# Thruster objects used to calculate the right angular velocity command
self.thrusters = list()
# Thrust forces vector
self.thrust = None
# Thruster allocation matrix: transform thruster inputs to force/torque
self.configuration_matrix = None
if rospy.has_param('~tam'):
tam = rospy.get_param('~tam')
self.configuration_matrix = numpy.array(tam)
# Set number of thrusters from the number of columns
self.n_thrusters = self.configuration_matrix.shape[1]
# Create publishing topics to each thruster
params = self.config['conversion_fcn_params']
conv_fcn = self.config['conversion_fcn']
if type(params) == list and type(conv_fcn) == list:
if len(params) != self.n_thrusters or len(conv_fcn) != self.n_thrusters:
raise rospy.ROSException('Lists conversion_fcn and '
'conversion_fcn_params must have '
'the same number of items as of '
'thrusters')
for i in range(self.n_thrusters):
topic = self.config['thruster_topic_prefix'] + str(i) + \
self.config['thruster_topic_suffix']
if list not in [type(params), type(conv_fcn)]:
thruster = Thruster.create_thruster(
conv_fcn, i, topic, None, None,
**params)
else:
thruster = Thruster.create_thruster(
conv_fcn[i], i, topic, None, None,
**params[i])
if thruster is None:
rospy.ROSException('Invalid thruster conversion '
'function=%s'
% self.config['conversion_fcn'])
self.thrusters.append(thruster)
rospy.loginfo('Thruster allocation matrix provided!')
rospy.loginfo('TAM=')
rospy.loginfo(self.configuration_matrix)
self.thrust = numpy.zeros(self.n_thrusters)
if not self.update_tam():
raise rospy.ROSException('No thrusters found')
# (pseudo) inverse: force/torque to thruster inputs
self.inverse_configuration_matrix = None
if self.configuration_matrix is not None:
self.inverse_configuration_matrix = numpy.linalg.pinv(
self.configuration_matrix)
# If an output directory was provided, store matrix for further use
if self.output_dir is not None:
with open(join(self.output_dir, 'TAM.yaml'), 'w') as yaml_file:
yaml_file.write(
yaml.safe_dump(
dict(tam=self.configuration_matrix.tolist())))
else:
rospy.loginfo('Invalid output directory for the TAM matrix, dir=' + str(self.output_dir))
self.ready = True
rospy.loginfo('ThrusterManager: ready')
def __init__(self):
# Timeout (to filter out inactivity)
self._timeout = 0.5
# Initializing the arm interface for the manipulator in the current
# namespace
self._arm_interface = ArmInterface()
# Last goal for the end-effector pose/position
self._last_goal = self._arm_interface.get_config_in_ee_frame('home')
# Retrieve the publish rate
self._publish_rate = 25
if not rospy.has_param('~publish_rate'):
self._publish_rate = rospy.get_param('~publish_rate')
if self._publish_rate <= 0:
raise rospy.ROSException('Invalid negative publish rate')
self._tau = 0.1
# Get the transformation between the robot's base link and the
# vehicle's base link
self.listener = tf.TransformListener()
# Get latest transform available
latest = rospy.Time(0)
base = self._arm_interface.namespace + 'base_link'
self.listener.waitForTransform(base, self._arm_interface.base_link,
latest, latest + rospy.Duration(100))
[pos, quat] = self.listener.lookupTransform(
base, self._arm_interface.base_link, latest)
rot = PyKDL.Rotation.Quaternion(quat[0], quat[1], quat[2], quat[3])
# Store transformation from the arm's base link and base
self._trans = PyKDL.Frame(rot, PyKDL.Vector(pos[0], pos[1], pos[2]))
# Velocity reference
self._command = None
# Last controller update
self._last_controller_update = rospy.get_time()
# Last velocity reference update time stamp
self._last_reference_update = rospy.get_time()
rospy.set_param('~name', self.LABEL)
self._joint_effort_pub = dict()
for joint in self._arm_interface.joint_names:
self._joint_effort_pub[joint] = rospy.Publisher(
self._arm_interface.namespace +
joint + '/controller/command',
Float64, queue_size=1)
# Input velocity command subscriber, remap this topic to set a custom
# command topic
self._vel_command_sub = rospy.Subscriber(
'cmd_vel', Twist, self._vel_command_callback)
# Topic that will receive the flag if the home button was pressed. If
# the flag is true, the manipulator's goal is set to the stow
# configuration
self._home_pressed_sub = rospy.Subscriber(
'home_pressed', Bool, self._home_button_pressed)
# Topic publishes the current goal set as reference to the manipulator
self._goal_pub = rospy.Publisher(
'reference', PoseStamped, queue_size=1)
# Topic to publish a visual marker for visualization of the current
# reference in RViz
self._goal_marker_pub = rospy.Publisher(
'reference_marker', Marker, queue_size=1)
def __init__(self):
self.mutex = Lock()
self.srv_mutex = Lock()
################################################################################################################
self.knowledge_base = '/rosplan_knowledge_base'
if rospy.has_param('~knowledge_base'):
self.knowledge_base = rospy.get_param('~knowledge_base')
# Init clients and publishers
rospy.wait_for_service(self.knowledge_base + '/update_array')
self.update_kb_srv = rospy.ServiceProxy(
self.knowledge_base + '/update_array', KnowledgeUpdateServiceArray)
rospy.wait_for_service(self.knowledge_base +
'/domain/predicate_details')
self.get_predicates_srv = rospy.ServiceProxy(
self.knowledge_base + '/domain/predicate_details',
GetDomainPredicateDetailsService)
rospy.wait_for_service(self.knowledge_base + '/domain/functions')
self.get_functions_srv = rospy.ServiceProxy(
self.knowledge_base + '/domain/functions',
GetDomainAttributeService)
rospy.wait_for_service(self.knowledge_base + '/state/instances')
self.get_instances_srv = rospy.ServiceProxy(
self.knowledge_base + '/state/instances', GetInstanceService)
rospy.wait_for_service(self.knowledge_base + '/state/propositions')
self.get_state_propositions_srv = rospy.ServiceProxy(
self.knowledge_base + '/state/propositions', GetAttributeService)
rospy.wait_for_service(self.knowledge_base + '/state/functions')
self.get_state_functions_srv = rospy.ServiceProxy(
self.knowledge_base + '/state/functions', GetAttributeService)
self.set_sensed_predicate_srv = rospy.ServiceProxy(
self.knowledge_base + '/update_sensed_predicates', SetNamedBool)
################################################################################################################
# Get cfg
self.cfg_topics = self.cfg_service = {}
self.functions_path = None
found_config = False
if rospy.has_param('~topics'):
self.cfg_topics = rospy.get_param('~topics')
found_config = True
if rospy.has_param('~services'):
self.cfg_service = rospy.get_param('~services')
found_config = True
if rospy.has_param('~functions'):
self.functions_path = rospy.get_param('~functions')[0]
regexp = re.compile('\$\(find (.*)\)')
groups = regexp.match(self.functions_path).groups()
if len(groups):
try:
ros_pkg_path = rospkg.RosPack().get_path(groups[0])
self.functions_path = regexp.sub(ros_pkg_path,
self.functions_path)
except:
rospy.logerr(
'KCL: (RosplanSensing) Error: Package %s was not found! Fix configuration file and retry.'
% groups[0])
rospy.signal_shutdown('Wrong path in cfg file')
return
if not found_config:
rospy.logerr(
'KCL: (RosplanSensing) Error: configuration file is not defined!'
)
rospy.signal_shutdown('Config not found')
return
################################################################################################################
# Load scripts
if self.functions_path:
self.scripts = imp.load_source('sensing_scripts',
self.functions_path)
# Declare tools in the scripts module:
self.scripts.get_kb_attribute = self.get_kb_attribute
self.scripts.rospy = rospy
################################################################################################################
# Init variables
self.sensed_topics = {}
self.sensed_services = {}
self.params = {} # Params defined for each predicate
######
# Subscribe to all the topics
self.offset = {} # Offset for reading cfg
for predicate_name, predicate_info in self.cfg_topics.iteritems():
if type(predicate_info
) is list: # We have nested elements in the predicate
for i, pi in enumerate(predicate_info):
pi['sub_idx'] = i
subscribed = self.subscribe_topic(predicate_name, pi)
if not subscribed:
rospy.loginfo('Could not subscribe for predicate ' +
predicate_name + ' and config: ' +
str(pi))
continue
else:
predicate_info[
'sub_idx'] = 0 # As we don't have nested elements in this predicate
subscribed = self.subscribe_topic(predicate_name,
predicate_info)
if not subscribed:
rospy.loginfo('Could not subscribe for predicate ' +
predicate_name + ' and config: ' +
str(predicate_info))
############
# Create clients for all the services
self.service_clients = []
self.service_names = []
self.service_type_names = []
self.service_predicate_names = []
self.last_called_time = []
self.time_between_calls = []
self.request_src = []
self.response_process_src = []
self.clients_sub_idx = []
for predicate_name, predicate_info in self.cfg_service.iteritems():
if type(predicate_info) is list:
for i, pi in enumerate(predicate_info):
pi['sub_idx'] = i
client_created = self.create_service_client(
predicate_name, pi)
if not client_created:
rospy.loginfo(
'Could not create client for predicate ' +
predicate_name + ' and config: ' + str(pi))
continue
else:
predicate_info[
'sub_idx'] = 0 # As we don't have nested elements in this predicate
client_created = self.create_service_client(
predicate_name, predicate_info)
if not client_created:
rospy.loginfo('Could not create client for predicate ' +
predicate_name + ' and config: ' +
str(predicate_info))
continue
def __init__(self):
# initialize ROS
self.speed = 0.2
# Start node
rospy.init_node("recognizer")
rospy.on_shutdown(self.shutdown)
self._lm_param = "~lm"
self._dict_param = "~dict"
self._kws_param = "~kws"
self._stream_param = "~stream"
self._wavpath_param = "~wavpath"
# you may need to change publisher destination depending on what you run
self.pub_ = rospy.Publisher('~output', String, queue_size=1)
if rospy.has_param(self._lm_param):
print("here for _lm param: ", rospy.get_param(self._lm_param))
self.lm = rospy.get_param(self._lm_param)
else:
rospy.loginfo("Loading the default acoustic model")
self.lm = "/usr/share/pocketsphinx/model/hmm/en_US/hub4wsj_sc_8k"
rospy.loginfo("Done loading the default acoustic model")
if rospy.has_param(self._dict_param):
print("here for _dict param: ", rospy.get_param(self._dict_param))
self.lexicon = rospy.get_param(self._dict_param)
else:
rospy.logerr(
'No dictionary found. Please add an appropriate dictionary argument.'
)
return
if rospy.has_param(self._kws_param):
print("here for _kws param: ", rospy.get_param(self._kws_param))
self.kw_list = rospy.get_param(self._kws_param)
else:
rospy.logerr(
'kws cant run. Please add an appropriate keyword list file.')
return
if rospy.has_param(self._stream_param):
print("here for _stream param: ",
rospy.get_param(self._stream_param))
self.is_stream = rospy.get_param(self._stream_param)
if not self.is_stream:
if rospy.has_param(self._wavpath_param):
self.wavpath = rospy.get_param(self._wavpath_param)
if self.wavpath == "none":
rospy.logerr(
'Please set the wav path to the correct file location'
)
else:
rospy.logerr('No wav file is set')
else:
rospy.logerr(
'Audio is not set to a stream (true) or wav file (false).')
self.is_stream = rospy.get_param(self._stream_param)
self.start_recognizer()
def __init__(self, rover):
self.rover = rover
rospy.init_node(self.rover + '_IMU')
if rospy.has_param('~imu_mode'): # if respawning
self._get_mode()
else:
self.current_mode = IMU.MODE_3D # default to 3D mode
self.current_state = IMU.STATE_IDLE # idle until data file is loaded
self.gyro_timer = None
self.gyro_start_time = None
self.gyro_status_msg = ''
self.cal = {}
self.roll = 0
self.pitch = 0
self.yaw = 0
# used during file validation
self.rolls = []
self.pitches = []
# Default param values. Set to final values after validating
self.finished_validating = False
self.needs_calibration = False
self.DEBUG = rospy.get_param('~publish_debug_topic', default=False)
self.LOAD_RAW_DATA = rospy.get_param('~load_raw_data', default=False)
self.RAW_DATA_PATH = rospy.get_param(
'~raw_data_path',
default='/home/swarmie/KSC_extended_calibration.csv')
# Raw data collected while in a calibration state is stored in a list
# of lists, which is converted to a numpy array when needed.
self.mag_data = [[], [], []]
self.gyro_data = [[], [], []]
# Default matrices
self.acc_offsets = [[0], [0], [0]]
self.acc_transform = [[1., 0, 0], [0, 1., 0], [0, 0, 1.]]
self.mag_offsets = [[0], [0], [0]]
self.mag_transform = [[1., 0, 0], [0, 1., 0], [0, 0, 1.]]
self.misalignment = [[1., 0, 0], [0, 1., 0], [0, 0, 1.]]
self.gyro_bias = [[0], [0], [0]]
self.gyro_scale = [[1., 0, 0], [0, 1., 0], [0, 0, 1.]]
# Subscribers
self.imu_raw_sub = rospy.Subscriber(self.rover + '/imu/raw',
SwarmieIMU,
self.imu_callback,
queue_size=10)
# Publishers
self.imu_pub = rospy.Publisher(self.rover + '/imu', Imu, queue_size=10)
self.imu_diag_pub = rospy.Publisher(self.rover + '/imu/cal_diag',
DiagnosticArray,
queue_size=10,
latch=True)
if self.DEBUG:
self.imu_cal_data_pub = rospy.Publisher(self.rover +
'/imu/raw/calibrated',
SwarmieIMU,
queue_size=10)
self.info_log = rospy.Publisher('/infoLog', String, queue_size=10)
self.diags_log = rospy.Publisher('/diagsLog',
String,
queue_size=10,
latch=True)
# Services
self.start_imu_cal = rospy.Service(
self.rover + '/start_imu_calibration', Empty,
self.start_imu_calibration)
self.store_cal = rospy.Service(self.rover + '/store_imu_calibration',
Empty, self.store_calibration)
self.start_misalign_cal = rospy.Service(
self.rover + '/start_misalignment_calibration', Empty,
self.start_misalignment_calibration)
self.start_gyro_bias_cal = rospy.Service(
self.rover + '/start_gyro_bias_calibration', Empty,
self.start_gyro_bias_calibration)
self.start_gyro_scale_cal = rospy.Service(
self.rover + '/start_gyro_scale_calibration', Empty,
self.start_gyro_scale_calibration)
self._is_finished_val = rospy.Service(
self.rover + '/imu/is_finished_validating', QueryCalibrationState,
self._is_finished_validating)
self._needs_cal = rospy.Service(self.rover + '/imu/needs_calibration',
QueryCalibrationState,
self._needs_calibration)
# Try waiting for subscriber on /diagsLog. Helps to make sure first
# message or two actually make it onto the rqt gui.
rate = rospy.Rate(10)
for i in range(20):
if self.diags_log.get_num_connections() > 0:
break
rate.sleep()
# If node is respawning for some reason
if rospy.has_param('~imu_calibration_matrices'):
self.cal = rospy.get_param('~imu_calibration_matrices')
self._get_mode()
self.acc_offsets = self.cal['acc_offsets']
self.acc_transform = self.cal['acc_transform']
self.mag_offsets = self.cal['mag_offsets']
self.mag_transform = self.cal['mag_transform']
self.misalignment = self.cal['misalignment']
self.gyro_bias = self.cal['gyro_bias']
self.gyro_scale = self.cal['gyro_scale']
self.current_state = IMU.STATE_NORMAL
self.finished_validating = True
self.needs_calibration = False
msg = self.rover + ': reloaded calibration matrices after respawn.'
if self.current_mode == IMU.MODE_2D:
msg += ' Using 2D mode.'
elif self.current_mode == IMU.MODE_3D:
msg += ' Using 3D mode.'
rospy.loginfo(msg)
self.diags_log.publish(msg)
elif self.LOAD_RAW_DATA:
self.load_and_validate_calibration()
# Publish current calibration once:
self.publish_diagnostic_msg()
g_vel_scales = [0.1, 0.1] # default to very slow
def keys_cb(msg, twist_pub):
global g_last_twist, g_vel_scales
if len(msg.data) == 0 or msg.data[0] not in key_mapping.keys():
return # unknown key
vels = key_mapping[msg.data[0]]
g_last_twist.angular.z = vels[0] * g_vel_scales[0]
g_last_twist.linear.x = vels[1] * g_vel_scales[1]
twist_pub.publish(g_last_twist)
if __name__ == '__main__':
rospy.init_node('keys_to_twist')
twist_pub = rospy.Publisher('cmd_vel', Twist, queue_size=1)
rospy.Subscriber('keys', String, keys_cb, twist_pub)
g_last_twist = Twist() # initializes to zero
if rospy.has_param('~linear_scale'):
g_vel_scales[1] = rospy.get_param('~linear_scale')
else:
rospy.logwarn("linear scale not provided; using %.1f" % g_vel_scales[1])
if rospy.has_param('~angular_scale'):
g_vel_scales[0] = rospy.get_param('~angular_scale')
else:
rospy.logwarn("angular scale not provided; using %.1f" % g_vel_scales[0])
rate = rospy.Rate(10)
while not rospy.is_shutdown():
twist_pub.publish(g_last_twist)
rate.sleep()
def __init__(self, name, default=None, help=None, param_type=ParamType.UNKNOWN,
min_value=None, max_value=None, __editable__=True):
self._name = rospy.names.resolve_name(name)
self._help = help
self._editable = __editable__
if not isinstance(param_type, ParamType):
raise ValueError(
"Parameter 'param_type' must be an instance of duckietown.ParamType. "
'Got %s instead.' % str(type(param_type))
)
self._type = param_type
self._update_listeners = []
# parse optional args
# - min value
if min_value is not None and param_type not in MIN_MAX_SUPPORTED_TYPES:
raise ValueError(
"Parameter 'min_value' not supported for parameter of type '%s'." % param_type.name
)
self._min_value = ParamType.parse(param_type, min_value)
# - max value
if max_value is not None and param_type not in MIN_MAX_SUPPORTED_TYPES:
raise ValueError(
"Parameter 'max_value' not supported for parameter of type '%s'." % param_type.name
)
self._max_value = ParamType.parse(param_type, max_value)
# - default value
self._default_value = ParamType.parse(param_type, default)
if self._default_value is not None:
# verify lower-bound
if self._min_value is not None and self._default_value < self._min_value:
raise ValueError(
"Given default value %s is below the min_value %s for parameter '%s'" % (
str(self._default_value), str(self._min_value), name
)
)
# verify upper-bound
if self._max_value is not None and self._default_value > self._max_value:
raise ValueError(
"Given default value %s is above the max_value %s for parameter '%s'" % (
str(self._default_value), str(self._max_value), name
)
)
# - help string
if help is not None and not isinstance(help, str):
raise ValueError(
"Parameter 'help' in DTParam expects a value of type 'str', got '%s' instead." % (
str(type(help))
)
)
# ---
node = get_instance()
if node is None:
raise ValueError(
'You cannot create a DTParam object before initializing a DTROS object'
)
# get parameter value
if rospy.has_param(self._name):
self._value = rospy.__get_param__(self._name)
else:
self._value = self._default_value
rospy.set_param(self._name, self._default_value)
# add param to current node
node._add_param(self)
# register for changes (only for editable parameters)
if self._editable:
rospy.get_master().target.subscribeParam(
rospy.names.get_caller_id(),
rospy.core.get_node_uri(),
self._name
)
rospy.logdebug('Parameter "%s" was registered for updates' % self._name)
# register node against diagnostics
if DTROSDiagnostics.enabled():
DTROSDiagnostics.getInstance().register_param(
self._name,
self._help,
self._type,
self._min_value,
self._max_value,
self._editable
)
from numpy import pi
from uuv_world_ros_plugins_msgs.srv import *
if __name__ == '__main__':
print('Starting current perturbation node')
rospy.init_node('set_gm_current_perturbation')
print('Programming the generation of a current perturbation')
if rospy.is_shutdown():
print('ROS master not running!')
sys.exit(-1)
params = ['component', 'mean', 'min', 'max', 'noise', 'mu']
values = dict()
for p in params:
assert rospy.has_param('~' + p)
values[p] = rospy.get_param('~' + p)
assert values['component'] in ['velocity', 'horz_angle', 'vert_angle']
if values['component'] == 'velocity':
assert values['mean'] > 0
else:
values['min'] *= pi / 180.0
values['max'] *= pi / 180.0
values['mean'] *= pi / 180.0
assert values['min'] < values['max']
assert values['noise'] >= 0
assert values['mu'] >= 0
rospy.wait_for_service('/hydrodynamics/set_current_%s_model' %
def main():
n = rospy.init_node('joint_position_tester')
is_simulation = True
if rospy.has_param('~is_simulation'):
is_simulation = rospy.get_param("~is_simulation", "true")
rospy.logwarn("Kinematics Node is in " + (
"Simulation Mode" if is_simulation else "Real Robot Mode"))
else:
rospy.logwarn(
"No argument input to indicate simulation or real robot.")
arm = Arm('r_arm', n, is_simulation)
joint_angle_d = [0.0] * arm.joint_num
joint_angle_d[4] = np.pi / 2
arm.move_to_joint_angle(joint_angle_d, 0, 3, 100)
print("move to home position!")
rospy.sleep(2)
# angles = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
# arm.move_to_joint_angle(angles, 3, 100)
# print("move to working home position!")
# rospy.sleep(3)
# print("current {} pose:".format('end'))
# arm.intra_joint_angle[2] = 0.2
# T = arm.forward_kinematics(arm.intra_joint_angle, 'jaw', 'r')
# print(T)
# T[1, 3] -= 0.01
# print('pose: {}'.format(T))
# # arm.move_to_pose(T, 2, 200, 'c')
# print("original intra joint angles: {}".format(arm.intra_joint_angle))
# ij, _ = arm.inverse_kinematics(T, arm.intra_joint_angle, frame='jaw', ref='r')
# print("intra joint angles: {}".format(ij))
T = arm.forward_kinematics(arm.joint_angle, 'jaw', 's')
print('jaw pose: {}'.format(T))
print('jaw quaternion: {}'.format(Quaternion(matrix=T)))
T = arm.forward_kinematics(arm.joint_angle[:arm.robot_dof], 'end', 's')
print('end pose: {}'.format(T))
# print("original joint angles: {}".format(arm.joint_angle))
# T[2, 3] -= 0.1
# # T[0, 3] -= 0.01
# print('pose2: {}'.format(T))
# tar_joint_angle = arm.inverse_kinematics_rcm(T)
# # print("target joint angles: {}".format(tar_joint_angle))
# # angles = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
# arm.move_to_joint_angle(tar_joint_angle, 3, 100)
#
#
# print("original joint angles: {}".format(arm.joint_angle))
# T[0, 3] -= 0.05
# # T[0, 3] -= 0.01
# print('pose2: {}'.format(T))
# tar_joint_angle = arm.inverse_kinematics_rcm(T)
# # print("target joint angles: {}".format(tar_joint_angle))
# # angles = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
# arm.move_to_joint_angle(tar_joint_angle, 3, 100)
#
#
# print("original joint angles: {}".format(arm.joint_angle))
# T[0, 3] += 0.1
# # T[0, 3] -= 0.01
# print('pose2: {}'.format(T))
# tar_joint_angle = arm.inverse_kinematics_rcm(T)
# # print("target joint angles: {}".format(tar_joint_angle))
# # angles = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
# arm.move_to_joint_angle(tar_joint_angle, 3, 100)
#
#
# print("original joint angles: {}".format(arm.joint_angle))
# T[0, 3] -= 0.05
# T[1, 3] -= 0.05
# # T[0, 3] -= 0.01
# print('pose2: {}'.format(T))
# tar_joint_angle = arm.inverse_kinematics_rcm(T)
# # print("target joint angles: {}".format(tar_joint_angle))
# # angles = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
# arm.move_to_joint_angle(tar_joint_angle, 3, 100)
#
# print("original joint angles: {}".format(arm.joint_angle))
# T[1, 3] += 0.05
# # T[0, 3] -= 0.01
# print('pose2: {}'.format(T))
# tar_joint_angle = arm.inverse_kinematics_rcm(T)
# # print("target joint angles: {}".format(tar_joint_angle))
# # angles = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
# arm.move_to_joint_angle(tar_joint_angle, 3, 100)
# T[2, 3] += 0.05
# # T[1, 3] -= 0.05
# arm.move_to_pose(T, 0.5, 2, 50, 'c')
# rospy.sleep(0.5)
# print("send goal")
# goal = FollowJointTrajectoryActionGoal()
# arm.jta_servo.send_goal_and_wait(goal)
# print("goal sent")
# T[0, 3] -= 0.03
# arm.move_to_pose(T, 2, 50, 'c')
# rospy.sleep(0.5)
#
# T[0, 3] += 0.06
# arm.move_to_pose(T, 2, 50, 'c')
#
# T[0, 3] += 0.05
# arm.move_to_pose(T, 2, 50, 'j')
#
# T[0, 3] -= 0.05
# T[1, 3] -= 0.05
# arm.move_to_pose(T, 2, 50, 'c')
#
# T[0, 3] -= 0.05
# T[1, 3] += 0.05
# arm.move_to_pose(T, 2, 50, 'c')
rospy.spin()
if eva_dis!=-1 and (current_pos[0]**2+current_pos[1]**2) > eva_dis and not shuttingdown:
print '[evaluate]: dis > evadis-----------success!'
success = True
shuttingdown = True
shutdown()
def go_callback(msg):
global go
if not go:
go=True
print('[evaluate] go.')
if __name__=="__main__":
rospy.init_node('evaluate', anonymous=True)
## create necessary directories
if rospy.has_param('delay_evaluation'):
delay_evaluation=rospy.get_param('delay_evaluation')
if rospy.has_param('flight_duration'):
flight_duration=rospy.get_param('flight_duration')
if rospy.has_param('min_allowed_distance'):
min_allowed_distance=rospy.get_param('min_allowed_distance')
if rospy.has_param('starting_height'):
starting_height=rospy.get_param('starting_height')
if rospy.has_param('eva_dis'):
eva_dis=rospy.get_param('eva_dis')
if rospy.has_param('log_folder'):
log_folder=rospy.get_param('log_folder')
else:
log_folder = '/tmp/log'
if rospy.has_param('pidfile'):
pidfile=rospy.get_param('pidfile')
def __init__(self,
eval=False,
launch_file=['rktl_autonomy', 'rocket_league_train.launch'],
launch_args=[],
run_id=None):
super().__init__(node_name='rocket_league_agent',
eval=eval,
launch_file=launch_file,
launch_args=launch_args,
run_id=run_id)
## Constants
# Actions
self._MIN_VELOCITY = -rospy.get_param('/cars/throttle/max_speed')
self._MAX_VELOCITY = rospy.get_param('/cars/throttle/max_speed')
self._MIN_CURVATURE = -tan(rospy.get_param(
'/cars/steering/max_throw')) / rospy.get_param('cars/length')
self._MAX_CURVATURE = tan(rospy.get_param(
'/cars/steering/max_throw')) / rospy.get_param('cars/length')
# Action space overrides
if rospy.has_param('~action_space/velocity/min'):
min_velocity = rospy.get_param('~action_space/velocity/min')
assert min_velocity > self._MIN_VELOCITY
self._MIN_VELOCITY = min_velocity
if rospy.has_param('~action_space/velocity/max'):
max_velocity = rospy.get_param('~action_space/velocity/max')
assert max_velocity < self._MAX_VELOCITY
self._MAX_VELOCITY = max_velocity
if rospy.has_param('~action_space/curvature/min'):
min_curvature = rospy.get_param('~action_space/curvature/min')
assert min_curvature > self._MIN_CURVATURE
self._MIN_CURVATURE = min_curvature
if rospy.has_param('~action_space/curvature/max'):
max_curvature = rospy.get_param('~action_space/curvature/max')
assert max_curvature < self._MAX_CURVATURE
self._MAX_CURVATURE = max_curvature
# Observations
self._FIELD_WIDTH = rospy.get_param('/field/width')
self._FIELD_LENGTH = rospy.get_param('/field/length')
self._GOAL_DEPTH = rospy.get_param('~observation/goal_depth', 0.075)
self._MAX_OBS_VEL = rospy.get_param('~observation/velocity/max_abs',
3.0)
self._MAX_OBS_ANG_VEL = rospy.get_param(
'~observation/angular_velocity/max_abs', 2 * pi)
# Learning
self._MAX_TIME = rospy.get_param('~max_episode_time', 30.0)
self._CONSTANT_REWARD = rospy.get_param('~reward/constant', 0.0)
self._BALL_DISTANCE_REWARD = rospy.get_param('~reward/ball_dist_sq',
0.0)
self._GOAL_DISTANCE_REWARD = rospy.get_param('~reward/goal_dist_sq',
0.0)
self._WIN_REWARD = rospy.get_param('~reward/win', 100.0)
self._LOSS_REWARD = rospy.get_param('~reward/loss', 0.0)
self._REVERSE_REWARD = rospy.get_param('~reward/reverse', 0.0)
self._WALL_REWARD = rospy.get_param('~reward/walls/value', 0.0)
self._WALL_THRESHOLD = rospy.get_param('~reward/walls/threshold', 0.0)
# Publishers
self._command_pub = rospy.Publisher('cars/car0/command',
ControlCommand,
queue_size=1)
self._reset_srv = rospy.ServiceProxy('sim_reset', Empty)
# State variables
self._car_odom = None
self._ball_odom = None
self._score = None
self._start_time = None
# Subscribers
rospy.Subscriber('cars/car0/odom', Odometry, self._car_odom_cb)
rospy.Subscriber('ball/odom', Odometry, self._ball_odom_cb)
rospy.Subscriber('match_status', MatchStatus, self._score_cb)
# block until environment is ready
rospy.wait_for_service('sim_reset')
def __init__(self):
rospy.init_node("STT_online_node")
self.socketIO = SocketIO('http://127.0.0.1', 5000, LoggingNamespace)
if rospy.has_param("~stt_online_server_name"):
self.action_server = actionlib.SimpleActionServer(
rospy.get_param("~stt_online_server_name"),
speechToTextPalbator.msg.SttOnlineAction,
self.handle_server_callback,
auto_start=False)
self.action_server_feedback = speechToTextPalbator.msg.SttOnlineFeedback(
)
self.action_server_result = speechToTextPalbator.msg.SttOnlineResult(
)
else:
rospy.logerr(
"{class_name} : No name specified for STT online server.".
format(class_name=self.__class__.__name__))
self.current_directory = os.path.dirname(os.path.realpath(__file__))
if rospy.has_param('~config_recorder'):
self.config_recorder = rospy.get_param("~config_recorder")
else:
rospy.logerr(
"{class_name} : No configuration specified for audio recorder".
format(class_name=self.__class__.__name__))
self.audio_dir = self.config_recorder['audio_record_dir']
if rospy.has_param('~config'):
self.config = rospy.get_param('~config')
else:
rospy.logerr(
"{class_name} : No configuration specified for Speech Recognition Server"
.format(class_name=self.__class__.__name__))
if rospy.has_param("~parser_action_database"):
self.parser_action_database = rospy.get_param(
"~parser_action_database")
else:
rospy.logerr(
"{class_name} : No configuration specified for parser action database"
.format(class_name=self.__class__.__name__))
if rospy.has_param("~tts_server_name"):
self.client_TTS = actionlib.SimpleActionClient(
rospy.get_param("~tts_server_name"),
ttsMimic.msg.TtsMimicAction)
rospy.loginfo("{class_name} : Waiting for TTS server ...".format(
class_name=self.__class__.__name__))
self.client_TTS.wait_for_server()
rospy.loginfo("{class_name} : TTS connected".format(
class_name=self.__class__.__name__))
else:
rospy.logerr(
"{class_name} : No name specified for TTS server".format(
class_name=self.__class__.__name__))
self.enable_detection = None
rospy.on_shutdown(self.shutdown)
self.action_server.start()
rospy.loginfo("{class_name} : server for STT online initiated".format(
class_name=self.__class__.__name__))
def define(self):
self.error_reason = {
3300: '输入参数不正确',
3301: '识别错误',
3302: '验证失败',
3303: '语音服务器后端问题',
3304: '请求 GPS 过大，超过限额',
3305: '产品线当前日请求数超过限额'
}
if rospy.has_param('~REG_NUM_SAMPLES'):
pass
else:
rospy.set_param('~REG_NUM_SAMPLES', 2000)
if rospy.has_param('~REG_SAMPLING_RATE'):
pass
else:
rospy.set_param('~REG_SAMPLING_RATE', 8000)
if rospy.has_param('~REG_UPPER_LEVEL'):
pass
else:
rospy.set_param('~REG_UPPER_LEVEL', 5000)
if rospy.has_param('~REG_LOWER_LEVEL'):
pass
else:
rospy.set_param('~REG_LOWER_LEVEL', 500)
if rospy.has_param('~REG_COUNT_NUM'):
pass
else:
rospy.set_param('~REG_COUNT_NUM', 20)
if rospy.has_param('~REG_SAVE_LENGTH'):
pass
else:
rospy.set_param('~REG_SAVE_LENGTH', 8)
if rospy.has_param('~REG_TIME_OUT'):
pass
else:
rospy.set_param('~REG_TIME_OUT', 60)
if rospy.has_param('~REG_NO_WORDS'):
pass
else:
rospy.set_param('~REG_NO_WORDS', 6)
if rospy.has_param('~REG_Api_Key'):
pass
else:
rospy.set_param('~REG_Api_Key', "pmUzrWcsA3Ce7RB5rSqsvQt2")
if rospy.has_param('~REG_Secrect_Key'):
pass
else:
rospy.set_param('~REG_Secrect_Key',
"d39ec848d016a8474c7c25e308b310c3")
if rospy.has_param('~REG_Grant_type'):
pass
else:
rospy.set_param('~REG_Grant_type', "client_credentials")
if rospy.has_param('~REG_Token_url'):
pass
else:
rospy.set_param('~REG_Token_url',
"https://openapi.baidu.com/oauth/2.0/token")
if rospy.has_param('~REG_Reg_url'):
pass
else:
rospy.set_param('~REG_Reg_url', "http://vop.baidu.com/server_api")
if rospy.has_param('~REG_USER_ID'):
pass
else:
rospy.set_param('~REG_USER_ID', "8168466")
if rospy.has_param('~REG_FORMAT'):
pass
else:
rospy.set_param('~REG_FORMAT', "wav")
if rospy.has_param('~REG_LAN'):
pass
else:
rospy.set_param('~REG_LAN', "zh")
if rospy.has_param('~REG_nchannel'):
pass
else:
rospy.set_param('~REG_nchannel', 1)
self.NUM_SAMPLES = rospy.get_param(
'~REG_NUM_SAMPLES') # default 2000 pyaudio内置缓冲大小
#print 'self.NUM_SAMPLES',self.NUM_SAMPLES,type(self.NUM_SAMPLES)
self.SAMPLING_RATE = rospy.get_param(
'~REG_SAMPLING_RATE') # default 8000 取样频率
#print 'self.SAMPLING_RATE',self.SAMPLING_RATE,type(self.SAMPLING_RATE)
self.UPPER_LEVEL = rospy.get_param(
'~REG_UPPER_LEVEL') # default 5000 声音保存的阈值
#print 'self.UPPER_LEVEL',self.UPPER_LEVEL,type(self.UPPER_LEVEL)
self.LOWER_LEVEL = rospy.get_param(
'~REG_LOWER_LEVEL') # default 500 声音保存的阈值
#print 'self.LOWER_LEVEL',self.LOWER_LEVEL,type(self.LOWER_LEVEL)
self.COUNT_NUM = rospy.get_param(
'~REG_COUNT_NUM'
) # default 20 NUM_SAMPLES个取样之内出现COUNT_NUM个大于LOWER_LEVEL的取样则记录声音
#print 'self.COUNT_NUM',self.COUNT_NUM,type(self.COUNT_NUM)
self.SAVE_LENGTH = rospy.get_param(
'~REG_SAVE_LENGTH'
) # default 8 声音记录的最小长度：SAVE_LENGTH * NUM_SAMPLES 个取样
#print 'self.SAVE_LENGTH',self.SAVE_LENGTH,type(self.SAVE_LENGTH)
self.TIME_OUT = rospy.get_param('~REG_TIME_OUT') # default 60 录音时间，单位s
#print 'self.TIME_OUT',self.TIME_OUT,type(self.TIME_OUT)
self.NO_WORDS = rospy.get_param('~REG_NO_WORDS') # default 6
#print 'self.NO_WORDS',self.NO_WORDS,type(self.NO_WORDS)
self.Api_Key = rospy.get_param(
'~REG_Api_Key') # default "pmUzrWcsA3Ce7RB5rSqsvQt2"
#print 'self.Api_Key',self.Api_Key,type(self.Api_Key)
self.Secrect_Key = rospy.get_param(
'~REG_Secrect_Key') # default "d39ec848d016a8474c7c25e308b310c3"
#print 'self.Secrect_Key',self.Secrect_Key,type(self.Secrect_Key)
self.Grant_type = rospy.get_param(
'~REG_Grant_type') # default "client_credentials"
#print 'self.Grant_type',self.Grant_type,type(self.Grant_type)
self.Token_url = rospy.get_param(
'~REG_Token_url'
) # default 'https://openapi.baidu.com/oauth/2.0/token'
#print 'self.Token_url',self.Token_url,type(self.Token_url)
self.Reg_url = rospy.get_param(
'~REG_Reg_url') # default 'http://vop.baidu.com/server_api'
#print 'self.Reg_url',self.Reg_url,type(self.Reg_url)
self.USER_ID = rospy.get_param('~REG_USER_ID') # default '8168466'
#print 'self.USER_ID',self.USER_ID,type(self.USER_ID)
self.FORMAT = rospy.get_param('~REG_FORMAT') # default 'wav'
#print 'self.FORMAT',self.FORMAT,type(self.FORMAT)
self.LAN = rospy.get_param('~REG_LAN') # default 'zh'
#print 'self.LAN',self.LAN,type(self.LAN)
self.nchannel = rospy.get_param('~REG_nchannel') # default 1
#print 'self.nchannel',self.nchannel,type(self.nchannel)
self.Voice_String = []
def set_angular_velocity(self, msg):
if self.is_connected:
rate = int((msg.data*180/math.pi)/0.784)
self.robot.set_rotation_rate(rate, False)
def configure_collision_detect(self, msg):
pass
def reconfigure(self, config, level):
if self.is_connected:
self.robot.set_rgb_led(int(config['red']*255),int(config['green']*255),int(config['blue']*255),0,False)
return config
if __name__ == '__main__':
rospy.init_node('sphero')
name = rospy.get_name()
if rospy.has_param('%s/address'%name):
address = rospy.get_param('%s/address'%name)
if ':' in address:
print 'Looking for sphero with address %s.'%address
else:
address = None
else:
address = None
s = SpheroNode()
s.start(address)
s.spin()
s.stop()
if neighbour_nodes_relay_list:
# Prepare information
if auction_req.auction_data.command == 'join_auction':
role = "be_buyer"
else:
role = 'none'
auction_type = 'k-sap'
sending_node = rospy.get_name()
auctioneer_node = auction_req.auctioneer_node
# updated nodes_collected
if rospy.has_param('/nodes_collected'):
nodes_collected = rospy.get_param('/nodes_collected')+','+rospy.get_name()
rospy.set_param('/nodes_collected',nodes_collected)
else:
nodes_collected = rospy.get_param('~neighbour_nodes_list')
auction_data = auction_req.auction_data
for node in neighbour_nodes_relay_list:
# prepare neighbours to be buyers
service_path = node+'/auction_config_server'
rospy.wait_for_service(service_path)
neighbour_node_auction_config_server = rospy.ServiceProxy(service_path,
auction_srvs.srv.AuctionConfigService)
class OdomGraph:
def __init__(self):
self.x_list = list()
self.y_list = list()
def pose_cb(self, msg):
self.x_list.append(msg.x)
self.y_list.append(msg.y)
if __name__ == '__main__':
try:
rospy.init_node('odom_graph', anonymous=True)
output_to_file = False
if rospy.has_param('/output_to_file'):
rospy.logwarn("Has output to file")
if rospy.get_param('/output_to_file') == True or rospy.get_param(
'/output_to_file') == "true":
output_to_file = True
if rospy.has_param('/only_output_to_file'):
rospy.logwarn("Has only output to file")
if rospy.get_param(
'/only_output_to_file') == True or rospy.get_param(
'/only_output_to_file') == "true":
rospy.logwarn("only outputting to PDF!")
output_to_file = True
matplotlib.use("pdf")
folder = "."
if rospy.has_param('output_folder'):
folder = rospy.get_param('output_folder')
def __init__(self, arm_side, target_config, time=2.0):
"""Constructor"""
super(GotoSingleArmJointConfigState,
self).__init__(outcomes=['done', 'failed'],
input_keys=['current_config'])
if not rospy.has_param("behavior/robot_namespace"):
Logger.logerr(
"Need to specify parameter behavior/robot_namespace at the parameter server"
)
return
self._robot = rospy.get_param("behavior/robot_namespace")
if not rospy.has_param("behavior/joint_controllers_name"):
Logger.logerr(
"Need to specify parameter behavior/joint_controllers_name at the parameter server"
)
return
controller_namespace = rospy.get_param(
"behavior/joint_controllers_name")
################################ ATLAS ################################
self._configs = dict()
self._configs['flor'] = dict()
self._configs['flor']['left'] = {
11: {
'joint_name': 'l_arm_wry2',
'joint_value': -2.5
},
12: {
'joint_name': 'l_arm_wry2',
'joint_value': +2.5
}
}
self._configs['flor']['right'] = {
11: {
'joint_name': 'r_arm_wry2',
'joint_value': +2.5
},
12: {
'joint_name': 'r_arm_wry2',
'joint_value': -2.5
}
}
################################ THOR #################################
self._configs['thor_mang'] = dict()
self._configs['thor_mang']['left'] = {
11: {
'joint_name': 'l_wrist_yaw2',
'joint_value': 3.84
},
12: {
'joint_name': 'l_wrist_yaw2',
'joint_value': -3.84
}
}
self._configs['thor_mang']['right'] = {
11: {
'joint_name': 'r_wrist_yaw2',
'joint_value': -3.84
},
12: {
'joint_name': 'r_wrist_yaw2',
'joint_value': 3.84
}
}
#######################################################################
self._joint_name = self._configs[
self._robot][arm_side][target_config]['joint_name']
self._joint_value = self._configs[
self._robot][arm_side][target_config]['joint_value']
self._time = time
self._action_topic = "/" + controller_namespace + "/" + arm_side + \
"_arm_traj_controller" + "/follow_joint_trajectory"
self._client = ProxyActionClient(
{self._action_topic: FollowJointTrajectoryAction})
self._failed = False
def __init__(self):
# Initializing publisher with buffer size of 10 messages
self.pub_ = rospy.Publisher("kws_data", String, queue_size=10)
# Initalizing publisher for continuous ASR
self.continuous_pub_ = rospy.Publisher("jsgf_audio",
String,
queue_size=10)
# initialize node
rospy.init_node("kws_control")
# Call custom function on node shutdown
rospy.on_shutdown(self.shutdown)
# for detect is speech
self.count_subs = 0
self.not_speech = 0
self.dev = usb.core.find(idVendor=0x2886, idProduct=0x0018)
if self.dev:
self.Mic_tuning = Tuning(self.dev)
self.speech_end_hysteresis = 0
self.utt_started = 0
self.previous_data_buf = []
self.speech = 0
# Params
# File containing language model
_hmm_param = "~hmm"
# Dictionary
_dict_param = "~dict"
# List of keywords to detect
_kws_param = "~kws"
"""Not necessary to provide the next two if _kws_param is provided
Single word which needs to be detected
"""
_keyphrase_param = "~keyphrase"
# Threshold frequency of above mentioned word
_threshold_param = "~threshold"
# Option for continuous
self._option_param = "~option"
self.count_recognized = 0
# Variable to distinguish between kws list and keyphrase.
# Default is keyword list
self._list = True
self.stop_output = False
# Setting param values
if rospy.has_param(_hmm_param):
self.class_hmm = rospy.get_param(_hmm_param)
if rospy.get_param(_hmm_param) == ":default":
if os.path.isdir("/usr/local/share/pocketsphinx/model"):
rospy.loginfo("Loading the default acoustic model")
self.class_hmm = "/usr/local/share/pocketsphinx/model/en-us/en-us"
rospy.loginfo("Done loading the default acoustic model")
else:
rospy.logerr(
"No language model specified. Couldn't find defaut model."
)
return
else:
rospy.loginfo("Couldn't find lm argument")
if rospy.has_param(
_dict_param) and rospy.get_param(_dict_param) != ":default":
self.lexicon = rospy.get_param(_dict_param)
else:
rospy.logerr(
'No dictionary found. Please add an appropriate dictionary argument.'
)
return
if rospy.has_param(
_kws_param) and rospy.get_param(_kws_param) != ":default":
self._list = True
self.kw_list = rospy.get_param(_kws_param)
elif rospy.has_param(_keyphrase_param) and \
rospy.has_param(_threshold_param) and \
rospy.get_param(_keyphrase_param) != ":default" and \
rospy.get_param(_threshold_param) != ":default":
self._list = False
self.keyphrase = rospy.get_param(_keyphrase_param)
self.kws_threshold = rospy.get_param(_threshold_param)
else:
rospy.logerr(
'kws cant run. Please add an appropriate keyword list.')
return
# All params satisfied. Starting recognizer
self.start_recognizer()
