def vehicle(vehicle_id, vehicle_class, x=0., y=0., yaw=0., speed_in_ms=0.):
"""
Initialize ROS-node 'vehicle' and register subs, pubs and services.
@param vehicle_id: I{(int)} ID of the vehicle that is created.
@param vehicle_class: I{(str)} Name of the vehicle class that should be
created.
@param x: I{(float)} x-coordinate at which the vehicle should be spawned.
@param y: I{(float)} y-coordinate at which the vehicle should be spawned.
@param yaw: I{(float)} Initial yaw with which the vehicle should be
spawned.
@param speed_in_ms: I{(float)} Initial speed whith which the vehicle
should be spawned.
"""
rospy.init_node('vehicle', log_level=rospy.WARN)
if vehicle_class == BaseVehicle.__name__:
BaseVehicle(rospy.get_namespace(), vehicle_id, 20,
x, y, yaw, speed_in_ms)
elif vehicle_class == DummyVehicle.__name__:
DummyVehicle(rospy.get_namespace(), vehicle_id, 20,
x, y, yaw, speed_in_ms)
elif vehicle_class == WifiVehicle.__name__:
WifiVehicle(rospy.get_namespace(), vehicle_id, 20,
x, y, yaw, speed_in_ms)
else:
raise Exception("ERROR: Unknown vehicle class '%s'." % vehicle_class)
rospy.spin()
def __init__(self, description_file):
robot = xml.dom.minidom.parseString(description_file).getElementsByTagName('robot')[0]
self.free_joints = {}
self.warnings = {}
self.latest_state = None
self.last_time = rospy.get_time()
# Create all the joints based off of the URDF and assign them joint limits
# based on their properties
for child in robot.childNodes:
if child.nodeType is child.TEXT_NODE:
continue
if child.localName == 'joint':
jtype = child.getAttribute('type')
if jtype == 'fixed':
continue
name = child.getAttribute('name')
if jtype == 'continuous':
minval = -pi
maxval = pi
else:
limit = child.getElementsByTagName('limit')[0]
minval = float(limit.getAttribute('lower'))
maxval = float(limit.getAttribute('upper'))
if minval > 0 or maxval < 0:
zeroval = (maxval + minval)/2
else:
zeroval = 0
joint = {'min':minval, 'max':maxval, 'zero':zeroval, 'value':zeroval }
self.free_joints[name] = joint
#Setup the HuboState subscriber
self.hubo_sub = rospy.Subscriber(rospy.get_namespace() + "hubo_state", JointCommandState, self.hubo_cb)
#Setup the joint state publisher
self.hubo_pub = rospy.Publisher(rospy.get_namespace() + "joint_states", JointState)
def test_subscriber_appears_disappears(self):
topic_type = rospy.get_param('~input_topic_type')
check_connected_topics = rospy.get_param('~check_connected_topics')
wait_time = rospy.get_param('~wait_for_connection', 0)
msg_class = roslib.message.get_message_class(topic_type)
# Subscribe topic and bond connection
sub = rospy.Subscriber('~input', msg_class,
self._cb_test_subscriber_appears_disappears)
print('Waiting for connection for {} sec.'.format(wait_time))
rospy.sleep(wait_time)
# Check assumed topics are there
master = rosgraph.Master('test_connection')
_, subscriptions, _ = master.getSystemState()
for check_topic in check_connected_topics:
for topic, sub_node in subscriptions:
if topic == rospy.get_namespace() + check_topic:
break
else:
raise ValueError('Not found topic: {}'.format(check_topic))
sub.unregister()
# FIXME: below test won't pass on hydro
ROS_DISTRO = os.environ.get('ROS_DISTRO', 'indigo')
if ord(ROS_DISTRO[0]) < ord('i'):
sys.stderr.write('WARNING: running on rosdistro %s, and skipping '
'test for disconnection.\n' % ROS_DISTRO)
return
rospy.sleep(1) # wait for disconnection
# Check specified topics do not exist
_, subscriptions, _ = master.getSystemState()
for check_topic in check_connected_topics:
for topic, sub_node in subscriptions:
if topic == rospy.get_namespace() + check_topic:
raise ValueError('Found topic: {}'.format(check_topic))
def wifi_test(self):
message = None
while message is None:
message = self.w.receive()
rospy.sleep(0.5)
print rospy.get_namespace() + " got message: \"" + message + "\""
def ric_arm_rc():
global right_finger_pub,right_finger_min,right_finger_max
global left_finger_pub,left_finger_min,left_finger_max
global wrist_pub,wrist_min,wrist_max, wrist_ang
global elbow2_pub,elbow2_min,elbow2_max, elbow2_ang
global elbow1_pub,elbow1_min,elbow1_max, elbow1_ang
global shoulder_pub,shoulder_min,shoulder_max, shoulder_ang
global base_rotation_pub,base_rotation_min,base_rotation_max, base_rotation_ang
global got_links_states
global init
init=False
got_links_states=False;
wrist_ang=0.0;
elbow2_ang=0.0;
elbow1_ang=0.0;
shoulder_ang=0.0;
base_rotation_ang=0.0;
ns=rospy.get_namespace()
rospy.init_node('ric_gui', anonymous=True)
ns=rospy.get_namespace()
right_finger_max = rospy.get_param("right_finger_controller/motor/max")
right_finger_min = rospy.get_param("right_finger_controller/motor/min")
left_finger_max = rospy.get_param("left_finger_controller/motor/max")
left_finger_min = rospy.get_param("left_finger_controller/motor/min")
wrist_max = rospy.get_param("wrist_controller/motor/max")
wrist_min = rospy.get_param("wrist_controller/motor/min")
elbow2_max = rospy.get_param("elbow2_controller/motor/max")
elbow2_min = rospy.get_param("elbow2_controller/motor/min")
elbow1_max = rospy.get_param("elbow1_controller/motor/max")
elbow1_min = rospy.get_param("elbow1_controller/motor/min")
shoulder_max = rospy.get_param("shoulder_controller/motor/max")
shoulder_min = rospy.get_param("shoulder_controller/motor/min")
base_rotation_max = rospy.get_param("base_rotation_controller/motor/max")
base_rotation_min = rospy.get_param("base_rotation_controller/motor/min")
rospy.Subscriber(ns+"RC",ric_rc, rc_callback)
left_finger_pub = rospy.Publisher(ns+"left_finger_controller/command", Float64);
right_finger_pub = rospy.Publisher(ns+"right_finger_controller/command", Float64)
wrist_pub = rospy.Publisher(ns+"wrist_controller/command", Float64)
elbow2_pub = rospy.Publisher(ns+"elbow2_controller/command", Float64)
elbow1_pub = rospy.Publisher(ns+"elbow1_controller/command", Float64)
shoulder_pub = rospy.Publisher(ns+"shoulder_controller/command", Float64)
base_rotation_pub = rospy.Publisher(ns+"base_rotation_controller/command", Float64)
rospy.Subscriber("joint_states", JointState, js_callback)
while not rospy.is_shutdown():
#rospy.loginfo("test")
rospy.sleep(0.1)
def __init__(self):
self.node_name = rospy.get_name()
rospy.loginfo("[%s] Initializing......" %(self.node_name))
print "initializing"
self.framerate = self.setupParam("~framerate",30.0)
self.res_w = self.setupParam("~res_w",640)
self.res_h = self.setupParam("~res_h",480)
# For intrinsic calibration
rospack = rospkg.RosPack()
self.config = self.setupParam("~config","baseline")
self.cali_file_folder = rospack.get_path('duckietown') + "/config/" + self.config + "/calibration/camera_intrinsic/"
self.frame_id = rospy.get_namespace().strip('/') + "/camera_optical_frame"
self.has_published = False
self.pub_img= rospy.Publisher("~image/compressed",CompressedImage,queue_size=1)
# Create service (for camera_calibration)
self.srv_set_camera_info = rospy.Service("~set_camera_info",SetCameraInfo,self.cbSrvSetCameraInfo)
# Setup PiCamera
self.stream = io.BytesIO()
self.camera = PiCamera()
self.camera.framerate = self.framerate
self.camera.resolution = (self.res_w,self.res_h)
self.is_shutdown = False
# Setup timer
self.gen = self.grabAndPublish(self.stream,self.pub_img)
rospy.loginfo("[%s] Initialized." %(self.node_name))
def __init__(self, list_odometry_callbacks=list(), sub_to_odometry=True,
inertial_frame_id='world'):
"""Class constructor."""
assert inertial_frame_id in ['world', 'world_ned']
# Reading current namespace
self._namespace = rospy.get_namespace()
# Load the list of callback function handles to be called in the
# odometry callback
self._list_callbacks = list_odometry_callbacks
self._inertial_frame_id = inertial_frame_id
self._body_frame_id = None
if self._inertial_frame_id == 'world':
self._body_frame_id = 'base_link'
else:
self._body_frame_id = 'base_link_ned'
tf_buffer = tf2_ros.Buffer()
listener = tf2_ros.TransformListener(tf_buffer)
tf_trans_ned_to_enu = None
try:
tf_trans_ned_to_enu = tf_buffer.lookup_transform(
'world', 'world_ned', rospy.Time(),
rospy.Duration(1))
except Exception, e:
self._logger.error('No transform found between world and the '
'world_ned ' + self.namespace)
self._logger.error(str(e))
self.transform_ned_to_enu = None
def __init__(self):
rospy.init_node('relax_all_servos')
dynamixel_namespace = rospy.get_namespace()
if dynamixel_namespace == '/':
dynamixel_namespace = rospy.get_param('~dynamixel_namespace', '/dynamixel_controller')
dynamixels = rospy.get_param(dynamixel_namespace + '/dynamixels', dict())
servo_torque_enable = list()
servo_set_speed = list()
for name in sorted(dynamixels):
torque_enable_service = dynamixel_namespace + '/' + name + '_controller/torque_enable'
rospy.wait_for_service(torque_enable_service)
servo_torque_enable.append(rospy.ServiceProxy(torque_enable_service, TorqueEnable))
set_speed_service = dynamixel_namespace + '/' + name + '_controller/set_speed'
rospy.wait_for_service(set_speed_service)
servo_set_speed.append(rospy.ServiceProxy(set_speed_service, SetSpeed))
# Give the servos an intial speed that is relatively slow for safety
for set_speed in servo_set_speed:
set_speed(0.3)
# Relax all servos to give them a rest.
for torque_enable in servo_torque_enable:
torque_enable(False)
def _handle_save_data(self,req):
if req.flag_save == True:
# if GUI request data to be saved create file
# namespace, e.g. /Iris1/
namespace = rospy.get_namespace()
if not (namespace == ""):
# remove / symbol to namespace: e.g, we get namespace= Iris1
namespace = namespace.replace("/", "")
# string for time: used for generating files
# time_stamp = str(int(rospy.get_time() - self.TimeSaveData))
time_stamp = str(int(rospy.get_time()))
# file name provided by user in request
file_name = req.file_name
self.file_handle = file(self.package_save_path+'_'+time_stamp+'_'+file_name+namespace+'.txt', 'w')
# if GUI request data to be saved, set flag to true
self.SaveDataFlag = True
else:
# if GUI request data NOT to be saved, set falg to False
self.SaveDataFlag = False
# return message to Gui, to let it know resquest has been fulfilled
return SaveDataResponse(True)
def __init__(self):
self.ns = 'arm_controller'
#self.rate = 50.0 #rospy.get_param('~controllers/'+name+'/rate',50.0)
#self.joints = rospy.get_param('~' + self.ns + '/arm_joints')
#self.joints = [right_arm_tilt, right_arm_lift, right_arm_rotate, right_arm_elbow, right_arm_wrist_tilt]
namespace = rospy.get_namespace()
self.joints = rospy.get_param('right_arm/joints', '')
#self.joints = rospy.get_param('~controllers/'+name+'/joints')
rospy.loginfo('Configured for ' + str(len(self.joints)) + 'joints')
#rospy.logerr(self.joints)
#self.joint_subs = [JointSubscriber(name + '_controller') for name in self.joints]
#self.joint_pubs = [JointCommander(name + '_controller') for name in self.joints]
self.joint_subs = [JointSubscriber(name) for name in self.joints]
self.joint_pubs = [JointCommander(name) for name in self.joints]
self.joints_names = []
for idx in range(0,len(self.joints)):
self.joints_names.append(self.joints[idx])
#rospy.logerr(self.joints_names)
# action server
#name = rospy.get_param('~controllers/'+name+'/action_name','follow_joint_trajectory')
self.name = self.ns + '/follow_joint_trajectory'
self.server = actionlib.SimpleActionServer(self.name, FollowJointTrajectoryAction, execute_cb=self.actionCb, auto_start=False)
rospy.loginfo("Started FollowController ("+self.name+"). Joints: " + str(self.joints))
def initialize(self):
self.stopped = False
self.client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
self.client.wait_for_server()
self.circling = False
#self.init_guess_srv = rospy.ServiceProxy("init_guess_pub", InitGuess)
self.sub_commands_robot = rospy.Subscriber("/commands_robot", String, self.cb_commands_robot)
self.sub_position_share = rospy.Subscriber("/position_share", PoseTwistWithCovariance, self.cb_common_positions)
self.sub_goal = rospy.Subscriber("/goal", PoseStamped, self.cb_goal)
self.sub_ground_truth = rospy.Subscriber("base_pose_ground_truth", Odometry, self.cb_ground_truth)
self.pub_init_guess = rospy.Publisher("initialpose", PoseWithCovarianceStamped)
self.pub_commands_robot = rospy.Publisher("/commands_robot", String)
self.hostname = rospy.get_namespace()
self.noise_std = rospy.get_param("/noise_std", 0.0)
if (self.hostname == "/"):
self.hostname = gethostname()
self.goals = rospy.get_param("/%s/goals"%self.hostname,[])
else:
self.goals = rospy.get_param("%sgoals"%self.hostname,[])
if len(self.goals)>0:
rospy.loginfo("goals: %s"%str(self.goals))
self.cur_goal = 0
self.num_goals = len(self.goals["x"])
self.cur_goal_msg = self.return_cur_goal()
rospy.loginfo("Name: %s",self.hostname)
def __init__(self):
NaoNode.__init__(self)
rospy.init_node('nao_camera')
self.camProxy = self.getProxy("ALVideoDevice")
if self.camProxy is None:
exit(1)
# ROS pub/sub
self.pub_img_ = rospy.Publisher('image_raw', Image)
self.pub_info_ = rospy.Publisher('camera_info', CameraInfo)
self.set_camera_info_service_ = rospy.Service('set_camera_info', SetCameraInfo, self.set_camera_info)
# Messages
self.info_ = CameraInfo()
self.set_default_params_qvga(self.info_) #default params should be overwritten by service call
# parameters
self.camera_switch = rospy.get_param('~camera_switch', 0)
if self.camera_switch == 0:
self.frame_id = "/CameraTop_frame"
elif self.camera_switch == 1:
self.frame_id = "/CameraBottom_frame"
else:
rospy.logerr('Invalid camera_switch. Must be 0 or 1')
exit(1)
print "Using namespace ", rospy.get_namespace()
print "using camera: ", self.camera_switch
#TODO: parameters
self.resolution = kQVGA
self.colorSpace = kBGRColorSpace
self.fps = 30
# init
self.nameId = ''
self.subscribe()
def handle_Save_Data(self,req):
if req.ToSave == True:
# if GUI request data to be saved create file
# namespace, e.g. /Iris1/
ns = rospy.get_namespace()
# remove / symbol to namespace: e.g, we get ns= Iris1
ns = ns.replace("/", "")
# string for time: used for generating files
tt = str(int(rospy.get_time() - self.TimeSaveData))
# determine ROS workspace directory
rp = RosPack()
package_path = rp.get_path('quad_control')
self.file_handle = file(package_path+'/../../'+ns+'_data_'+tt+'.txt', 'w')
# if GUI request data to be saved, set falg to true
self.SaveDataFlag = True
else:
# if GUI request data NOT to be saved, set falg to False
self.SaveDataFlag = False
# return message to Gui, to let it know resquest has been fulfilled
return SaveDataResponse(True)
def __init__(self):
self.active = True
self.first_timestamp = -1 # won't start unless it's None
self.data = []
self.capture_time = 1.0 # capture time
self.capture_finished = True
self.tinit = None
self.trigger = False
self.node_state = 0
self.node_name = rospy.get_name()
self.pub_detections = rospy.Publisher("~raw_led_detection",LEDDetectionArray,queue_size=1)
self.pub_debug = rospy.Publisher("~debug_info",LEDDetectionDebugInfo,queue_size=1)
self.veh_name = rospy.get_namespace().strip("/")
if not self.veh_name:
# fall back on private param passed thru rosrun
# syntax is: rosrun <pkg> <node> _veh:=<bot-id>
if rospy.has_param('~veh'):
self.veh_name = rospy.get_param('~veh')
if not self.veh_name:
raise ValueError('Vehicle name is not set.')
rospy.loginfo('Vehicle: %s'%self.veh_name)
self.sub_cam = rospy.Subscriber("camera_node/image/compressed",CompressedImage, self.camera_callback)
self.sub_cam = rospy.Subscriber("~trigger",Byte, self.trigger_callback)
self.sub_switch = rospy.Subscriber("~switch",BoolStamped,self.cbSwitch)
print('Waiting for camera image...')
def relocateTopic(self, oldAddress, newAddress):
'''This very important method is meant to change the topics from one name (or address) to another in runtime.
Since that not possible in a literal way; "mux" tool is used to repeat them under the new name/address.
PRECONDITION: the oldAddress is in fact an address, not a name, since it hasn't got any sense to have names at this level
### Here's a next step when they complete their project >>> callService("rosspawn/start", "")
'''
myNamespace = '/'.join(rospy.get_namespace().split('/')[:-2])
newIndex = len(self.createdMuxes)+1
if not oldAddress in self.createdMuxes:
## In case that the oldAddress is a new address, the new "addressSub0" is the original one
for addressSub0 in [j for j in self.createdMuxes if self.createdMuxes[j][1]==oldAddress]:
oldAddress=addressSub0
if oldAddress in self.createdMuxes:
## In case the oldAddress is already relocated, the first node is killed
newIndex = self.createdMuxes[oldAddress][0]
rospy.loginfo(str("I will execute: rosnode kill " + myNamespace + '/mux' + str(newIndex)))
subprocess.Popen(shlex.split('rosnode kill ' + myNamespace + '/mux' + str(newIndex)), close_fds=True)
## Postcondition: the index is up to date and the previous mux node is supposed to be already killed
self.createdMuxes[oldAddress] = (newIndex, newAddress) ## Maybe the address should be checked first
rospy.loginfo("Relocating from " + oldAddress + " to " + newAddress)
rospy.loginfo('I will execute: roslaunch image_adaptor runMultiplexers.launch namespace:="' + myNamespace + '" node_id:="mux' + str(newIndex) + '" args:="' + newAddress + ' ' + oldAddress + '"')
subprocess.Popen(shlex.split('roslaunch image_adaptor runMultiplexers.launch namespace:="' + myNamespace + '" node_id:="mux' + str(newIndex) + '" args:="' + newAddress + ' ' + oldAddress + '"'), close_fds=True)
## subprocess.Popen(shlex.split('rosrun topic_tools mux ...
return True
def move_circ(pose_center, frame_id, start_angle, end_angle, radius, profile):
action_name = rospy.get_namespace()+'cartesian_trajectory_action'
client = actionlib.SimpleActionClient(action_name, CartesianControllerAction)
rospy.logwarn("Waiting for ActionServer: %s", action_name)
success = client.wait_for_server(rospy.Duration(2.0))
if(not success):
return (success, "ActionServer not available within timeout")
goal = CartesianControllerGoal()
goal.move_type = CartesianControllerGoal.CIRC
goal.move_circ.pose_center = pose_center
goal.move_circ.frame_id = frame_id
goal.move_circ.start_angle = start_angle
goal.move_circ.end_angle = end_angle
goal.move_circ.radius = radius
goal.profile = profile
# print goal
client.send_goal(goal)
print "goal sent"
state = client.get_state()
# print state
client.wait_for_result()
print "result received"
result = client.get_result()
return (result.success, result.message)
def handle_odometry(msg):
#rospy.loginfo("Read it %s" , msg)
#rospy.loginfo(rospy.get_namespace()[1:-1])
br = tf.TransformBroadcaster()
try:
br.sendTransform((msg.pose.pose.position.x,msg.pose.pose.position.y,msg.pose.pose.position.z),
(msg.pose.pose.orientation.x,msg.pose.pose.orientation.y,msg.pose.pose.orientation.z,msg.pose.pose.orientation.w),
rospy.Time.now() - rospy.Duration(1.0),
rospy.get_namespace()[1:-1],
"world")
except (TypeError):
br.sendTransform((msg.pose.pose.position.x,msg.pose.pose.position.y,msg.pose.pose.position.z),
(msg.pose.pose.orientation.x,msg.pose.pose.orientation.y,msg.pose.pose.orientation.z,msg.pose.pose.orientation.w),
rospy.Time.now(),
rospy.get_namespace()[1:-1],
"world")
def __init__(self):
self.node_name = rospy.get_name()
# Load parameters
# self.pub_freq = self.setupParam("~pub_freq",1.0)
self.config = self.setupParam("~config","baseline")
self.cali_file_name = self.setupParam("~cali_file_name","default")
# Setup publisher
self.pub_camera_info = rospy.Publisher("~camera_info",CameraInfo,queue_size=1)
# Get path to calibration yaml file
rospack = rospkg.RosPack()
self.cali_file = rospack.get_path('duckietown') + "/config/" + self.config + "/calibration/camera_intrinsic/" + self.cali_file_name + ".yaml"
self.camera_info_msg = None
# Load calibration yaml file
if not os.path.isfile(self.cali_file):
rospy.logwarn("[%s] Can't find calibration file: %s.\nUsing default calibration instead." %(self.node_name,self.cali_file))
self.cali_file = rospack.get_path('duckietown') + "/config/" + self.config + "/calibration/camera_intrinsic/default.yaml"
# Shutdown if no calibration file not found
if not os.path.isfile(self.cali_file):
rospy.signal_shutdown("Found no calibration file ... aborting")
# Print out and prepare message
rospy.loginfo("[%s] Using calibration file: %s" %(self.node_name,self.cali_file))
self.camera_info_msg = self.loadCameraInfo(self.cali_file)
self.camera_info_msg.header.frame_id = rospy.get_namespace() + "camera_optical_frame"
rospy.loginfo("[%s] CameraInfo: %s" %(self.node_name,self.camera_info_msg))
# self.timer_pub = rospy.Timer(rospy.Duration.from_sec(1.0/self.pub_freq),self.cbTimer)
self.sub_img_compressed = rospy.Subscriber("~compressed_image",CompressedImage,self.cbCompressedImage,queue_size=1)
def __init__(self):
self.bInitialized = False
# initialize
self.name = 'acquirevoltages2msg'
rospy.init_node(self.name, anonymous=False)
self.nodename = rospy.get_name()
self.namespace = rospy.get_namespace()
# Load the parameters.
self.params = rospy.get_param('%s' % self.nodename.rstrip('/'), {})
self.defaults = {'channels_ai':[0,1,2,3,4,5,6,7],
'channels_di':[0,1,2,3,4,5,6,7]}
SetDict().set_dict_with_preserve(self.params, self.defaults)
rospy.set_param('%s' % self.nodename.rstrip('/'), self.params)
self.command = None
# Messages & Services.
self.topicAI = '%s/ai' % self.namespace.rstrip('/')
self.pubAI = rospy.Publisher(self.topicAI, MsgAnalogIn)
self.topicDI = '%s/di' % self.namespace.rstrip('/')
self.pubDI = rospy.Publisher(self.topicDI, MsgDigitalIn)
self.subCommand = rospy.Subscriber('%s/command' % self.nodename.rstrip('/'), String, self.command_callback, queue_size=1000)
self.get_ai = rospy.ServiceProxy('get_ai', SrvPhidgetsInterfaceKitGetAI)
self.get_di = rospy.ServiceProxy('get_di', SrvPhidgetsInterfaceKitGetDI)
rospy.sleep(1) # Allow time to connect.
self.bInitialized = True
def __init__(self):
self.first_timestamp = None
self.data = []
self.capture_time = 2.3 # capture time
self.capture_finished = False
self.tinit = None
self.node_last_seen = -1
self.veh_name = rospy.get_namespace().strip("/")
if not self.veh_name:
# fall back on private param passed thru rosrun
# syntax is: rosrun <pkg> <node> _veh:=<bot-id>
if rospy.has_param('~veh'):
self.veh_name = rospy.get_param('~veh')
if not self.veh_name:
raise ValueError('Vehicle name is not set.')
self.sub_info = rospy.Subscriber("/"+self.veh_name+"/LED_detector_node/debug_info", LEDDetectionDebugInfo, self.info_callback)
self.sub_info = rospy.Subscriber("/"+self.veh_name+"/camera_node/image/compressed", CompressedImage, self.cam_callback)
self.sub_info = rospy.Subscriber("/"+self.veh_name+"/LED_detector_node/raw_led_detection", LEDDetectionArray, self.result_callback)
self.pub_trigger = rospy.Publisher("/"+self.veh_name+"/LED_detector_node/trigger",Byte,queue_size=1)
self.hbtimer = rospy.Timer(rospy.Duration.from_sec(.2), self.heartbeat_timer)
win.set_trigger_pub(self.pub_trigger)
def __init__(self):
rospy.init_node("enemy_manager")
self.ns = rospy.get_namespace()
self.launch = roslaunch.scriptapi.ROSLaunch()
self.launch.start()
names = ['t1', 't2', 't3']
self.nodes = {}
self.processes = {}
y = 0
for name in names:
y += 1
param_name = "/" + name + "/y"
if self.ns != "/":
param_name = self.ns + param_name
rospy.set_param(param_name, y)
self.nodes[name] = roslaunch.core.Node("grid_defense", "enemy.py",
name=name, namespace=self.ns,
args="")
self.processes[name] = self.launch.launch(self.nodes[name])
# while not rospy.is_shutdown():
# rospy.sleep(1.0)
rospy.spin()
for name in names:
rospy.loginfo("stopping " + name)
self.processes[name].stop()
def __init__(self):
rospy.init_node('clearpath_teleop')
self.turn_scale = rospy.get_param('~turn_scale')
self.drive_scale = rospy.get_param('~drive_scale')
self.deadman_button = rospy.get_param('~deadman_button', 0)
self.planner_button = rospy.get_param('~planner_button', 1)
self.cmd = None
self.joy = Joy()
cmd_pub = rospy.Publisher('cmd_vel', Twist)
announce_pub = rospy.Publisher('/clearpath/announce/teleops',
String, latch=True)
announce_pub.publish(rospy.get_namespace());
rospy.Subscriber("joy", Joy, self.callback)
rospy.Subscriber("plan_cmd_vel", Twist, self.planned_callback)
self.planned_motion = Twist()
rate = rospy.Rate(rospy.get_param('~hz', 20))
while not rospy.is_shutdown():
rate.sleep()
if self.cmd:
cmd_pub.publish(self.cmd)
def __init__(self):
rospy.init_node('ax12_joint_state_publisher', anonymous=True)
ax12_namespace = rospy.get_namespace()
rate = rospy.get_param('~rate', 10)
r = rospy.Rate(rate)
self.servos = ('head_pan', 'head_tilt')
self.joints = list()
self.controllers = list()
self.joint_states = dict({})
for s in self.servos:
joint = s + "_joint"
self.joint_states[joint] = JointStateMessage(joint, 0.0, 0.0, 0.0)
self.controllers.append(ax12_namespace + s + "_controller")
# Start controller state subscribers
[rospy.Subscriber(c + '/state', JointStateAX12, self.controller_state_handler) for c in self.controllers]
# Start publisher
self.joint_states_pub = rospy.Publisher('/joint_states', JointStatePR2)
rospy.loginfo("Starting AX12 Joint State Publisher at " + str(rate) + "Hz")
while not rospy.is_shutdown():
self.publish_joint_states()
r.sleep()
def __init__(self):
rospy.init_node("dynamixel_joint_state_publisher", anonymous=True)
rate = rospy.get_param("~rate", 20)
r = rospy.Rate(rate)
# The namespace and joints parameter needs to be set by the servo controller
# (The namespace is usually null.)
namespace = rospy.get_namespace()
self.joints = rospy.get_param(namespace + "/joints", "")
self.servos = list()
self.controllers = list()
self.joint_states = dict({})
for controller in sorted(self.joints):
self.joint_states[controller] = JointStateMessage(controller, 0.0, 0.0, 0.0)
self.controllers.append(controller)
# Start controller state subscribers
[rospy.Subscriber(c + "/state", JointStateDynamixel, self.controller_state_handler) for c in self.controllers]
# Start publisher
self.joint_states_pub = rospy.Publisher("/joint_states", JointStatePR2, queue_size=5)
rospy.loginfo("Starting Dynamixel Joint State Publisher at " + str(rate) + "Hz")
while not rospy.is_shutdown():
self.publish_joint_states()
r.sleep()
def timer_one_cb(self, event):
# check for start
if self.start is None:
return
rospy.loginfo("aMussel %s: Current time in seconds is %s ", rospy.get_namespace(), rospy.get_time())
def __init__(self):
rospy.init_node('cmps09')
# Opens up serial port
self.port = rospy.get_param('~port', '')
self.hz = rospy.get_param('~hz', 20)
self.timeout = rospy.get_param('~timeout', 0.2)
if self.port != '':
rospy.loginfo("CMPS09 using port %s.",self.port)
else:
rospy.logerr("No port specified for CMPS09!")
exit(1)
try:
self.transport = serial.Serial(port=self.port, baudrate=9600, timeout=self.timeout)
self.transport.open()
except serial.serialutil.SerialException as e:
rospy.logerr("CMPS09: %s" % e)
exit(1)
# Registers shutdown handler to close the serial port we just opened.
rospy.on_shutdown(self.shutdown_handler)
# Registers as publisher
self.pub = rospy.Publisher('cmd_vel', Twist)
announce_pub = rospy.Publisher('/clearpath/announce/teleops',
String, latch=True)
announce_pub.publish(rospy.get_namespace());
def _handle_get_nodelet_manager_name(self, req):
resp = GetNodeletManagerNameResponse()
resp.nodelet_manager_name = rospy.get_namespace()
if not resp.nodelet_manager_name.endswith('/'):
resp.nodelet_manager_name += "/"
resp.nodelet_manager_name += self.__launch_manager.nodelet_manager_name
return resp
def main():
debug = rospy.get_param('/debug')
if debug:
rospy.init_node('sensor_remap', log_level=rospy.DEBUG)
else:
rospy.init_node('sensor_remap')
helpers.wait_for_services()
global namespace
namespace = rospy.get_namespace()[1:]
# Set up publishers
global imu_publisher
global odom_publisher
global noisy_odom_publisher
imu_publisher = rospy.Publisher('imu_data_remapped', Imu, queue_size=1)
odom_publisher = rospy.Publisher('odom_remapped', Odometry, queue_size=1)
noisy_odom_publisher = rospy.Publisher('noisy_odom_remapped', Odometry, queue_size=1)
# subscribe to imu and odom from the robot
imu_subscriber = rospy.Subscriber('mobile_base/sensors/imu_data', Imu, imu_remap)
odom_subscriber = rospy.Subscriber('odom_throttle', Odometry, odom_remap)
noisy_odom_subscriber = rospy.Subscriber('odom_throttle', Odometry, noisy_odom_remap)
# Get the initial position of the robot
# initial_position_subscriber = rospy.Subscriber('initial_position', PoseWithCovarianceStamped, initial_position_callback)
# just run this to pub/sub to the robot
while not rospy.is_shutdown():
rospy.spin()
def __init__(self):
rospy.init_node('ptu_teleop')
pan_lim = rospy.get_param('~pan_lim', 2.775)
tilt_hi_lim = rospy.get_param('~tilt_hi_lim', 0.82)
tilt_lo_lim = -rospy.get_param('~tilt_lo_lim', -0.52)
tilt_scale = rospy.get_param('~tilt_scale', 1)
pan_scale = rospy.get_param('~pan_scale', 1)
self.pan_speed = rospy.get_param('~pan_speed', 1)
self.tilt_speed = rospy.get_param('~tilt_speed', 1)
self.ptu_button = rospy.get_param('~ptu_button', 2)
self.home_button = rospy.get_param('~home_button', 7)
# Desired PTU behaviour - set by joystick callback
self.des_pan_vel = 0
self.des_tilt_vel = 0
self.ptu_changed = False
self.ptu_reset = False
pan_setpt = 0
tilt_setpt = 0
cmd_ptu_pub = rospy.Publisher('ptu/cmd', JointState)
announce_pub = rospy.Publisher('/clearpath/announce/teleops',
String, latch=True)
announce_pub.publish(rospy.get_namespace());
rospy.Subscriber("joy", Joy, self.callback)
freq = rospy.get_param('~hz',50)
rate = rospy.Rate(freq)
while not rospy.is_shutdown():
rate.sleep()
if self.ptu_reset or self.des_pan_vel != 0 or self.des_tilt_vel != 0:
ptu = JointState()
ptu.name.append("Pan")
ptu.name.append("Tilt")
# If we want to home, home
if self.ptu_reset:
pan_setpt = 0
tilt_setpt = 0
else:
pan_setpt += pan_scale*self.des_pan_vel*freq/1000.0
tilt_setpt += tilt_scale*self.des_tilt_vel*freq/1000.0
pan_setpt = min(pan_lim,pan_setpt)
pan_setpt = max(-pan_lim,pan_setpt)
tilt_setpt = min(tilt_hi_lim,tilt_setpt)
tilt_setpt = max(-tilt_lo_lim,tilt_setpt)
ptu.position.append(pan_setpt)
ptu.position.append(tilt_setpt)
ptu.velocity.append(rad(self.pan_speed))
ptu.velocity.append(rad(self.tilt_speed))
cmd_ptu_pub.publish(ptu)
def __init__(self, node_name):
rospy.logdebug("YoubotProxy __init__")
super(YoubotProxy,self).__init__()
self.init_done = False # indicates that the object was initialized
# init ros node
rospy.init_node(node_name, anonymous=True, log_level=rospy.INFO)
rospy.loginfo("ROS node initialized: " + rospy.get_name())
rospy.loginfo("node namespace is : " + rospy.get_namespace())
rospy.loginfo("node uri is : " + rospy.get_node_uri())
# init object attributes
self.arm_num = rospy.get_param("~arm_num",1)
rospy.loginfo("arm number: " + str(self.arm_num))
self.gripper_move_duration = rospy.Duration(rospy.get_param("~gripper_move_duration",600.0))
rospy.loginfo("gripper move duration: " + str(self.gripper_move_duration))
self.arm_move_duration = rospy.Duration(rospy.get_param("~arm_move_duration",600.0))
rospy.loginfo("arm move duration: " + str(self.arm_move_duration))
# init joint_states subscriber
self._joint_positions_arm = [0]*5
self._joint_velocities_arm = [0]*5
self._joint_positions_gripper = [0]*2
# todo: create thread event object for joint_states variable
self._joint_states_topic = rospy.get_param("~joint_states_topic", "/arm_" + str(self.arm_num) + "/joint_states")
self._joint_states_sub = rospy.Subscriber(self._joint_states_topic, JointState, self.joint_states_cb)
# Gripper distance tolerance: 1 mm
self.gripper_distance_tol = rospy.get_param("~gripper_distance_tol", 0.05)
# Joint distance tolerance: 1/10 radian tolerance (1.2 degrees)
self.arm_joint_distance_tol = rospy.get_param("~joint_distance_tol",0.025)
rospy.loginfo("arm joint position tolerance: " + str(self.arm_joint_distance_tol))
self.arm_joint_velocity_tol = rospy.get_param("~joint_velocity_tol",0.05)
rospy.loginfo("arm joint velocity tolerance: " + str(self.arm_joint_velocity_tol))
# connect to modbus service
rospy.wait_for_service('youbot_modbus_server/get_station_status')
rospy.wait_for_service('youbot_modbus_server/get_button_status')
self.get_station_status = rospy.ServiceProxy('youbot_modbus_server/get_station_status', YoubotModbusSensorMsg)
self.get_button_status = rospy.ServiceProxy('youbot_modbus_server/get_button_status', YoubotModbusButtonMsg)
rospy.loginfo("modbus clients started")
print "I have made it here!"
# init arm publisher
self._arm_pub = rospy.Publisher("arm_" + str(self.arm_num) + "/arm_controller/position_command", JointPositions)
rospy.loginfo("Created arm joint position publisher ")
# init gripper action client
self._gripper_pub = rospy.Publisher("arm_" + str(self.arm_num) + "/gripper_controller/position_command", JointPositions)
rospy.loginfo("Created gripper joint position publisher ")
rospack = rospkg.RosPack()
path_to_posedict_yaml = rospy.get_param('~joint_pose_dict', rospack.get_path('twoarm_cage') + "/config/joint_pos_defs.yaml")
path_to_cmds_yaml = rospy.get_param('~cmd_seq', rospack.get_path('twoarm_cage') + "/config/arm" + str(self.arm_num) + "_commands.yaml")
self.load_control_plan(path_to_posedict_yaml, path_to_cmds_yaml)
# set init done flag
self.init_done = True
def env_init(self, env_info):
"""Setup for the environment called when the experiment first starts.
Note:
Initialize a tuple with the reward, first state observation, boolean
indicating if it's terminal.
"""
# initialize
try:
name = 'gazebo_env'
rospy.init_node(name, anonymous=False)
except rospy.ROSInitException:
print "Error: 初始化节点失败，检查gazebo环境是否提前运行。"
self.trajectory = []
# -----------Default Robot State-----------------------
self.randomFlag = env_info.get('random_flag', False)
self.robot_name = env_info.get('robot_name', 'uav1')
self.target_x = env_info.get('target_x', 0.0)
self.target_y = env_info.get('target_y', 0.0)
self.leader_name = 'uav1'
self.init_robot_state = ModelState()
self.init_robot_state.model_name = self.robot_name
self.init_robot_state.pose.position.x = 0.0
self.init_robot_state.pose.position.y = 0.0
self.init_robot_state.pose.position.z = 1.5
self.init_robot_state.pose.orientation.x = 0.0
self.init_robot_state.pose.orientation.y = 0.0
self.init_robot_state.pose.orientation.z = 0.0
self.init_robot_state.pose.orientation.w = 1.0
self.init_robot_state.twist.linear.x = 0.
self.init_robot_state.twist.linear.y = 0.
self.init_robot_state.twist.linear.z = 0.
self.init_robot_state.twist.angular.x = 0.
self.init_robot_state.twist.angular.y = 0.
self.init_robot_state.twist.angular.z = 0.
self.init_robot_state.reference_frame = 'world'
# ----------- Parameters in RL env class -----------
self.maze_dim = [11, 11]
self.target_position = (self.target_x, self.target_y,
self.init_robot_state.pose.position.z)
# self.end_state = [0, 0]
self.end_radius = env_info["end_radius"] # meters
# The robot's pose and twist information under world frame
self.current_state = ModelState()
# The leader's pose and twist information under world frame
if self.leader_name != self.robot_name:
self.leader_state = ModelState()
# -----------Publisher and Subscriber-------------
self.sub_state = rospy.Subscriber('/gazebo/model_states', ModelStates,
self.state_callback)
self.set_state = rospy.Publisher('/gazebo/set_model_state',
ModelState,
queue_size=10)
self.pub_command = rospy.Publisher(rospy.get_namespace() + 'pose_cmd',
Twist,
queue_size=10)
rospy.sleep(2.)
# # What function to call when you ctrl + c
# rospy.on_shutdown(self.shutdown)
self.reward_obs_term = [0.0, None, False]
def VelSend(self):
# Globals
global vec_x
global vec_y
global vec_z
global dis2form_x, dis2form_y, dis2form_z
# initialization
Out = NodeDeps.Init_Graph()
Opt = NodeDeps.Swarm_Optimization(Out[1])
c = Opt[0]
B = Opt[1]
F = Opt[2]
# init Node
rospy.init_node('Formation', anonymous=True)
ns = rospy.get_namespace()
# Publishers
pub_vel = rospy.Publisher('command/cmd_vel',
Twist,
queue_size=10,
latch=True)
# Subscribes
diff_topic = ns[0:-1] + "/command/diff_vec"
rospy.Subscriber(diff_topic, State, self.Diff_callback)
time.sleep(3)
# Rate
speed_rate = 50.0
rate = rospy.Rate(speed_rate)
node = int(ns[-2:-1])
msg = Twist()
while not rospy.is_shutdown():
try:
#print ns, c*B*F, Out[0][node-1,:], self.vec_x, self.vec_y
#print ns + "UX = ",c*B*F*dot(Out[0][node-1,:], asarray(vec_x))
#print ns + "UY = ",c*B*F*dot(Out[0][node-1,:], asarray(vec_y))
#print ns, self.dis2form_x, self.dis2form_y, self.dis2form_z
ux_sig = -c * B * F * dot(
Out[0][node - 1, :], asarray(
self.vec_x)) + 0.5 * (self.dis2form_x)
uy_sig = -c * B * F * dot(
Out[0][node - 1, :], asarray(
self.vec_y)) + 0.5 * (self.dis2form_y)
uz_sig = -c * B * F * dot(
Out[0][node - 1, :], asarray(
self.vec_z)) + 0.25 * (self.dis2form_z)
#print ux_sig
msg.linear.x = ux_sig
msg.linear.y = uy_sig
msg.linear.z = uz_sig
except:
msg.linear.x = 0
msg.linear.y = 0
msg.linear.z = 0
#print msg
pub_vel.publish(msg)
#rospy.spinOnce() # I dont need a spinOnce in rospy. rate.sleep has the same effect in rospy as ros::spinOnce in roscpp
rate.sleep()
def __init__(self, total_num_obs):
self.state = State()
name = rospy.get_namespace()
self.name = name[1:-1]
#self.num_of_objects = 0;
self.world = MovingForest(total_num_obs)
available_meshes_static = [
"package://mader/meshes/ConcreteDamage01b/model3.dae",
"package://mader/meshes/ConcreteDamage01b/model2.dae"
]
available_meshes_dynamic = [
"package://mader/meshes/ConcreteDamage01b/model4.dae"
]
# available_meshes=["package://mader/meshes/ConcreteDamage01b/model3.dae"]
self.x_all = []
self.y_all = []
self.z_all = []
self.offset_all = []
self.slower = []
self.meshes = []
self.type = []
#"dynamic" or "static"
self.bboxes = []
for i in range(self.world.num_of_dyn_objects):
self.x_all.append(
random.uniform(self.world.x_min, self.world.x_max))
self.y_all.append(
random.uniform(self.world.y_min, self.world.y_max))
self.z_all.append(
random.uniform(self.world.z_min, self.world.z_max))
self.offset_all.append(random.uniform(-2 * math.pi, 2 * math.pi))
self.slower.append(
random.uniform(self.world.slower_min, self.world.slower_max))
self.type.append("dynamic")
self.meshes.append(random.choice(available_meshes_dynamic))
self.bboxes.append(self.world.bbox_dynamic)
for i in range(self.world.num_of_stat_objects):
bbox_i = []
if (i < self.world.percentage_vert *
self.world.num_of_stat_objects):
bbox_i = self.world.bbox_static_vert
self.z_all.append(bbox_i[2] / 2.0)
#random.uniform(self.world.z_min, self.world.z_max) for sphere sim
self.type.append("static_vert")
self.meshes.append(random.choice(available_meshes_static))
else:
bbox_i = self.world.bbox_static_horiz
self.z_all.append(random.uniform(0.0, 3.0))
#-3.0, 3.0 for sphere sim
self.type.append(
"static_horiz"
) #They are actually dynamic (moving in z) //TODO (change name)
self.meshes.append(random.choice(available_meshes_dynamic))
self.x_all.append(
random.uniform(self.world.x_min - self.world.scale,
self.world.x_max + self.world.scale))
self.y_all.append(
random.uniform(self.world.y_min - self.world.scale,
self.world.y_max + self.world.scale))
self.offset_all.append(random.uniform(-2 * math.pi, 2 * math.pi))
self.slower.append(
random.uniform(self.world.slower_min, self.world.slower_max))
# self.type.append("static")
self.bboxes.append(bbox_i)
self.pubTraj = rospy.Publisher('/trajs',
DynTraj,
queue_size=1,
latch=True)
self.pubShapes_static = rospy.Publisher('/shapes_static',
Marker,
queue_size=1,
latch=True)
self.pubShapes_static_mesh = rospy.Publisher('/shapes_static_mesh',
MarkerArray,
queue_size=1,
latch=True)
self.pubShapes_dynamic_mesh = rospy.Publisher('/shapes_dynamic_mesh',
MarkerArray,
queue_size=1,
latch=True)
self.pubShapes_dynamic = rospy.Publisher('/shapes_dynamic',
Marker,
queue_size=1,
latch=True)
self.pubGazeboState = rospy.Publisher('/gazebo/set_model_state',
ModelState,
queue_size=1)
self.already_published_static_shapes = False
rospy.sleep(0.5)
def setUpClass(self):
self.group = MoveGroupInterface(self.PLANNING_GROUP, "robot_description", rospy.get_namespace())
#!/usr/bin/env python
import rospy
import tf
import geometry_msgs.msg
from geometry_msgs.msg import Twist
from std_msgs.msg import Empty
import os
import time
update_odom = True
test = rospy.get_namespace()
t = tf.Transformer(True, rospy.Duration(20.0))
def kill_odom_transform():
os.system("rosnode kill /map_2_odom >/dev/null &")
def launch_static(x,y,qz,qw):
command = "roslaunch grbils map_2_odom.launch "
command += "x:=" +str(x) + " y:=" +str(y) + " qx:=0.0 qy:=0.0 qz:=" + str(qz) + " qw:=" + str(qw) +" >/dev/null &"
print command
os.system(command)
def set_transform(from_a,to_b,x,y,qz,qw):
m = geometry_msgs.msg.TransformStamped()
m.header.frame_id = from_a
m.child_frame_id = to_b
m.transform.translation.x = x
m.transform.translation.y = y
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
import rospy
import sys
from uuv_gazebo_ros_plugins_msgs.srv import SetThrusterEfficiency
if __name__ == '__main__':
print 'Set the thruster output efficiency for vehicle, namespace=', rospy.get_namespace(
)
rospy.init_node('set_thrusters_states')
if rospy.is_shutdown():
rospy.ROSException('ROS master not running!')
starting_time = 0.0
if rospy.has_param('~starting_time'):
starting_time = rospy.get_param('~starting_time')
print 'Starting time= %fs' % starting_time
duration = 0.0
if rospy.has_param('~duration'):
duration = rospy.get_param('~duration')
# parsing input arguments
parser = argparse.ArgumentParser(description='Redundancy Resolution')
parser.add_argument('-q','--q_angle_change', default=-90.0,
metavar='[degrees]', type=float,help='desired joint angle change in degrees')
parser.add_argument('-i','--index', default=1,
metavar='[ith joint]', type=int,
help='which joint to set the desired angle (starting from 1st)')
parser.add_argument('-a','--alpha', default=-0.5,
metavar='[value]', type=float,help='stepsize scaling parameter alpha')
args = parser.parse_args()
q_angle_change_rad = args.q_angle_change/180.0*np.pi
q_index = args.index
alpha = -abs(args.alpha)
moveit_commander.roscpp_initialize('')
rospy.init_node('robot_jogging')
client = actionlib.SimpleActionClient('/xarm/xarm7_traj_controller/follow_joint_trajectory',
FollowJointTrajectoryAction)
client.wait_for_server()
arm_group_name = "xarm7"
robot = moveit_commander.RobotCommander("robot_description")
scene = moveit_commander.PlanningSceneInterface(ns=rospy.get_namespace())
arm_group = moveit_commander.MoveGroupCommander(arm_group_name, ns=rospy.get_namespace())
display_trajectory_publisher = rospy.Publisher(rospy.get_namespace() + 'move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
redundancy_resolution(q_angle_change_rad, q_index, alpha, client)
#!/usr/bin/env python
import rospy
from el2425_bitcraze.srv import SetTargetPosition
from geometry_msgs.msg import PoseStamped
from geometry_msgs.msg import Point
# ===== Simulate CF position ===
# Simple simulation: Assumes CF is at the goal position
# Publishes the goal points on the crazyflie_position topic #
posPub = rospy.Publisher(rospy.get_namespace() + "crazyflie_position",
Point,
queue_size=10,
latch=True)
def callback(data):
point = Point()
point.x = data.pose.position.x
point.y = data.pose.position.y
point.z = data.pose.position.z
posPub.publish(point)
if __name__ == '__main__':
rospy.init_node('position_sim')
goalURI = rospy.get_namespace() + "goal"
rospy.Subscriber(goalURI, PoseStamped, callback)
rospy.spin()
def __init__(self):
# Save the name of the node
self.node_name = rospy.get_name()
self.car_name = rospy.get_namespace()
self.user_name = pwd.getpwuid(os.getuid()).pw_name
self.mode = None
#Exit condition for the node
self.calib_done = False
#Params for U-Turn
self.lane_follow_override = False
self.last_tag = 0
self.timer_called = False
#Tags to do a U-turn at
self.tag_id_inter_1 = 316
self.tag_id_inter_2 = 320
#Standing at stopline
self.stopped = False
#Node active?
self.active = False
#Publishers
self.pub_car_cmd = rospy.Publisher("~car_cmd",
Twist2DStamped,
queue_size=1)
self.pub_start = rospy.Publisher("~calibration_start",
BoolStamped,
queue_size=1)
self.pub_stop = rospy.Publisher("~calibration_stop",
BoolStamped,
queue_size=1)
self.pub_calc_start = rospy.Publisher("~calibration_calculation_start",
BoolStamped,
queue_size=1)
self.rate = rospy.Rate(30)
# Subscribers
self.sub_mode = rospy.Subscriber("~mode",
FSMState,
self.cbFSMState,
queue_size=1)
self.sub_car_cmd_in = rospy.Subscriber("~car_cmd_in",
Twist2DStamped,
self.publishControl,
queue_size=1)
self.sub_topic_tag = rospy.Subscriber("~tag",
AprilTagsWithInfos,
self.cbTag,
queue_size=1)
self.sub_at_stop_line = rospy.Subscriber("~at_stop_line",
BoolStamped,
self.cbStop,
queue_size=1)
self.sub_in_calibration_area = rospy.Subscriber("~in_calib",
BoolStamped,
self.cbCalib,
queue_size=1)
self.sub_switch = rospy.Subscriber("~switch",
BoolStamped,
self.cbSwitch,
queue_size=1)
self.sub_calc_stop = rospy.Subscriber("~calibration_calculation_stop",
BoolStamped,
self.CalibrationDone,
queue_size=1)
from sensor_msgs.msg import Imu, Temperature
from std_msgs.msg import Float32
import random
mock = thrust_command_msg()
def mock_catch(msg):
global mock
mock = msg
if __name__ == "__main__":
rospy.init_node('mock_prop')
ns = rospy.get_namespace() # This should return /surface
status_sub = rospy.Subscriber(ns + 'thrust_mock', thrust_command_msg,
mock_catch)
# Publishers out onto the ROS System
thrust_pub = rospy.Publisher(ns + 'thrust_command',
thrust_command_msg,
queue_size=10)
rate = rospy.Rate(50) # 50hz
# TODO: I2C related activities
while not rospy.is_shutdown():
thrust_pub.publish(mock)
rate.sleep()
def spin(self):
r = rospy.Rate(rospy.get_param(rospy.get_namespace() + 'rate', 5))
while not rospy.is_shutdown():
self.update()
r.sleep()
def __init__(self, node_name):
# Initialize the DTROS parent class
super(ATLocalizationNode, self).__init__(
node_name=node_name, node_type=NodeType.PERCEPTION)
self.veh = rospy.get_namespace().strip("/")
# bridge between opencv and ros
self.bridge = CvBridge()
# construct subscriber for images
self.camera_sub = rospy.Subscriber(
'camera_node/image/compressed', CompressedImage, self.callback, queue_size=1, buff_size=(20*(1024**2)))
# self.debug_pub = rospy.Publisher(
# '~debug_image/compressed', CompressedImage)
# tf broadcasters
self.static_tf_br = tf2_ros.StaticTransformBroadcaster()
self.tf_br = tf2_ros.TransformBroadcaster()
# april-tag detector
self.at_detector = Detector(searchpath=['apriltags'],
families='tag36h11',
nthreads=4,
quad_decimate=4.0,
quad_sigma=0.0,
refine_edges=1,
decode_sharpening=0.25,
debug=0)
# keep track of the ID of the landmark tag
self.at_id = None
# get camera calibration parameters (homography, camera matrix, distortion parameters)
intrinsics_file = '/data/config/calibrations/camera_intrinsic/' + \
rospy.get_namespace().strip("/") + ".yaml"
extrinsics_file = '/data/config/calibrations/camera_extrinsic/' + \
rospy.get_namespace().strip("/") + ".yaml"
rospy.loginfo('Reading camera intrinsics from {}'.format(
intrinsics_file))
rospy.loginfo('Reading camera extrinsics from {}'.format(
extrinsics_file))
intrinsics = readYamlFile(intrinsics_file)
extrinsics = readYamlFile(extrinsics_file)
self.h = intrinsics['image_height']
self.w = intrinsics['image_width']
cam_mat = np.array(
intrinsics['camera_matrix']['data']).reshape(3, 3)
distortion_coeff = np.array(
intrinsics['distortion_coefficients']['data'])
H_ground2img = np.linalg.inv(
np.array(extrinsics['homography']).reshape(3, 3))
# precompute some quantities
new_cam_mat, _ = cv2.getOptimalNewCameraMatrix(
cam_mat, distortion_coeff, (640, 480), 1.0)
self.map1, self.map2, = cv2.initUndistortRectifyMap(
cam_mat, distortion_coeff, np.eye(3), new_cam_mat, (640, 480), cv2.CV_32FC1)
self.camera_params = (
new_cam_mat[0, 0], new_cam_mat[1, 1], new_cam_mat[0, 2], new_cam_mat[1, 2])
# define and broadcast static tfs
self.camloc_camcv = np.array([[0.0, 0.0, 1.0, 0.0],
[-1.0, 0.0, 0.0, 0.0],
[0.0, -1.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 1.0]])
self.atcv_atloc = np.array([[0.0, 1.0, 0.0, 0.0],
[0.0, 0.0, -1.0, 0.0],
[-1.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 1.0]])
camcv_base_estimated = estimateTfFromHomography(
H_ground2img, new_cam_mat)
theta = 15.0 / 180.0 * np.pi
base_camloc_nominal = np.array([[np.cos(theta), 0.0, np.sin(theta), 0.0582],
[0.0, 1.0, 0.0, 0.0],
[-np.sin(theta), 0.0,
np.cos(theta), 0.1072],
[0.0, 0.0, 0.0, 1.0]])
self.camloc_base = self.camloc_camcv @ camcv_base_estimated
# self.camloc_base = np.linalg.inv(base_camloc_nominal)
self.map_atloc = np.array([[1.0, 0.0, 0.0, 0.0],
[0.0, 1.0, 0.0, 0.0],
[0.0, 0.0, 1.0, 0.085],
[0.0, 0.0, 0.0, 1.0]])
broadcastTF(self.map_atloc, 'map', 'april_tag', self.static_tf_br)
alt, # alt - Altitude in meters in AMSL altitude, not WGS84 if absolute or relative, above terrain if GLOBAL_TERRAIN_ALT_INT
0, # X velocity in NED frame in m/s
0, # Y velocity in NED frame in m/s
0, # Z velocity in NED frame in m/s
0,
0,
0, # afx, afy, afz acceleration (not supported yet, ignored in GCS_Mavlink)
0,
0) # yaw, yaw_rate (not supported yet, ignored in GCS_Mavlink)
# send command to vehicle
vehicle.send_mavlink(msg)
if __name__ == '__main__':
time.sleep(1)
S = int(rospy.get_namespace()[-2])
# Address on which vehicle is communicating
MasterPort = sys.argv[1] #'tcp:127.0.0.1:5760'# sim
RosInterfacePort = '127.0.0.1:' + str(
17000 + S) # Loop back and pass to ros
QGCInterfacePort = '192.168.1.105:' + str(
18000 + S) #'192.168.0.155:'+str(18000+S) #IP of system running QGC
t = threading.Thread(target=NodeDeps.launchMAVProxy,
args=(
MasterPort,
RosInterfacePort,
QGCInterfacePort,
)) # Start mavproxy
t.setDaemon(False) # thread SHOULD exit when main thread is killed
def handle_take_sample(self):
sample_list = self.handeye_client.take_sample()
self.samples_taken += 1
print("{} Sample {} taken ".format(rospy.get_namespace(),
self.samples_taken))
self.state = AutomaticMovements.SAMPLE_TOOK
def __init__(self, inertial_frame_id='world'):
"""Class constructor."""
assert inertial_frame_id in ['world', 'world_ned']
# Reading current namespace
self._namespace = rospy.get_namespace()
self._inertial_frame_id = inertial_frame_id
self._body_frame_id = None
self._logger = get_logger()
if self._inertial_frame_id == 'world':
self._body_frame_id = 'base_link'
else:
self._body_frame_id = 'base_link_ned'
try:
import tf2_ros
tf_buffer = tf2_ros.Buffer()
listener = tf2_ros.TransformListener(tf_buffer)
tf_trans_ned_to_enu = tf_buffer.lookup_transform(
'world', 'world_ned', rospy.Time(), rospy.Duration(10))
self.q_ned_to_enu = np.array([
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
])
except Exception as ex:
self._logger.warning('Error while requesting ENU to NED transform'
', message={}'.format(ex))
self.q_ned_to_enu = quaternion_from_euler(2 * np.pi, 0, np.pi)
self.transform_ned_to_enu = quaternion_matrix(self.q_ned_to_enu)[0:3,
0:3]
if self.transform_ned_to_enu is not None:
self._logger.info('Transform world_ned (NED) to world (ENU)=\n' +
str(self.transform_ned_to_enu))
self._mass = 0
if rospy.has_param('~mass'):
self._mass = rospy.get_param('~mass')
if self._mass <= 0:
raise rospy.ROSException('Mass has to be positive')
self._inertial = dict(ixx=0, iyy=0, izz=0, ixy=0, ixz=0, iyz=0)
if rospy.has_param('~inertial'):
inertial = rospy.get_param('~inertial')
for key in self._inertial:
if key not in inertial:
raise rospy.ROSException('Invalid moments of inertia')
self._inertial = inertial
self._cog = [0, 0, 0]
if rospy.has_param('~cog'):
self._cog = rospy.get_param('~cog')
if len(self._cog) != 3:
raise rospy.ROSException('Invalid center of gravity vector')
self._cob = [0, 0, 0]
if rospy.has_param('~cog'):
self._cob = rospy.get_param('~cob')
if len(self._cob) != 3:
raise rospy.ROSException('Invalid center of buoyancy vector')
self._body_frame = 'base_link'
if rospy.has_param('~base_link'):
self._body_frame = rospy.get_param('~base_link')
self._volume = 0.0
if rospy.has_param('~volume'):
self._volume = rospy.get_param('~volume')
if self._volume <= 0:
raise rospy.ROSException('Invalid volume')
# Fluid density
self._density = 1028.0
if rospy.has_param('~density'):
self._density = rospy.get_param('~density')
if self._density <= 0:
raise rospy.ROSException('Invalid fluid density')
# Bounding box
self._height = 0.0
self._length = 0.0
self._width = 0.0
if rospy.has_param('~height'):
self._height = rospy.get_param('~height')
if self._height <= 0:
raise rospy.ROSException('Invalid height')
if rospy.has_param('~length'):
self._length = rospy.get_param('~length')
if self._length <= 0:
raise rospy.ROSException('Invalid length')
if rospy.has_param('~width'):
self._width = rospy.get_param('~width')
if self._width <= 0:
raise rospy.ROSException('Invalid width')
# Calculating the rigid-body mass matrix
self._M = np.zeros(shape=(6, 6), dtype=float)
self._M[0:3, 0:3] = self._mass * np.eye(3)
self._M[0:3, 3:6] = - self._mass * \
cross_product_operator(self._cog)
self._M[3:6, 0:3] = self._mass * \
cross_product_operator(self._cog)
self._M[3:6, 3:6] = self._calc_inertial_tensor()
# Loading the added-mass matrix
self._Ma = np.zeros((6, 6))
if rospy.has_param('~Ma'):
self._Ma = np.array(rospy.get_param('~Ma'))
if self._Ma.shape != (6, 6):
raise rospy.ROSException('Invalid added mass matrix')
# Sum rigid-body and added-mass matrices
self._Mtotal = np.zeros(shape=(6, 6))
self._calc_mass_matrix()
# Acceleration of gravity
self._gravity = 9.81
# Initialize the Coriolis and centripetal matrix
self._C = np.zeros((6, 6))
# Vector of restoring forces
self._g = np.zeros(6)
# Loading the linear damping coefficients
self._linear_damping = np.zeros(shape=(6, 6))
if rospy.has_param('~linear_damping'):
self._linear_damping = np.array(rospy.get_param('~linear_damping'))
if self._linear_damping.shape == (6, ):
self._linear_damping = np.diag(self._linear_damping)
if self._linear_damping.shape != (6, 6):
raise rospy.ROSException(
'Linear damping must be given as a 6x6 matrix or the diagonal coefficients'
)
# Loading the nonlinear damping coefficients
self._quad_damping = np.zeros(shape=(6, ))
if rospy.has_param('~quad_damping'):
self._quad_damping = np.array(rospy.get_param('~quad_damping'))
if self._quad_damping.shape != (6, ):
raise rospy.ROSException(
'Quadratic damping must be given defined with 6 coefficients'
)
# Loading the linear damping coefficients proportional to the forward speed
self._linear_damping_forward_speed = np.zeros(shape=(6, 6))
if rospy.has_param('~linear_damping_forward_speed'):
self._linear_damping_forward_speed = np.array(
rospy.get_param('~linear_damping_forward_speed'))
if self._linear_damping_forward_speed.shape == (6, ):
self._linear_damping_forward_speed = np.diag(
self._linear_damping_forward_speed)
if self._linear_damping_forward_speed.shape != (6, 6):
raise rospy.ROSException(
'Linear damping proportional to the '
'forward speed must be given as a 6x6 '
'matrix or the diagonal coefficients')
# Initialize damping matrix
self._D = np.zeros((6, 6))
# Vehicle states
self._pose = dict(pos=np.zeros(3), rot=quaternion_from_euler(0, 0, 0))
# Velocity in the body frame
self._vel = np.zeros(6)
# Acceleration in the body frame
self._acc = np.zeros(6)
# Generalized forces
self._gen_forces = np.zeros(6)
def handle_compute_calibration(self):
result = self.handeye_client.compute_calibration()
if result.valid:
print("{} Calibration successful".format(rospy.get_namespace()))
else:
print("{} Calibration failed".format(rospy.get_namespace()))
#!/usr/bin/env python
import json # talk to watson
import watson_developer_cloud # connect to watson
from unicodereplace import asciiFixerFactory
import rospy
from assistant.msg import ChatbotAnswer
from rtspeech.msg import RealtimeTranscript
loglevel = rospy.get_param('/debug/loglevel', rospy.INFO)
rospy.init_node('assistant', anonymous=False, log_level=loglevel)
credfilepath = rospy.get_param(rospy.get_namespace() + 'assistant_cred',
'/home/corobi/.cloudkeys/chatbot_cred.json')
minimumconfidence = rospy.get_param(
rospy.get_namespace() + 'minimumconfidence', 0.6)
language = rospy.get_param(rospy.get_namespace() + 'language', 'en-US')
asciifix = asciiFixerFactory(language)
class WatsonChatbot:
def __init__(self, credfile):
with open(credfile, 'rb') as infile:
creds = json.load(infile)
self._assistant = watson_developer_cloud.AssistantV1(
'2018-02-16',
url=creds['url'],
username=creds['username'],
def handle_save_calibration(self):
self.handeye_client.save()
print("{} Calibration saved".format(rospy.get_namespace()))
def __init__(self, context,namespace = None):
# it is either "" or the input given at creation of plugin
self.namespace = rospy.get_namespace()[1:]
self.name_others = rospy.get_param("/" + self.namespace + "name_others", '').rsplit(' ')
super(ChooseLTLPlanPlugin, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('ChooseLTLPlanPlugin')
# Process standalone plugin command-line arguments
from argparse import ArgumentParser
parser = ArgumentParser()
# Add argument(s) to the parser.
parser.add_argument("-q", "--quiet", action="store_true",
dest="quiet",
help="Put plugin in silent mode")
args, unknowns = parser.parse_known_args(context.argv())
if not args.quiet:
print 'arguments: ', args
print 'unknowns: ', unknowns
# Create QWidget
self._widget = QWidget()
# Get path to UI file which is a sibling of this file
# in this example the .ui and .py file are in the same folder
ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)), 'choose_ltl_plan.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
# Give QObjects reasonable names
self._widget.setObjectName('ChooseLTLPlannerUi')
# Show _widget.windowTitle on left-top of each plugin (when
# it's set in _widget). This is useful when you open multiple
# plugins at once. Also if you open multiple instances of your
# plugin at once, these lines add number to make it easy to
# tell from pane to pane.
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
# Add widget to the user interface
context.add_widget(self._widget)
# ---------------------------------------------- #
self._widget.open_file.clicked.connect(self.open_file)
self.filename = ""
self._widget.load_lmks.clicked.connect(self.load_lmks)
self._widget.update_initial_pose.clicked.connect(self.update_initial_pose)
self._widget.stop.clicked.connect(self.stop)
self._widget.start_planner.clicked.connect(self.start_speed_control)
self._widget.goto_initial_position.clicked.connect(self.goto_initial_position)
self._widget.slow_take_off.clicked.connect(self.slow_take_off)
self._widget.sim_cube_3quads.clicked.connect(self.load_sim_cube_3quads)
self._widget.sim_1quad_sim.clicked.connect(self.load_sim_1quad_sim)
self._widget.sim_1quad_real.clicked.connect(self.load_sim_1quad_real)
self._widget.savedata_3quads_check.stateChanged.connect(self.save_3quads)
self._widget.savedata_1quad_sim_check.stateChanged.connect(self.save_1quad_sim)
self.current_position = numpy.array([0.]*3)
self.current_yaw = 0.
self.initial_position = numpy.array([0.,0.,1.])
self.initial_v_psi = 0
self.initial_v_theta = 0
# package = 'quad_control'
# executable = 'planner_node.py'
# args = ""
# self.firefly = False
# if self.namespace:
# args = "--namespace "+self.namespace
# if self.firefly:
# args += "firefly"
# node = roslaunch.core.Node(package, executable,args=args,output="screen", namespace=self.namespace)
# launch = roslaunch.scriptapi.ROSLaunch()
# launch.start()
# self.process = launch.launch(node)
# launch collision avoidance node if it is active
# collision_av_active = rospy.get_param("collision_avoidance_active", True)
# if (collision_av_active):
# package = 'quad_control'
# executable = 'collision_avoidance_node.py'
# node = roslaunch.core.Node(package, executable, output = "screen", namespace=self.namespace)
# launcher = roslaunch.scriptapi.ROSLaunch()
# launcher.start()
# launcher.launch(node)
# package = 'quad_control'
# executable = 'magnitude_control_node.py'
# node = roslaunch.core.Node(package, executable, output = "screen", namespace=self.namespace)
# launcher = roslaunch.scriptapi.ROSLaunch()
# launcher.start()
# launcher.launch(node)
self.trace = []
self.canvas = None
self.fig = None
self.RotorSObject = RotorSConverter()
rospy.Subscriber("quad_state", quad_state, self.update_trace)
#!/usr/bin/env python
import roslib
import sys
import rospy
from referee_modules.referee_interface import GetInfoClient, ShootClient, AmmoConsumeClient
if __name__ == '__main__':
rospy.init_node('simulate_shooter')
ns = rospy.get_namespace()
rospy.Rate(0.3).sleep()
rate = rospy.Rate(8) ##Shooting rate in hz
count = 0
while not rospy.is_shutdown():
try:
info = GetInfoClient(ns)
if info["is_enemy_1_detected"] and info["ammo"] > 0 and info[
"game_state"] == 2:
if count % 2 == 1:
ShootClient(info["enemy_pose1"]["pose"]["position"]["x"],
info["enemy_pose1"]["pose"]["position"]["y"],
ns)
count += 1
AmmoConsumeClient(1, ns)
else:
ShootClient(-5.0, -5.0, ns)
count += 1
rate.sleep()
except:
rate.sleep()
def __init__(self):
self.bInitialized = False
self.bRunning = False
# initialize
self.name = 'Flystate2ledpanels'
rospy.init_node(self.name, anonymous=True)
self.nodename = rospy.get_name()
self.namespace = rospy.get_namespace()
# Load the parameters.
self.params = rospy.get_param('%s' % self.nodename.rstrip('/'), {})
self.defaults = {'method': 'voltage', # 'voltage' or 'usb'; How we communicate with the panel controller.
'pattern_id': 1,
'mode': 'velocity', # 'velocity' or 'position'; Fly is controlling vel or pos.
'axis': 'x', # 'x' or 'y'; The axis on which the frames move.
'coeff_voltage':{
'adc0':1, # When using voltage method, coefficients adc0-3 and funcx,y determine how the panels controller interprets its input voltage(s).
'adc1':0, # e.g. xvel = adc0*bnc0 + adc1*bnc1 + adc2*bnc2 + adc3*bnc3 + funcx*f(x) + funcy*f(y); valid on [-128,+127], and 10 corresponds to 1.0.
'adc2':0,
'adc3':0,
'funcx':0,
'funcy':0,
},
'coeff_usb':{ # When using usb method, coefficients x0,xl1,xl2, ... ,yaa,yar,yxi determine the pos or vel command sent to the controller over USB.
'x0': 0.0,
'xl1':1.0,
'xl2':0.0,
'xr1':-1.0,
'xr2':0.0,
'xha':0.0,
'xhr':0.0,
'xaa':0.0,
'xar':0.0,
'xxi':0.0,
'y0': 0.0,
'yl1':0.0,
'yl2':0.0,
'yr1':0.0,
'yr2':0.0,
'yha':0.0,
'yhr':0.0,
'yaa':0.0,
'yar':0.0,
'yxi':0.0,
}
}
SetDict().set_dict_with_preserve(self.params, self.defaults)
self.update_coefficients_from_params()
rospy.set_param('%s' % self.nodename.rstrip('/'), self.params)
self.msgpanels = MsgPanelsCommand(command='all_off',
arg1=0,
arg2=0,
arg3=0,
arg4=0,
arg5=0,
arg6=0)
# Publishers.
self.pubPanelsCommand = rospy.Publisher('/ledpanels/command',
MsgPanelsCommand)
# Subscriptions.
self.subFlystate = rospy.Subscriber('%s/flystate' %
self.namespace.rstrip('/'),
MsgFlystate,
self.flystate_callback,
queue_size=1000)
self.subCommand = rospy.Subscriber('%s/command' %
self.nodename.rstrip('/'),
String,
self.command_callback,
queue_size=1000)
rospy.sleep(1) # Time to connect publishers & subscribers.
self.pubPanelsCommand.publish(
MsgPanelsCommand(command='set_posfunc_id',
arg1=1,
arg2=0,
arg3=0,
arg4=0,
arg5=0,
arg6=0)) # Set default function ch1.
self.pubPanelsCommand.publish(
MsgPanelsCommand(command='set_posfunc_id',
arg1=2,
arg2=0,
arg3=0,
arg4=0,
arg5=0,
arg6=0)) # Set default function ch2.
self.pubPanelsCommand.publish(
MsgPanelsCommand(command='set_pattern_id',
arg1=self.params['pattern_id'],
arg2=0,
arg3=0,
arg4=0,
arg5=0,
arg6=0))
self.pubPanelsCommand.publish(
MsgPanelsCommand(command='set_mode',
arg1=0,
arg2=0,
arg3=0,
arg4=0,
arg5=0,
arg6=0)) # xvel=funcx, yvel=funcy
self.pubPanelsCommand.publish(
MsgPanelsCommand(command='set_position',
arg1=0,
arg2=0,
arg3=0,
arg4=0,
arg5=0,
arg6=0)) # Set position to 0
self.pubPanelsCommand.publish(
MsgPanelsCommand(command='send_gain_bias',
arg1=0,
arg2=0,
arg3=0,
arg4=0,
arg5=0,
arg6=0)) # Set vel to 0
self.pubPanelsCommand.publish(
MsgPanelsCommand(command='stop',
arg1=0,
arg2=0,
arg3=0,
arg4=0,
arg5=0,
arg6=0))
self.pubPanelsCommand.publish(
MsgPanelsCommand(command='all_off',
arg1=0,
arg2=0,
arg3=0,
arg4=0,
arg5=0,
arg6=0))
if (self.params['method'] == 'voltage'):
# Assemble a command: set_mode_(pos|vel)_custom_(x|y)
cmd = 'set_mode'
if (self.params['mode'] == 'velocity'):
cmd += '_vel'
elif (self.params['mode'] == 'position'):
cmd += '_pos'
else:
rospy.logwarn('%s: mode must be '
'velocity'
' or '
'position'
'.' % self.name)
if (self.params['axis'] == 'x'):
cmd += '_custom_x'
elif (self.params['axis'] == 'y'):
cmd += '_custom_y'
else:
rospy.logwarn('%s: axis must be '
'x'
' or '
'y'
'.' % self.name)
# Set the panels controller to the custom mode, with the specified coefficients.
self.pubPanelsCommand.publish(
MsgPanelsCommand(command=cmd,
arg1=self.params['coeff_voltage']['adc0'],
arg2=self.params['coeff_voltage']['adc1'],
arg3=self.params['coeff_voltage']['adc2'],
arg4=self.params['coeff_voltage']['adc3'],
arg5=self.params['coeff_voltage']['funcx'],
arg6=self.params['coeff_voltage']['funcy']))
self.bInitialized = True
self.enemy_scored_dict[target[
"name"]] = self.target_pos_dict[target["name"]]
def getPosition(self):
now = rospy.Time.now()
self.__listener.waitForTransform("{}/odom".format(self.name),
"{}/base_link".format(self.name), now,
rospy.Duration(1.0))
position, quaternion = self.__listener.lookupTransform(
"{}/odom".format(self.name), "{}/base_link".format(self.name), now)
return position, quaternion
if __name__ == '__main__':
rospy.init_node("simple_navigation_goals")
my_bot = MyBot(rospy.get_namespace().replace("/", ""))
current_goal = []
# main loop
while not rospy.is_shutdown():
# check current score
my_bot.updateScore()
# retarget
position, quaternion = my_bot.getPosition()
new_goal = []
targets_dict = my_bot.unscored_dict
targets_dict.update(my_bot.enemy_scored_dict)
#print(targets_dict)
import optparse
parser =\
optparse.OptionParser(
usage="usage: net_monitor.py --diag-hostname=com-X")
parser.add_option("--diag-hostname", dest="diag_hostname",
help="Computer name in diagnostics output (ex: 'com-1')",
metavar="DIAG_HOSTNAME",
action="store", default = hostname)
options, args = parser.parse_args(rospy.myargv())
try:
rospy.init_node('net_monitor_%s' % hostname)
except rospy.exceptions.ROSInitException:
print >> sys.stderr,\
'Network monitor is unable to initialize node. Master may not be running.'
sys.exit(0)
namespace = rospy.get_namespace() or hostname
net_node = NetMonitor(hostname, namespace, options.diag_hostname)
rate = rospy.Rate(1.0)
try:
while not rospy.is_shutdown():
rate.sleep()
net_node.publish_stats()
except KeyboardInterrupt:
pass
except Exception, e:
traceback.print_exc()
rospy.logerr(traceback.format_exc())
net_node.cancel_timers()
sys.exit(0)
#! /usr/bin/env python
import rospy
import actionlib
from cob_cartesian_controller.msg import CartesianControllerAction, CartesianControllerGoal
from cob_cartesian_controller.msg import Profile
if __name__ == '__main__':
rospy.init_node('test_move_lin')
action_name = rospy.get_namespace() + 'cartesian_trajectory_action'
client = actionlib.SimpleActionClient(action_name,
CartesianControllerAction)
rospy.logwarn("Waiting for ActionServer: %s", action_name)
client.wait_for_server()
rospy.logwarn("...done")
# Fill in the goal here
goal = CartesianControllerGoal()
goal.move_type = CartesianControllerGoal.LIN
goal.move_lin.pose_goal.position.x = -0.9
goal.move_lin.pose_goal.position.y = 0.0
goal.move_lin.pose_goal.position.z = 0.0
goal.move_lin.pose_goal.orientation.x = 0.0
goal.move_lin.pose_goal.orientation.y = 0.0
goal.move_lin.pose_goal.orientation.z = 0.0
goal.move_lin.pose_goal.orientation.w = 1.0
goal.move_lin.frame_id = 'world'
goal.profile.vel = 0.2
goal.profile.accl = 0.1
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 != '':
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')
robot_description_param = self.namespace + 'robot_description'
self.use_robot_descr = False
self.axes = {}
if rospy.has_param(robot_description_param):
self.use_robot_descr = True
self.parse_urdf(rospy.get_param(robot_description_param))
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:
if self.namespace != '':
target = '{}base_link'.format(self.namespace)
target = target[1::]
source = '{}base_link_ned'.format(self.namespace)
else:
target = 'base_link'
source = 'base_link_ned'
source = source[1::]
tf_trans_ned_to_enu = tf_buffer.lookup_transform(
target, source, rospy.Time(), rospy.Duration(1))
except Exception as e:
rospy.loginfo('No transform found between base_link and base_link_ned'
' for vehicle {}, message={}'.format(self.namespace, e))
self.base_link_ned_to_enu = None
if tf_trans_ned_to_enu is not None:
self.base_link_ned_to_enu = transformations.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']))
# Set the tf_prefix parameter
rospy.set_param('thruster_manager/tf_prefix', self.namespace)
# 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, namespace=None, arm_name=None):
if None in [namespace, arm_name]:
ns = [
item for item in rospy.get_namespace().split('/')
if len(item) > 0
]
if len(ns) != 2:
rospy.ROSException(
'The controller must be called inside the manipulator namespace'
)
self._namespace = ns[0]
self._arm_name = ns[1]
else:
self._namespace = namespace
self._arm_name = arm_name
if self._namespace[0] != '/':
self._namespace = '/' + self._namespace
if self._namespace[-1] != '/':
self._namespace += '/'
if not rospy.has_param('~gripper'):
raise rospy.ROSException('Gripper configuration not given')
# Retrieve gripper configuration information
self._gripper_config = rospy.get_param('~gripper')
print self._gripper_config
# Retrive gripper type
if 'type' not in self._gripper_config:
raise ROSException('Gripper type not defined')
if self._gripper_config['type'] not in self.TYPE:
raise ROSException('Gripper type not defined')
self._gripper_type = self._gripper_config['type']
self._closed_limit = 'upper'
if 'closed_limit' in self._gripper_config:
self._closed_limit = self._gripper_config['closed_limit']
self._full_open_limit = 'upper'
if 'full_open_limit' in self._gripper_config:
self._full_open_limit = self._gripper_config['full_open_limit']
rospy.loginfo('Gripper type=' + self._gripper_type)
# Chain groups
self._groups = dict()
# Loading the kinematic chain for each finger
for group in self._gripper_config['groups']:
chain = self._gripper_config['groups'][group]
self._groups[group] = KinChainInterface(
name=group,
base=self._gripper_config['base'],
ee_link=chain['ee'],
namespace=self._namespace,
arm_name=self._arm_name,
compute_fk_for_all=False)
# Published topics
self._pubTopics = dict()
# Subscribed topics
self._subTopics = dict()
# Subscribe to the joint states topic
self._subTopics['joint_states'] = rospy.Subscriber(
self._namespace + 'joint_states',
JointState,
self._on_joint_states,
queue_size=1,
tcp_nodelay=True)
# Publish end-effector state
self._pubTopics['state'] = rospy.Publisher(
self._namespace + self._arm_name + '/ee_state',
EndeffectorState,
queue_size=1)
# Set gripper state
self._gripper_state = 'ready'
# Retrieving the name of the control joint
self._control_joint = None
self._control_joint_group = None
if 'control_joint' in self._gripper_config:
self._control_joint = self._gripper_config['control_joint']
# Correct the name including the namespace in TF
for name in self._groups:
for joint in self._groups[name].joint_names:
if self._gripper_config['control_joint'] in joint:
self._control_joint = joint
self._control_joint_group = name
break
if self._control_joint is not None:
break
self._pubTopics['command'] = rospy.Publisher(
self._namespace + self._control_joint + '/controller/command',
Float64,
queue_size=1)
rospy.loginfo(self._control_joint_group + ' - ' +
self._control_joint)
# Publishing rate for state topics
self._publish_rate = 50
# Set timer to regularly publish the state
self._publish_state_timer = rospy.Timer(
rospy.Duration(1.0 / self._publish_rate),
self._publish_endeffector_state)
rospy.on_shutdown(self._on_shutdown)
class Rhino:
#instance of servomotor on a specified port
__myMotor = ServoMotor(
rospy.get_param(rospy.get_namespace() + 'port', '/dev/ttyUSB0'))
__rpm = rospy.get_param(rospy.get_namespace() + 'rpm', 200)
#constructor
def __init__(self):
rospy.init_node('motor', anonymous=True)
#setting up encoder data publisher
self.encoder_pub = rospy.Publisher('encoderTicks',
Int32,
queue_size=10)
Rhino.__myMotor.openSerialPort()
Rhino.__myMotor.isAvailable()
self.maxSpeed = Rhino.__myMotor.getMaxMotorSpeed()
#setting up subscribers
rospy.Subscriber('motor_speed', Float64, self.setSpeed)
rospy.Subscriber('absolute_cmd', Float64, self.setAbsolute)
rospy.Subscriber('relative_cmd', Float64, self.setRelative)
#setting up services
self.read_damping = rospy.Service('read_damping', readDamping,
self.dampingRead)
self.write_damping = rospy.Service('write_damping', writeDamping,
self.dampingWrite)
self.load_factory_settings = rospy.Service('load_factory_settings',
loadFactorySettings,
self.loadFactory)
self.set_position_encoder = rospy.Service('set_position_encoder',
setPositionEncoder,
self.setPositionEnc)
self.get_absolute_position = rospy.Service('get_absolute_position',
getAbsolutePosition,
self.getAbsPos)
self.set_feedback_gain = rospy.Service('set_feedback_gain',
setFeedbackGain, self.setFG)
self.get_feedback_gain = rospy.Service('get_feedback_gain',
getFeedbackGain, self.getFG)
self.set_proportionate_gain = rospy.Service('set_proportionate_gain',
setProportionateGain,
self.setPG)
self.get_proportionate_gain = rospy.Service('get_proportionate_gain',
getProportionateGain,
self.getPG)
self.get_integral_gain = rospy.Service('get_integral_gain',
getIntegralGain, self.getIG)
self.set_integral_gain = rospy.Service('set_integral_gain',
setIntegralGain, self.setIG)
self.set_max_motor_speed = rospy.Service('set_max_motor_speed',
setMaxMotorSpeed,
self.setMaxSpeed)
get_max_motor_speed = rospy.Service('get_max_motor_speed',
getMaxMotorSpeed, self.getMaxSpeed)
self.auto_calibrate = rospy.Service('auto_calibrate', autoCalibrate,
self.autoC)
def spin(self):
r = rospy.Rate(rospy.get_param(rospy.get_namespace() + 'rate', 5))
while not rospy.is_shutdown():
self.update()
r.sleep()
def update(self):
self.data = Int32()
self.data.data = Rhino.__myMotor.getPositionEncoder()
if self.data.data:
self.encoder_pub.publish(self.data)
def __del__(self):
Rhino.__myMotor.closeSerialPort()
def setSpeed(self, msg):
rospy.loginfo(msg)
self.speed = msg.data * 9.5492965855137 * 65000 / Rhino.__rpm / self.maxSpeed #255
Rhino.__myMotor.writeMotorSpeed(self.speed)
def setAbsolute(self, msg):
self.position = msg.data * 286.4788976
Rhino.__myMotor.setAbsolutePostion(self.position)
def setRelative(self, msg):
self.position = msg.data * 286.4788976
Rhino.__myMotor.setRelativePostion(self.position)
def dampingRead(self, request):
return readDampingResponse(Rhino.__myMotor.readDamping())
def dampingWrite(self, request):
Rhino.__myMotor.writeDamping(request.damp)
return []
def loadFactory(self, request):
Rhino.__myMotor.loadFactorySettings()
return []
def setPositionEnc(self, request):
Rhino.__myMotor.setPositionEncoder(request.encoder)
return []
def getAbsPos(self, request):
return getAbsolutePositionResponse(
Rhino.__myMotor.getAbsolutePostion())
def setFG(self, request):
Rhino.__myMotor.setFeedbackGain(request.gain)
return []
def getFG(self, request):
return getFeedbackGainResponse(Rhino.__myMotor.getFeedbackGain())
def setPG(self, request):
Rhino.__myMotor.setProportionateGain(request.pgain)
return []
def getPG(self, request):
return getProportionateGainResponse(
Rhino.__myMotor.getProportionateGain())
def setIG(self, request):
Rhino.__myMotor.setIntegralGain(request.igain)
return []
def getIG(self, request):
return getIntegralGainResponse(Rhino.__myMotor.getIntegralGain())
def setMaxSpeed(self, request):
Rhino.__myMotor.setMaxMotorSpeed(request.mspeed)
return []
def getMaxSpeed(self, request):
return getMaxMotorSpeedResponse(Rhino.__myMotor.getMaxMotorSpeed())
def autoC(self, request):
Rhino.__myMotor.autoCalibrate()
return []
def add_gazebo_model(model_name, model_xml, initial_pose):
rospy.wait_for_service("/gazebo/spawn_sdf_model")
spawn_sdf_model = rospy.ServiceProxy("/gazebo/spawn_sdf_model", SpawnModel)
spawn_sdf_model(model_name, model_xml, rospy.get_namespace(), initial_pose, "")
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
rospy.loginfo(
'ThrusterManager::update_rate=' + str(self.config['update_rate']))
# Set the tf_prefix parameter
rospy.set_param('thruster_manager/tf_prefix', self.namespace)
# 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)
print '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)
print 'Thruster allocation matrix provided!'
print 'TAM='
print 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:
print 'Invalid output directory for the TAM matrix, dir=', self.output_dir
self.ready = True
print ('ThrusterManager: ready')
def handle_robot_pose(msg, robot_name):
br = tf2_ros.TransformBroadcaster()
t = geometry_msgs.msg.TransformStamped()
t.header.stamp = rospy.Time.now()
t.header.frame_id = "world"
t.child_frame_id = robot_name + '/map'
t.transform.translation = msg.pose.pose.position
t.transform.rotation = msg.pose.pose.orientation
try:
trans = tfBuffer.lookup_transform(robot_name + '/odom',
robot_name + '/map', rospy.Time())
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException) as e:
t.child_frame_id = robot_name + '/odom' # if can't find robot#/map to robot#/odom tf, then connect world to robot#/odom instead
br.sendTransform(t)
# [INITIALIZE NODE]
rospy.init_node('tf_broadcaster')
tfBuffer = tf2_ros.Buffer()
listener = tf2_ros.TransformListener(tfBuffer)
robot_name = rospy.get_namespace()[1:-1]
rospy.Subscriber('odom', Odometry, handle_robot_pose, robot_name)
# [MAIN CONTROL LOOP]
if __name__ == '__main__':
rospy.spin()
