def __init__(self):
node_name = rospy.get_name()
#print "node_name: " + str(node_name)
rospy.init_node("button_1_smoother")
node_name = rospy.get_name()
#print "node_name: " + str(node_name)
rate = rospy.get_param("~rate", 14.429)
self.history_size = rospy.get_param("~history_size", 5)
self.thresh = rospy.get_param("~thresh", 0.05)
try:
self.input_topic = rospy.get_param("~input_topic")
except:
err = 'Please specify an input topic'
rospy.logerr('Please specify an input topic')
raise Exception(err)
try:
self.output_topic = rospy.get_param("~output_topic")
except:
err = 'Please specify an output topic'
rospy.logerr(err)
raise Exception(err)
self.tf = tf.TransformListener()
self.br = tf.TransformBroadcaster()
self.rate = rospy.Rate(rate)
self.history = RingBuffer(self.history_size)
self.parent = self.get_parent(self.input_topic)
def triggerParser(self, trigger):
rospy.loginfo(rospy.get_name()+"From Trigger: %s", trigger.data)
if trigger.data.find("wrong") > -1:
self.stopTrigger()
self.startCommand()
else:
rospy.loginfo(rospy.get_name()+" can't find the trigger word")
def updater(self): while not rospy.is_shutdown(): if self.wii_a == True and self.wii_a_changed == True: self.wii_a_changed = False rospy.loginfo(rospy.get_name() + ': Current position: %.3f %.3f' % (self.wptnav.pose[0], self.wptnav.pose[1])) if self.wii_home == True and self.wii_home_changed == True: self.wii_home_changed = False rospy.loginfo(rospy.get_name() + ": User reloaded waypoint list") self.load_wpt_list() self.goto_next_wpt() if self.wii_up == True and self.wii_up_changed == True: self.wii_up_changed = False rospy.loginfo(rospy.get_name() + ": User skipped waypoint") self.goto_next_wpt() if self.wii_down == True and self.wii_down_changed == True: self.wii_down_changed = False rospy.loginfo(rospy.get_name() + ": User selected previous waypoint") self.goto_previous_wpt() if self.automode: ros_time = rospy.Time.now() time = ros_time.secs + ros_time.nsecs*1e-9 (self.status, self.linear_speed, self.angular_speed) = self.wptnav.update(time) if self.status == self.wptnav.UPDATE_ARRIVAL: self.goto_next_wpt() else: self.publish_cmd_vel_message() if self.status_publish_interval != 0: self.status_publish_count += 1 if self.status_publish_count % self.status_publish_interval == 0: self.publish_status_message() self.r.sleep()
def __init__(self):
self.node_name = rospy.get_name()
# self.d_PerCount = ((3.14159265 * 0.13) / 5940)*0.3/0.635
# self.Length = 0.18
self.R = 0
self.L = 0
self.err_sumR = 0
self.err_sumL = 0
self.err_lastR = 0
self.err_lastL = 0
self.swith = True
self.pretime = 0.0
self.sub_joint_state_car = rospy.Subscriber('/duckiebot_with_gripper/joint_states', JointState, self.cbJoinstate, queue_size=1)
# Subscription
self.pub_car_cmd = rospy.Publisher("/gazebo_sub_jointstate/control_value",Point,queue_size=1)
# self.pub_threshold = rospy.Publisher("/gazebo_sub_jointstate/threshold_value",UInt32,queue_size=1)
# self.timer = rospy.Timer(rospy.Duration.from_sec(1.0), self.updateParams)
srv = Server(PIDConfig, self.callback)
self.sub_encoder = rospy.Subscriber("/encoder", Point, self.cbEncoder, queue_size=1)
self.sub_reset_encoder = rospy.Subscriber("/reset_encoder", Bool, self.cbReset, queue_size=1)
# safe shutdown
rospy.on_shutdown(self.custom_shutdown)
# timer
rospy.loginfo("[%s] Initialized " %(rospy.get_name()))
def on_automode_message(self, msg):
if msg.data == True: # if autonomous mode requested
if self.state == self.STATE_IDLE:
if self.wptnav.pose != False: # if we have a valid pose
(num_wpt, next_wpt) = self.wptlist.status()
if num_wpt > 0:
self.state = self.STATE_NAVIGATE
if self.wptnav.state == self.wptnav.STATE_STANDBY:
self.wptnav.resume()
rospy.loginfo(rospy.get_name() + ": Resuming waypoint navigation")
elif self.wptnav.state == self.wptnav.STATE_STOP:
self.goto_first_wpt()
rospy.loginfo(rospy.get_name() + ": Switching to waypoint navigation")
else:
if self.no_route_plan_warn == False:
rospy.logwarn(rospy.get_name() + ": No route plan available")
self.no_route_plan_warn = True
else: # no valid pose yet
if self.automode_warn == False:
self.automode_warn = True
rospy.logwarn(rospy.get_name() + ": Absolute pose is required for autonomous navigation")
else: # if manual mode requested
if self.state != self.STATE_IDLE:
self.state = self.STATE_IDLE
self.wptnav.standby()
rospy.loginfo(rospy.get_name() + ": Switching to manual control")
def __init__(self):
'''
'''
# initialize as anonymous node
rospy.init_node("anon_node_B", anonymous = True)
# register exit handler
atexit.register(self.exit_handler)
# register SIGINT handler
signal.signal(signal.SIGINT, self.signal_handler)
# create dummy chatter node for testing purposes
pub = rospy.Publisher(rospy.get_name() + "/type_B_chatter", String)
# initialize anonymous name publisher
name_pub = rospy.Publisher(ANON_NAME_TOPIC, String)
time.sleep(0.25)
# create standard dummy message
my_msg = rospy.get_param("/" + rospy.get_name() + "/" + str(MSG_PARAM))
# run!
# publish anonymous name so node launcher can see
stringy = "%s" % str(rospy.get_name())
name_pub.publish(stringy)
while not rospy.is_shutdown():
stringy = "Chatter: %s \n@ %s" % (my_msg, rospy.get_time())
pub.publish(stringy)
time.sleep(1)
def parse_cline(self,argv):
self.wszp = 0.25
self.ovrp = 0.125
self.pwr = True
try:
opts, args = getopt.getopt(argv, 'w:o:p:', ['window-size=','overlap=','powers_of_two='])
except:
traceback.print_exc()
rospy.loginfo(rospy.get_name()+': Command line fail')
sys.exit(2)
for opt, arg in opts:
if opt in ('-w','--window-size'):
rospy.loginfo(rospy.get_name()+': window-size='+arg)
self.wszp = float(arg)
elif opt in ('-o','--overlap'):
rospy.loginfo(rospy.get_name()+': overlap='+arg)
self.ovrp = float(arg)
elif opt in ('-p','--powers_of_two'):
if arg=='False':
self.pwr = False
rospy.loginfo(rospy.get_name()+': powers_of_two='+str(self.pwr))
if self.wszp <= 0:
self.wszp = 0.25
if self.ovrp <= 0:
self.ovrp = 0.125
if self.ovrp > self.wszp:
self.ovrp = self.wszp
def updater(self): while not rospy.is_shutdown(): # default is True prev_act_en = self.act_en_msg.data self.act_en_msg.data = True # cricital fault if true or too old if self.critfault_en == True: if self.critfault_state != 0 or self.critfault_next_tout < rospy.get_time(): self.act_en_msg.data = False # deadman fault if false or too old if self.deadman_en == True: if self.deadman_state == False or self.deadman_next_tout < rospy.get_time(): self.act_en_msg.data = False #print self.deadman_next_tout, rospy.get_time(), self.deadman_state # publish actuation_enable message self.act_en_msg.header.stamp = rospy.get_rostime() self.act_en_pub.publish (self.act_en_msg) if prev_act_en != self.act_en_msg.data: if self.act_en_msg.data == True: rospy.logwarn(rospy.get_name() + ": Enabling actuation") else: rospy.logwarn(rospy.get_name() + ": Disabling actuation") # go back to sleep self.r.sleep()
def updater(self): while not rospy.is_shutdown(): if self.hmi_digital_joy_a_up == True and self.hmi_digital_joy_a_up_changed == True: self.hmi_digital_joy_a_up_changed = False rospy.loginfo(rospy.get_name() + ": User skipped waypoint") self.goto_next_wpt() if self.hmi_digital_joy_a_down == True and self.hmi_digital_joy_a_down_changed == True: self.hmi_digital_joy_a_down_changed = False rospy.loginfo(rospy.get_name() + ": User selected previous waypoint") self.goto_previous_wpt() if self.state == self.STATE_NAVIGATE: (self.status, self.linear_vel, self.angular_vel) = self.wptnav.update(rospy.get_time()) if self.status == self.wptnav.UPDATE_ARRIVAL: rospy.loginfo(rospy.get_name() + ": Arrived at waypoint: %s (error %.2fm)" % (self.wpt[self.wptnav.W_ID], self.wptnav.dist)) # activate wait mode if self.wpt[self.wptnav.W_WAIT] >= 0.0: self.wait_after_arrival = self.wpt[self.wptnav.W_WAIT] else: self.wait_after_arrival = self.wpt_def_wait_after_arrival if self.wait_after_arrival > 0.01: self.linear_vel = 0.0 self.angular_vel = 0.0 self.publish_cmd_vel_message() self.publish_implement_message() rospy.loginfo(rospy.get_name() + ": Waiting %.1f seconds" % (self.wait_after_arrival)) self.state = self.STATE_WAIT self.wait_timeout = rospy.get_time() + self.wait_after_arrival else: self.state = self.STATE_NAVIGATE self.goto_next_wpt() else: self.publish_cmd_vel_message() self.publish_implement_message() elif self.state == self.STATE_WAIT: if rospy.get_time() > self.wait_timeout: self.state = self.STATE_NAVIGATE self.goto_next_wpt() else: self.linear_vel = 0.0 self.angular_vel = 0.0 self.publish_cmd_vel_message() self.publish_implement_message() # publish status if self.status_publish_interval != 0: self.status_publish_count += 1 if self.status_publish_count % self.status_publish_interval == 0: self.publish_status_message() # publish pid if self.pid_publish_interval != 0: self.pid_publish_count += 1 if self.pid_publish_count % self.pid_publish_interval == 0: self.publish_pid_message() self.r.sleep()
def _load_parameters(cls, masteruri, params, clear_params):
"""
Load parameters onto the parameter server
"""
import roslaunch
import roslaunch.launch
import xmlrpclib
param_server = xmlrpclib.ServerProxy(masteruri)
p = None
try:
# multi-call style xmlrpc
param_server_multi = xmlrpclib.MultiCall(param_server)
# clear specified parameter namespaces
# #2468 unify clear params to prevent error
for p in clear_params:
param_server_multi.deleteParam(rospy.get_name(), p)
r = param_server_multi()
# for code, msg, _ in r:
# if code != 1:
# raise StartException("Failed to clear parameter: %s"%(msg))
# multi-call objects are not reusable
param_server_multi = xmlrpclib.MultiCall(param_server)
for p in params.itervalues():
# suppressing this as it causes too much spam
#printlog("setting parameter [%s]"%p.key)
param_server_multi.setParam(rospy.get_name(), p.key, p.value)
r = param_server_multi()
for code, msg, _ in r:
if code != 1:
raise StartException("Failed to set parameter: %s"%(msg))
except roslaunch.core.RLException, e:
raise StartException(e)
def __init__(self):
rospy.loginfo(rospy.get_name() + ": Start")
# defines
self.c = '$'
# static parameters
self.update_rate = 100 # set update frequency [Hz]
# get parameters
self.device = rospy.get_param("~device", "/dev/ttyUSB0") #
# get topic names
self.topic_timing = rospy.get_param("~timing_pub",'/fmInformation/rt_timing')
# setup topic publishers
self.timing_pub = rospy.Publisher(self.topic_timing, IntStamped)
self.timing_msg = IntStamped()
# configure and open serial device
ser_error = False
try :
self.ser = serial.Serial(self.device, 57600, timeout=0)
except Exception as e:
rospy.logerr(rospy.get_name() + ": Unable to open serial device: %s" % self.device)
self.ser.close()
ser_error = True
if ser_error == False:
# call updater function
self.r = rospy.Rate(self.update_rate)
self.updater()
def kinect_cb(self,rawDepthImage):
# If data is requested for localization
if(rospy.get_param('visual_tracker')==rospy.get_name()):
self.cloud= read_points(rawDepthImage, field_names=None, skip_nans=False, uvs=[])
# Convert a cloud generator to a list
self.points = []
for pt in self.cloud:
pt = list(pt)
pt.append(1)
self.points.append(pt)
nearestPoint=self.getNearestPoint()
print nearestPoint[2]
# Create a new message to be sent to ROSToArduino
messageToROS=fromObserver_msg()
messageToROS.observerID=rospy.get_name()
messageToROS.coordinates=(nearestPoint)
self.KinectToROS_pub.publish(messageToROS)
# Reset the visual_tracker parameter
rospy.set_param('visual_tracker','')
def __init__(self):
self.node_name = rospy.get_name()
self.lane_reading = None
self.pub_counter = 0
# Setup parameters
self.setGains()
#cv
self.bridge = CvBridge()
self.eyes=(0,0,0,0)
# Publicaiton
self.pub_car_cmd = rospy.Publisher("~car_cmd",Twist2DStamped,queue_size=1)
self.pub_image_original = rospy.Publisher("~image_original", Image, queue_size=1)
self.pub_image_gray = rospy.Publisher("~image_gray", Image, queue_size=1)
# Subscriptions
self.sub_lane_reading = rospy.Subscriber("~image", CompressedImage, self.cbPose, queue_size=1)
# safe shutdown
rospy.on_shutdown(self.custom_shutdown)
# timer
self.gains_timer = rospy.Timer(rospy.Duration.from_sec(1.0), self.getGains_event)
rospy.loginfo("[%s] Initialized " %(rospy.get_name()))
def __call__(cls, *args, **kwargs):
"""Called when you call ConnectionBasedTransport()"""
obj = type.__call__(cls, *args, **kwargs)
# display node input/output topics
parser = argparse.ArgumentParser()
parser.add_argument('--inout', action='store_true')
args = parser.parse_args(rospy.myargv()[1:])
if args.inout:
obj.subscribe()
tp_manager = rospy.topics.get_topic_manager()
print('Publications:')
for topic, topic_type in tp_manager.get_publications():
if topic == '/rosout':
continue
topic = unresolve_name(rospy.get_name(), topic)
print(' * {0} [{1}]'.format(topic, topic_type))
print('Subscriptions:')
for topic, topic_type in tp_manager.get_subscriptions():
topic = unresolve_name(rospy.get_name(), topic)
print(' * {0} [{1}]'.format(topic, topic_type))
sys.exit(0)
obj._post_init()
return obj
def __init__(self, tag_pose_window=1.0, progress_timeout=5.,
setup_transform=None, grasp_transform=None):
#TODO: GET ALL THE MAGIC NUMBERS INTO PARAMETERS
#TODO: Collect all magic strings into parameters
self.tag_pose_window = rospy.Duration(tag_pose_window)
self.progress_timeout = rospy.Duration(progress_timeout)
self.setup_transform = setup_transform if setup_transform is not None else [0]*6
self.grasp_transform = grasp_transform if grasp_transform is not None else [0]*6
self.gripper_pose = None
self.gripper_pose_sub = rospy.Subscriber('haptic_mpc/gripper_pose', PoseStamped, self.pose_cb)
self.tag_poses = {}
self.tag_sub = rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.tag_pose_cb)
self.goal_pose_pub = rospy.Publisher('haptic_mpc/goal_pose', PoseStamped)
self.test_pub_1 = rospy.Publisher('test_pose_1', PoseStamped, latch=True)
self.test_pub_2 = rospy.Publisher('test_pose_2', PoseStamped, latch=True)
self.last_pos_err = self.last_ort_err = np.inf
self.last_progress_time = rospy.Time.now()
self.gripper_ac = actionlib.SimpleActionClient('l_gripper_controller/gripper_action', Pr2GripperCommandAction)
rospy.loginfo("[%s] Waiting for gripper action server" %(rospy.get_name()))
if self.gripper_ac.wait_for_server(rospy.Duration(10.0)):
rospy.loginfo("[%s] Gripper action server started" %(rospy.get_name()))
else:
rospy.logerr("[%s] Gripper action server not found" %(rospy.get_name()))
self.action_server = actionlib.SimpleActionServer('ar_tool_grasp_action', ARToolGraspAction, self.grasp_cb, False)
self.action_server.start()
rospy.loginfo("[%s] AR Tool Grasp Action Server Started" %(rospy.get_name()))
def goal_cb(self, pt_msg):
rospy.loginfo("[{0}] New LookatIK goal received.".format(rospy.get_name()))
if (pt_msg.header.frame_id != self.camera_ik.base_frame):
try:
now = pt_msg.header.stamp
self.tfl.waitForTransform(pt_msg.header.frame_id,
self.camera_ik.base_frame,
now, rospy.Duration(1.0))
pt_msg = self.tfl.transformPoint(self.camera_ik.base_frame, pt_msg)
except (LookupException, ConnectivityException, ExtrapolationException):
rospy.logwarn("[{0}] TF Error tranforming point from {1} to {2}".format(rospy.get_name(),
pt_msg.header.frame_id,
self.camera_ik.base_frame))
target = np.array([pt_msg.point.x, pt_msg.point.y, pt_msg.point.z])
# Get IK Solution
current_angles = list(copy.copy(self.joint_angles_act))
iksol = self.camera_ik.lookat_ik(target, current_angles[:len(self.camera_ik.arm_indices)])
# Start with current angles, then replace angles in camera IK with IK solution
# Use desired joint angles to avoid sagging over time
jtp = JointTrajectoryPoint()
jtp.positions = list(copy.copy(self.joint_angles_des))
for i, joint_name in enumerate(self.camera_ik.arm_joint_names):
jtp.positions[self.joint_names.index(joint_name)] = iksol[i]
deltas = np.abs(np.subtract(jtp.positions, current_angles))
duration = np.max(np.divide(deltas, self.max_vel_rot))
jtp.time_from_start = rospy.Duration(duration)
jt = JointTrajectory()
jt.joint_names = self.joint_names
jt.points.append(jtp)
rospy.loginfo("[{0}] Sending Joint Angles.".format(rospy.get_name()))
self.joint_traj_pub.publish(jt)
def __init__(self, robotfile, manipname, freeindices,
freeinc=[0.75, 0.75, 0.75],
weights = [1.0, 0.95, 0.8, 0.66, 0.2]):
self.side = side
self.weights=weights
self.env = op.Environment()
self.env.Load(robotfile)
self.robot = self.env.GetRobots()[0]
self.base_frame = self.robot.GetLinks()[0].GetName()
self.manip = self.robot.SetActiveManipulator(manipname)
self.arm_joint_names = [self.robot.GetJoints()[ind].GetName() for ind in self.manip.GetArmJoints()]
self.arm_indices = self.manip.GetArmIndices()
self.joint_angles = [0]*len(self.arm_indices)
freejoints = [self.robot.GetJoints()[ind].GetName() for ind in freeindices]
self.ikmodel = op.databases.inversekinematics.InverseKinematicsModel(self.robot,
iktype=op.IkParameterization.Type.Lookat3D,
forceikfast=True,
freeindices=freeindices,
freejoints=freejoints)
self.ikmodel.freeinc = freeinc
if not self.ikmodel.load():
rospy.loginfo("[%s] LookatIK Model not available: Generating" % rospy.get_name())
self.ikmodel.generate()
self.ikmodel.save()
elif FORCE_REBUILD:
rospy.loginfo("[%s] Force rebuild of LookatIK Model: Generating" % rospy.get_name())
self.ikmodel.generate()
self.ikmodel.save()
else:
rospy.loginfo("[%s] Lookat IK Model Loaded" % rospy.get_name())
def _prepareROSMaster(cls, masteruri):
if not masteruri:
masteruri = roslib.rosenv.get_master_uri()
#start roscore, if needed
try:
master = xmlrpclib.ServerProxy(masteruri)
master.getUri(rospy.get_name())
except:
# run a roscore
from urlparse import urlparse
master_port = str(urlparse(masteruri).port)
new_env = dict(os.environ)
new_env['ROS_MASTER_URI'] = masteruri
cmd_args = [nm.ScreenHandler.getSceenCmd(''.join(['/roscore', '--', master_port])), 'roscore', '--port', master_port]
subprocess.Popen(shlex.split(' '.join([str(c) for c in cmd_args])), env=new_env)
# wait for roscore to avoid connection problems while init_node
result = -1
count = 0
while result == -1 and count < 2:
try:
master = xmlrpclib.ServerProxy(masteruri)
result, uri, msg = master.getUri(rospy.get_name())
except:
time.sleep(1)
count += 1
def on_automode_message(self, msg):
if msg.data == True: # if autonomous mode requested
if self.state == self.STATE_IDLE:
if self.wptnav.pose != False: # if we have a valid pose
self.state = self.STATE_NAVIGATE
rospy.loginfo(rospy.get_name() + ": Switching to waypoint navigation")
if self.wptlist_loaded == False: # If wpts not already loaded then load the wpts and then make the robot move to the next wpt
rospy.loginfo(rospy.get_name() + ": Loading waypoint list")
self.load_wpt_list()
self.goto_next_wpt()
self.wptlist_loaded = True
elif self.wptnav.state == self.wptnav.STATE_STANDBY: # If robot is in wait state then make the robot resume the navigation
self.wptnav.resume()
rospy.loginfo(rospy.get_name() + ": Resuming waypoint navigation")
else: # no valid pose yet
if self.automode_warn == False:
self.automode_warn = True
rospy.logwarn(rospy.get_name() + ": Absolute pose is required for autonomous navigation")
else: # if manual mode requested
if self.state != self.STATE_IDLE:
self.state = self.STATE_IDLE
self.wptnav.standby()
rospy.loginfo(rospy.get_name() + ": Switching to manual control")
def objectDetected(msg): if msg.obj_detected == 0: updateMood(-15,0) rospy.loginfo(rospy.get_name()+"Detected not bottle") elif msg.obj_detected == 1: updateMood(20,1) rospy.loginfo(rospy.get_name()+"Bottle detected")
def on_rc_topic(self, msg): # get remote control behaviour if (msg.switches & 0x01) == 0: behaviour = self.BHV_RC else: behaviour = self.BHV_NAV # if behaviour changed if behaviour != self.bhv: # suspend current behavour if self.bhv == self.BHV_RC: self.bhv_rc.suspend() elif self.bhv == self.BHV_NAV: self.bhv_wn.suspend() # activate new behaviour self.bhv = behaviour if self.bhv == self.BHV_RC: self.bhv_rc.activate() rospy.loginfo(rospy.get_name() + ": Switching to remote control behaviour") elif self.bhv == self.BHV_NAV: self.bhv_wn.activate() rospy.loginfo(rospy.get_name() + ": Switching to waypoint navigation behaviour") self.publish_active_behaviour_message() # pass remote control info to current behaviour if self.bhv == self.BHV_RC: self.bhv_rc.on_rc_topic(msg) elif self.bhv == self.BHV_NAV: self.bhv_wn.on_rc_topic(msg)
def goal_data_cb(self, msg):
# Receive: desired (goal) pose of tag
print "Tag goal pose (received): ", msg.tag_goal_pose
if not 'base_link' in msg.tag_goal_pose.header.frame_id:
#Transform to base link if necessary
try:
now = rospy.get_rostime()
msg.tag_goal_pose.header.stamp = now
self.tfl.waitForTransform("base_link", msg.tag_goal_pose.header.frame_id,
now, rospy.Duration(5.0))
msg.tag_goal_pose = self.tfl.transformPose('base_link', msg.tag_goal_pose)
except Exception as e:
rospy.logerr("[%s] TF Error:\r\n%s" % (rospy.get_name(), e.message))
return
self.debug_pub.publish(msg.tag_goal_pose)
tag_goal_xyt = self.pose_to_2d(msg.tag_goal_pose) #convert tag goal to (x,y,theta) triple
print "Tag goal pose (transformed): ", msg.tag_goal_pose
print "Tag goal tuple: ", tag_goal_xyt
#Stop existing state machine if necessary
if self.servo_sm_thread is not None:
self.servo_sm_thread.preempt()
self.servo_sm_thread.join(10)
if self.servo_sm_thread.is_alive():
rospy.logerr("[%s] Old servoing thread is not dead yet!" % rospy.get_name())
return
#Start new state machine with new data
self.servo_sm_thread = ServoOnTagGoal(msg.marker_topic,
tag_goal_xyt,
msg.tag_id,
self.find_tag_timeout)
self.servo_sm_thread.start()
rospy.loginfo("[%s] Started new servo state machine." % rospy.get_name())
def __init__(self,publish_predicates=True,start_subscriber=True,gripper_name='c_model'):
self.valid_predicates = ValidPredicates()
self.valid_predicates.assignments = assignments=[gripper_name]
self.valid_predicates.predicates = ['gripper_open','gripper_closed','gripper_moving',
'gripper_basic_mode','gripper_pinch_mode','gripper_wide_mode',
'gripper_scissor_mode','gripper_activated',
'finger_a_contact','finger_b_contact','finger_c_contact',
'any_finger_contact']
self.valid_predicates.pheader.source = rospy.get_name()
self.predicate_msg = PredicateList()
self.gripper_name = gripper_name
self.publish_predicates = publish_predicates
if self.publish_predicates:
# create predicator things
self.pub = rospy.Publisher("predicator/input",PredicateList,queue_size=1000)
self.vpub = rospy.Publisher("predicator/valid_input",ValidPredicates,queue_size=1000)
if start_subscriber:
self.sub = rospy.Subscriber("CModelRobotInput",inputMsg,self.callback)
self.name = rospy.get_name()
self.gripper_mode = 'pinch_mode'
self.activated = False
self.contact = False
self.closed = False
self.moving = False
def __init__(self):
rospy.loginfo(rospy.get_name() + ": Start")
# defines
self.count = 0
# static parameters
self.update_rate = 50 # set update frequency [Hz]
self.deadman_tout_duration = 0.2 # [s]
self.cmd_vel_tout_duration = 0.2 # [s]
# get parameters
addr = rospy.get_param("~socket_address",'localhost')
port = rospy.get_param("~socket_port",'8080')
timeout = rospy.get_param("~socket_timeout",'5')
self.debug = rospy.get_param("~debug",'false')
# get topic names
self.topic_routept = rospy.get_param("~routept_pub",'/fmPlan/route_point')
# setup topic publishers
self.routept_pub = rospy.Publisher(self.topic_routept, RoutePt)
self.routept_msg = RoutePt()
# setup subscription topic callbacks
# setup socket
self.sd = socketd(addr, port, timeout, self.debug)
if self.sd.socket_ok == True:
# call updater function
self.r = rospy.Rate(self.update_rate)
self.updater()
else:
(msg_type, msg_text) = self.sd.message()
rospy.logerr(rospy.get_name() + ": %s", msg_text)
def CallbackStatus(self, data):
try:
sensor = next(s for s in self._sensors if str(s['communication_id']) == str(data.communication_id))
for type in self._sensorTypes:
#We will need to check which sensorType our sensor has in order to interpret the value
if(str(sensor['sensorType']) == str(type['id'])):
#Add the uninterpreted value into the value field
sensor['value'] = data.value
#Set the last_interpreted value to the current one
sensor['last_interpreted_value'] = sensor['interpreted_value']
#set the last updated datetime
sensor['lastUpdated'] = time.strftime('%Y-%m-%d %H:%M:%S')
#check if we have an analog sensor or not
if(type['onValue'] != None and type['offValue'] != None):
if(str(data.value) == "0"):
#interpreted value - set to value set in database
sensor['interpreted_value'] = type['offValue']
elif (str(data.value) == "1"):
##interpreted value - set to value set in database
sensor['interpreted_value'] = type['onValue']
#we had an analog sensor, so we will write the raw value into the value
else:
sensor['interpreted_value'] = data.value
#Write the new sensor information to the database
SensorsInDatabase().UpdateSensorValue(sensor)
rospy.loginfo(rospy.get_name() + ": Sensor status update (%s): %s" % (sensor['name'], sensor['interpreted_value']))
except StopIteration:
rospy.loginfo(rospy.get_name() + ": Sensor update for unknown sensor with name (%s) and id %s - please add it to the database" % (data.name, data.communication_id))
def __init__(self):
# defines
rospy.loginfo(rospy.get_name() + ": Start")
# get parameters
self.update_rate = rospy.get_param("~update_rate", "5") # update frequency [Hz]
self.offset_e = rospy.get_param("~easting_offset",0.0) # [m]
self.offset_n = rospy.get_param("~northing_offset",0.0) # [m]
self.trkpt_threshold = rospy.get_param("~trackpoint_threshold",0.1) # [m]
map_title = rospy.get_param("~map_title", "Track")
map_window_size = rospy.get_param("~map_window_size",5.0) # [inches]
# initialize the polygon map
self.polyplot = polygon_map_plot(map_title, map_window_size, self.offset_e, self.offset_n)
# import polygons
file = open('polygon_map.txt', 'r')
file_content = csv.reader(file, delimiter='\t')
for name,e1,n1,e2,n2,e3,n3,e4,n4 in file_content:
polygon = [[float(e1),float(n1)],[float(e2),float(n2)],[float(e3),float(n3)],[float(e4),float(n4)]]
self.polyplot.add_polygon (polygon)
file.close()
rospy.loginfo(rospy.get_name() + ": Loaded %ld polygons" % self.polyplot.poly_total)
# get topic names
self.pose_topic = rospy.get_param("~pose_sub",'/fmKnowledge/pose')
self.polygon_map_topic = rospy.get_param("~polygon_map_pub",'/fmKnowledge/polygon_map')
# setup subscription topic callbacks
rospy.Subscriber(self.pose_topic, Odometry, self.on_pose_message)
rospy.Subscriber(self.polygon_map_topic, IntStamped, self.on_polygon_map_message)
# call updater function
self.r = rospy.Rate(self.update_rate)
self.updater()
def __init__(self):
self.node_name = rospy.get_name()
self.vicon_name = self.setupParameter("~vicon_veh_name","duckiecar")
self.vicon_pose_queue = []
self.last_pub_time = rospy.Time.now()
self.lane_pose = None
self.lane_pose_queue = None
# Setup Parameters
self.pub_freq = self.setupParameter("~pub_freq",50.0) # 50 Hz
self.pub_delay = self.setupParameter("~pub_delay",0.0)
self.x_goal = self.setupParameter("~x_goal",0.0)
self.phi_goal = self.setupParameter("~phi_goal",-math.pi/2)
# Publications
self.pub_lane_reading = rospy.Publisher("~lane_pose", LanePose, queue_size=1)
# Subscriptions
self.sub_vicon = rospy.Subscriber("~vicon_pose", PoseStamped, self.cbPose)
# timer
self.pub_timer = rospy.Timer(rospy.Duration.from_sec(1.0/self.pub_freq),self.cbTimerPub)
self.timer_param = rospy.Timer(rospy.Duration.from_sec(1.0),self.cbParamUpdate)
rospy.loginfo("[%s] Initialized " %(rospy.get_name()))
def __init__(self):
self.node_name = rospy.get_name()
# vehicle position update by mocap
self.position = Point()
# vehicle yaw
self.yaw = 0
self.dist = 0
# state switch
self.switch = True
# arrived label
self.arrived = False
# patrol mode in S type
self.label = 0
# target_point
self.X = [1.04, 1.01, -0.11, -0.08, 1.02, 0.98, -0.14, -0.08, 1]
self.Y = [0.35, 0.71, 0.58, 0.96, 0.94, 1.3, 1.2, 1.55, 1.53]
# spining_angle
self.R = [30 60 90 120 150 180 210 240 270 300 330 355]
self.R_order = 0
# Publicaiton
self.pub_car_cmd_ = rospy.Publisher("~car_cmd",Twist2DStamped,queue_size=1)
# Subscription
self.sub_vehicle_pose_ = rospy.Subscriber("~vehicle_pose", Pose, self.cbPose, queue_size=1)
# safe shutdown
rospy.on_shutdown(self.custom_shutdown)
# timer
rospy.loginfo("[%s] Initialized " %(rospy.get_name()))
def __init__(self):
rospy.init_node('motion_service')
# Load config
config_loader = RobotConfigLoader()
try:
robot_config_name = rospy.get_param(rospy.get_name() + '/robot_config')
except KeyError:
rospy.logwarn('Could not find robot config param in /' + rospy.get_name +'/robot_config. Using default config for '
'Thor Mang.')
robot_config_name = 'thor'
config_loader.load_xml_by_name(robot_config_name + '_config.xml')
# Create publisher for first target
if len(config_loader.targets) > 0:
self._motion_publisher = MotionPublisher(
config_loader.robot_config, config_loader.targets[0].joint_state_topic, config_loader.targets[0].publisher_prefix)
else:
rospy.logerr('Robot config needs to contain at least one valid target.')
self._motion_data = MotionData(config_loader.robot_config)
# Subscriber to start motions via topic
self.motion_command_sub = rospy.Subscriber('motion_command', ExecuteMotionCommand, self.motion_command_request_cb)
# Create action server
self._action_server = SimpleActionServer(rospy.get_name() + '/motion_goal', ExecuteMotionAction, None, False)
self._action_server.register_goal_callback(self._execute_motion_goal)
self._action_server.register_preempt_callback(self._preempt_motion_goal)
self._preempted = False
def __init__(self):
self.node_name = rospy.get_name()
self.scale= rospy.get_param("~scale")
self.shift_x = rospy.get_param("~shift_x")
self.shift_y = rospy.get_param("~shift_y")
self.line_marker = Marker()
self.color =ColorRGBA()
self.set_line_marker()
self.x = 0
self.y = 0
self.th = 0
self.d_PerCount = ((3.14159265 * 0.13) / 5940)*0.3/0.635
self.Length = 0.18
self.previousR = 0
self.previousL = 0
self.pub_state_pose_ = tf2_ros.TransformBroadcaster()
self.pub_marker = rospy.Publisher('~point_visualizer', Marker, queue_size=1)
# Subscription
self.sub_encoder = rospy.Subscriber("/encoder", Point, self.cbEncoder, queue_size=1)
# safe shutdown
rospy.on_shutdown(self.custom_shutdown)
# timer
rospy.loginfo("[%s] Initialized " %(rospy.get_name()))
def __init__(self):
rospy.init_node('Arduino', log_level=rospy.DEBUG)
self.name = rospy.get_name()
# Cleanup when termniating the node
rospy.on_shutdown(self.shutdown)
self.port = rospy.get_param("~port", "/dev/ttyACM0")
self.baud = int(rospy.get_param("~baud", 57600))
self.timeout = rospy.get_param("~timeout", 0.5)
self.base_frame = rospy.get_param("~base_frame", 'base_link')
# Overall loop rate: should be faster than fastest sensor rate
self.rate = int(rospy.get_param("~rate", 50))
r = rospy.Rate(self.rate)
# Rate at which summary SensorState message is published. Individual sensors publish
# at their own rates.
self.sensorstate_rate = int(rospy.get_param("~sensorstate_rate", 10))
self.use_base_controller = rospy.get_param("~use_base_controller",
False)
# Set up the time for publishing the next SensorState message
now = rospy.Time.now()
self.t_delta_sensors = rospy.Duration(1.0 / self.sensorstate_rate)
self.t_next_sensors = now + self.t_delta_sensors
# Initialize a Twist message
self.cmd_vel = Twist()
# A cmd_vel publisher so we can stop the robot when shutting down
self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=30)
# The SensorState publisher periodically publishes the values of all sensors on
# a single topic.
self.sensorStatePub = rospy.Publisher('~sensor_state', SensorState)
# A service to position a PWM servo
rospy.Service('~servo_write', ServoWrite, self.ServoWriteHandler)
# A service to read the position of a PWM servo
rospy.Service('~servo_read', ServoRead, self.ServoReadHandler)
# A service to turn set the direction of a digital pin (0 = input, 1 = output)
rospy.Service('~digital_set_direction', DigitalSetDirection,
self.DigitalSetDirectionHandler)
# A service to turn a digital sensor on or off
rospy.Service('~digital_write', DigitalWrite, self.DigitalWriteHandler)
# A service to set pwm values for the pins
rospy.Service('~analog_write', AnalogWrite, self.AnalogWriteHandler)
# Initialize the controlller
self.controller = Arduino(self.port, self.baud, self.timeout)
# Make the connection
self.controller.connect()
rospy.loginfo("Connected to Arduino on port " + self.port + " at " +
str(self.baud) + " baud")
# Reserve a thread lock
mutex = thread.allocate_lock()
# Initialize any sensors
self.mySensors = list()
sensor_params = rospy.get_param("~sensors", dict({}))
for name, params in sensor_params.iteritems():
# Set the direction to input if not specified
try:
params['direction']
except:
params['direction'] = 'input'
if params['type'] == "Ping":
sensor = Ping(self.controller, name, params['pin'],
params['rate'], self.base_frame)
elif params['type'] == "GP2D12":
sensor = GP2D12(self.controller, name, params['pin'],
params['rate'], self.base_frame)
elif params['type'] == 'Digital':
sensor = DigitalSensor(self.controller,
name,
params['pin'],
params['rate'],
self.base_frame,
direction=params['direction'])
elif params['type'] == 'Analog':
sensor = AnalogSensor(self.controller,
name,
params['pin'],
params['rate'],
self.base_frame,
direction=params['direction'])
elif params['type'] == 'PololuMotorCurrent':
sensor = PololuMotorCurrent(self.controller, name,
params['pin'], params['rate'],
self.base_frame)
elif params['type'] == 'PhidgetsVoltage':
sensor = PhidgetsVoltage(self.controller, name, params['pin'],
params['rate'], self.base_frame)
elif params['type'] == 'PhidgetsCurrent':
sensor = PhidgetsCurrent(self.controller, name, params['pin'],
params['rate'], self.base_frame)
# if params['type'] == "MaxEZ1":
# self.sensors[len(self.sensors)]['trigger_pin'] = params['trigger_pin']
# self.sensors[len(self.sensors)]['output_pin'] = params['output_pin']
try:
self.mySensors.append(sensor)
rospy.loginfo(name + " " + str(params) +
" published on topic " + rospy.get_name() +
"/sensor/" + name)
except:
rospy.logerr("Sensor type " + str(params['type']) +
" not recognized.")
# Initialize the base controller if used
if self.use_base_controller:
self.myBaseController = BaseController(self.controller,
self.base_frame)
# Start polling the sensors and base controller
while not rospy.is_shutdown():
for sensor in self.mySensors:
mutex.acquire()
sensor.poll()
mutex.release()
if self.use_base_controller:
mutex.acquire()
self.myBaseController.poll()
mutex.release()
# Publish all sensor values on a single topic for convenience
now = rospy.Time.now()
if now > self.t_next_sensors:
msg = SensorState()
msg.header.frame_id = self.base_frame
msg.header.stamp = now
for i in range(len(self.mySensors)):
msg.name.append(self.mySensors[i].name)
msg.value.append(self.mySensors[i].value)
try:
self.sensorStatePub.publish(msg)
except:
pass
self.t_next_sensors = now + self.t_delta_sensors
r.sleep()
def StatesCallback(self, data):
self.real = data
self.realnew = True
def EnvironmentCallback(self, data):
self.temp = data.temperature
if self.realnew:
self.airspeed = np.sqrt(
pow(self.real.velocity.linear.x - data.wind.x, 2) +
pow(self.real.velocity.linear.y - data.wind.y, 2) +
pow(self.real.velocity.linear.z - data.wind.z, 2))
###################################################################################################
####################### Main Program ############################################################
###################################################################################################
if __name__ == '__main__':
rospy.init_node('gyro_model')
fullname = rospy.get_name().split('/')
gyro = Gyroscope(fullname[-1])
while not rospy.is_shutdown():
if gyro.realnew:
gyro.realnew = False
gyro.iterate(gyro.real)
gyro.measurement.header.stamp = rospy.Time.now()
gyro.pub.publish(gyro.measurement)
gyro.rate.sleep()
def set_pose(self, x, y, z=3, BODY_FLU=True):
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
# ROS uses ENU internally, so we will stick to this convention
if BODY_FLU:
pose.header.frame_id = 'base_link'
else:
pose.header.frame_id = 'map'
pose.pose.position.x = x
pose.pose.position.y = y
pose.pose.position.z = z
return pose
if __name__ == "__main__":
con = Commander()
time.sleep(5)
rospy.loginfo(rospy.get_name() + "I heard x=%s y=%s ", xc, yc)
for i in range(len(xc)):
rospy.loginfo(rospy.get_name() + "I heard x=%s y=%s ", xc[i], yc[i])
con.move(xc[i], yc[i], 3, BODY_OFFSET_ENU=False)
# rospy.spin()
rospy.loginfo("commander completed!!!")
def shutdown_sequence(self):
rospy.loginfo(str(rospy.get_name()) + ": Shutting Down")
def __init__(self):
# Allow the simulator to start
time.sleep(4)
# When this node shutsdown
rospy.on_shutdown(self.shutdown_sequence)
# Set the rate
self.rate = 100.0
self.dt = 1.0 / self.rate
# Getting the PID parameters
stable_gains = rospy.get_param(
'/position_controller_node/gains/stable/', {
'p': 1,
'i': 0.0,
'd': 0.0
})
Kp_s, Ki_s, Kd_s = stable_gains['p'], stable_gains['i'], stable_gains[
'd']
# If the speed is set to unstable waypoint
Kp = Kp_s
Ki = Ki_s
Kd = Kd_s
# Display incoming parameters
rospy.loginfo(
str(rospy.get_name()) +
": Launching with the following parameters:")
rospy.loginfo(str(rospy.get_name()) + ": p - " + str(Kp))
rospy.loginfo(str(rospy.get_name()) + ": i - " + str(Ki))
rospy.loginfo(str(rospy.get_name()) + ": d - " + str(Kd))
rospy.loginfo(str(rospy.get_name()) + ": rate - " + str(self.rate))
# Creating the PID's
self.pos_x_PID = PID(Kp, Ki, Kd, self.rate)
self.pos_y_PID = PID(Kp, Ki, Kd, self.rate)
self.pos_z_PID = PID(Kp, Ki, Kd, self.rate)
# Get the setpoints
self.x_setpoint = 0
self.y_setpoint = 0
self.z_setpoint = 2.5
# Create the current output readings
self.x_pos = 0
self.y_pos = 0
self.z_pos = 0
# Create the subscribers and publishers
self.vel_set_sub = rospy.Publisher('/uav/input/velocity',
Vector3,
queue_size=1)
self.gps_sub = rospy.Subscriber("uav/sensors/gps", PoseStamped,
self.get_gps)
self.pos_set_sub = rospy.Subscriber("uav/input/position", Vector3,
self.set_pos)
self.at_goal_pub = rospy.Publisher("uav/sensors/atwaypoint",
Bool,
queue_size=1)
# Run the communication node
self.ControlLoop()
self._feedback.sequence = []
self._feedback.sequence.append(0)
self._feedback.sequence.append(1)
# publish info to the console for the user
rospy.loginfo('%s: Executing, creating fibonacci sequence of order %i with seeds %i, %i' % (self._action_name, goal.order, self._feedback.sequence[0], self._feedback.sequence[1]))
# start executing the action
for i in range(1, goal.order):
# check that preempt has not been requested by the client
if self._as.is_preempt_requested():
rospy.loginfo('%s: Preempted' % self._action_name)
self._as.set_preempted()
success = False
break
self._feedback.sequence.append(self._feedback.sequence[i] + self._feedback.sequence[i-1])
# publish the feedback
self._as.publish_feedback(self._feedback)
# this step is not necessary, the sequence is computed at 1 Hz for demonstration purposes
r.sleep()
if success:
self._result.sequence = self._feedback.sequence
rospy.loginfo('%s: Succeeded' % self._action_name)
self._as.set_succeeded(self._result)
if __name__ == '__main__':
rospy.init_node('fibonacci')
server = FibonacciAction(rospy.get_name())
rospy.spin()
def __init__(self):
'''
Default image_topic for:
Basler ace cameras with camera_aravis driver: camera/image_raw
Pt Grey Firefly cameras with pt grey driver : camera/image_mono
'''
# default parameters (parameter server overides them)
self.params = {
'image_topic': 'camera/image_mono',
'min_persistence_to_draw': 10,
'max_frames_to_draw': 50,
'camera_encoding':
'mono8', # fireflies are bgr8, basler gige cams are mono8
'roi_l': 0,
'roi_r': -1,
'roi_b': 0,
'roi_t': -1,
'~circular_mask_x': None,
'~circular_mask_y': None,
'~circular_mask_r': None,
'~roi_points': None,
'~detect_tracking_pipelines': False, # rename?
'save_demo': False
}
for parameter, default_value in self.params.items():
use_default = False
try:
if parameter[0] == '~':
value = rospy.get_param(parameter)
else:
try:
value = rospy.get_param('multi_tracker/liveviewer/' +
parameter)
except KeyError:
value = rospy.get_param('multi_tracker/tracker/' +
parameter)
# for maintaining backwards compatibility w/ Floris' config files that
# would use 'none' to signal default should be used.
# may break some future use cases.
if self.params[parameter] is None:
if isinstance(value, str):
use_default = True
except KeyError:
use_default = True
if use_default:
rospy.loginfo(rospy.get_name() + ' using default parameter: ' + \
parameter + ' = ' + str(default_value))
value = default_value
if parameter[0] == '~':
del self.params[parameter]
parameter = parameter[1:]
self.params[parameter] = value
# If we are tracking an experiment that is being played back
# ("retracking"), we don't want to further restrict roi, and we will
# always use the same camera topic.
if rospy.get_param('/use_sim_time', False):
self.params['image_topic'] = 'camera/image_raw'
self.params['roi_l'] = 0
self.params['roi_r'] = -1
self.params['roi_b'] = 0
self.params['roi_t'] = -1
self.videowriter = None
if self.params['save_demo']:
# TODO include timestamp?
self.video_filename = 'tracking_demo.avi'
self.desired_frame_rate = 30.0
self.mode = 'color'
rospy.init_node('liveviewer')
self.tracked_trajectories = {}
self.clear_rois()
# should i allow both roi_* and detect_tracking_pipelines?
roi_params = [
'circular_mask_x', 'circular_mask_y', 'circular_mask_r',
'roi_points'
]
if self.params['detect_tracking_pipelines']:
if any(map(lambda x: self.params[x] != None, roi_params)):
rospy.logfatal('liveviewer: roi parameters other than rectangular roi_[l/r/b/t] ' + \
'are not supported when detect_tracking_pipelines is set to True')
# add single rois defined in params to instance variables for consistency later
# roi_* are still handled differently, and can be set alongside tracker roi_*'s
# which are now private (node specific) parameters
else:
n = rospy.get_name()
try:
node_num = int(n.split('_')[-1])
except ValueError:
node_num = 1
circle_param_names = [
'circular_mask_x', 'circular_mask_y', 'circular_mask_r'
]
self.add_roi(node_num, circle_param_names)
poly_param_names = ['roi_points']
# TODO break roi stuff and subscription initiation
# into one function?
self.add_roi(node_num, poly_param_names)
# otherwise, we need to start subscriptions as we
# detect other pipelines in our namespace
self.start_subscribers(node_num)
# TODO put in dict above? (& similar lines in other files)
# displays extra information about trajectory predictions / associations if True
self.debug = rospy.get_param('multi_tracker/liveviewer/debug', False)
# initialize display
self.window_name = 'liveviewer'
self.cvbridge = CvBridge()
if self.params['detect_tracking_pipelines']:
self.contours = dict()
else:
self.contours = None
self.window_initiated = False
self.image_mask = None
# Subscriptions - subscribe to images, and tracked objects
sizeImage = 128 + 1024 * 1024 * 3 # Size of header + data.
self.subImage = rospy.Subscriber(self.params['image_topic'],
Image,
self.image_callback,
queue_size=5,
buff_size=2 * sizeImage,
tcp_nodelay=True)
# for adding images to background
add_image_to_background_service_name = 'multi_tracker/tracker/add_image_to_background'
rospy.wait_for_service(add_image_to_background_service_name)
try:
self.add_image_to_background = rospy.ServiceProxy(
add_image_to_background_service_name,
addImageToBackgroundService)
except:
rospy.logerr(
'could not connect to add image to background service - is tracker running?'
)
def reconfigure_callback(self, config, level):
with self.lock:
for i, mean in enumerate(["mean_x", "mean_y", "mean_z", "mean_roll", "mean_pitch", "mean_yaw"]):
self.v_err_mean[i] = config[mean]
for i, sigma in enumerate(["sigma_x", "sigma_y", "sigma_z", "sigma_roll", "sigma_pitch", "sigma_yaw"]):
self.v_err_sigma[i] = config[sigma]
rospy.loginfo("[%s]" + "velocity mean updated: x: {0}, y: {1}, z: {2}, roll: {3}, pitch: {4}, yaw: {5}".format(*self.v_err_mean), rospy.get_name())
rospy.loginfo("[%s]" + "velocity sigma updated: x: {0}, y: {1}, z: {2}, roll: {3}, pitch: {4}, yaw: {5}".format(*self.v_err_sigma), rospy.get_name())
return config
if task.max_duration.secs == 0:
task.max_duration = task.end_before - task.start_after # Default execution time: window length
if task.priority == 0:
task.priority = 1
return task
def button_pressed_callback(self, active_screen):
# reset timeout
rospy.loginfo('button_pressed_callback')
self.interaction_times.append(rospy.Time.now())
self.pages.append(active_screen.data)
self.reset_time = rospy.Time.now().to_sec() + self.extension_time
if self.server.is_active():
self.interruptible = False
def preempt_cb(self):
clicks = Clicks()
clicks.time_array = self.interaction_times
clicks.page_array = self.pages
self.interaction_times = []
self.pages = []
self.pub.publish(clicks)
if __name__ == "__main__":
rospy.init_node("info_task_server")
i = InfoTaskServer(rospy.get_name())
rospy.spin()
import string
import math
import roslib
roslib.load_manifest('adis_16488')
import rospy
import tf
from sensor_msgs.msg import Imu
from adis_16488.msg import ADIS16488
IMU_FRAME = 'imu_link'
grad2rad = 3.141592654 / 180.0
if __name__ == '__main__':
rospy.init_node('adis_16488')
imu_pub = rospy.Publisher(rospy.get_name() + '/imu_data_raw', Imu)
adis_info_pub = rospy.Publisher(rospy.get_name() + '/adis_16488_info',
ADIS16488)
port = rospy.get_param("~port", "/dev/ttyACM0")
baud = int(rospy.get_param("~baud", "115200"))
ser = serial.Serial(port, baud, timeout=1)
rospy.loginfo('Publishing imu data on topics:\n' + rospy.get_name() +
'/imu_data_raw\n' + '/adis_16488_info')
imuMsg = Imu()
ADISInfo = ADIS16488()
imuMsg.orientation_covariance = [999999, 0, 0, 0, 9999999, 0, 0, 0, 999999]
imuMsg.angular_velocity_covariance = [9999, 0, 0, 0, 99999, 0, 0, 0, 0.02]
imuMsg.linear_acceleration_covariance = [0.2, 0, 0, 0, 0.2, 0, 0, 0, 0.2]
yaw = 0.0
def main():
rospy.init_node('centipede_fsm_node', anonymous=True)
centipede_to_seg_dict = {}
seg_to_centipede_dict = {}
num_segments = None
ros_rate = None
if ( (len(sys.argv) < 6) ):
print("usage: centipede_fsm_node.py centipede_to_segment0 segment0_to_centipede \
centipede_to_segment1 setment1_to_centipede (etc...) num_segments ros_rate")
else:
print sys.stderr, sys.argv
num_segments = int(sys.argv[-4])
print("num_segments = " + str(num_segments))
ros_rate = float(sys.argv[-3])
for i in range(num_segments):
centipede_to_seg_dict['seg' + str(i)] = sys.argv[2*i+1]
seg_to_centipede_dict['seg' + str(i)] = sys.argv[2*i+2]
# Step State instances
# Start - Lift
start_state_dict = {}
for i in range(num_segments):
start_state_dict['seg' + str(i)] = Step(centipede_to_seg_dict['seg' + str(i)], "StartLift",
seg_to_centipede_dict['seg' + str(i)], "Ready", None, ros_rate, 'seg' + str(i))
# Step A - Plant-Push
step_a_state_dict = {}
for i in range(num_segments):
step_a_state_dict['seg' + str(i)] = Step(centipede_to_seg_dict['seg' + str(i)], "Plant",
seg_to_centipede_dict['seg' + str(i)], "Ready", None, ros_rate, 'seg' + str(i))
# Step B - Recover
step_b_state_dict = {}
for i in range(num_segments):
step_b_state_dict['seg' + str(i)] = Step(centipede_to_seg_dict['seg' + str(i)], "Recover",
seg_to_centipede_dict['seg' + str(i)], "Ready", None, ros_rate, 'seg' + str(i))
# Concurrence instances # NOTE: could probably make this more flexible w/ dictionaries but also makes code more opaque :/
# Start - Lift
start_state_1st_con = Concurrence(outcomes=['success', 'failure'], default_outcome='failure',
outcome_map={'success':{'SEG0':'success', 'SEG3':'success'}})
with start_state_1st_con:
for i in range(0, num_segments, 3):
seg_key = 'seg' + str(i)
Concurrence.add(seg_key.upper(), start_state_dict[seg_key])
start_state_2nd_con = Concurrence(outcomes=['success', 'failure'], default_outcome='failure',
outcome_map={'success':{'SEG1':'success', 'SEG4':'success'}})
with start_state_2nd_con:
for i in range(1, num_segments, 3):
seg_key = 'seg' + str(i)
Concurrence.add(seg_key.upper(), start_state_dict[seg_key])
start_state_3rd_con = Concurrence(outcomes=['success', 'failure'], default_outcome='failure',
outcome_map={'success':{'SEG2':'success', 'SEG5':'success'}})
with start_state_3rd_con:
for i in range(2, num_segments, 3):
seg_key = 'seg' + str(i)
Concurrence.add(seg_key.upper(), start_state_dict[seg_key])
# Step A - Plant-Push
step_a_state_1st_con = Concurrence(outcomes=['success', 'failure'], default_outcome='failure',
outcome_map={'success':{'SEG0':'success', 'SEG3':'success'}})
with step_a_state_1st_con:
for i in range(0, num_segments, 3):
seg_key = 'seg' + str(i)
Concurrence.add(seg_key.upper(), step_a_state_dict[seg_key])
step_a_state_2nd_con = Concurrence(outcomes=['success', 'failure'], default_outcome='failure',
outcome_map={'success':{'SEG1':'success', 'SEG4':'success'}})
with step_a_state_2nd_con:
for i in range(1, num_segments, 3):
seg_key = 'seg' + str(i)
Concurrence.add(seg_key.upper(), step_a_state_dict[seg_key])
step_a_state_3rd_con = Concurrence(outcomes=['success', 'failure'], default_outcome='failure',
outcome_map={'success':{'SEG2':'success', 'SEG5':'success'}})
with step_a_state_3rd_con:
for i in range(2, num_segments, 3):
seg_key = 'seg' + str(i)
Concurrence.add(seg_key.upper(), step_a_state_dict[seg_key])
# Step B - Recover
step_b_state_1st_con = Concurrence(outcomes=['success', 'failure'], default_outcome='failure',
outcome_map={'success':{'SEG0':'success', 'SEG3':'success'}})
with step_b_state_1st_con:
for i in range(0, num_segments, 3):
seg_key = 'seg' + str(i)
Concurrence.add(seg_key.upper(), step_b_state_dict[seg_key])
step_b_state_2nd_con = Concurrence(outcomes=['success', 'failure'], default_outcome='failure',
outcome_map={'success':{'SEG1':'success', 'SEG4':'success'}})
with step_b_state_2nd_con:
for i in range(1, num_segments, 3):
seg_key = 'seg' + str(i)
Concurrence.add(seg_key.upper(), step_b_state_dict[seg_key])
step_b_state_3rd_con = Concurrence(outcomes=['success', 'failure'], default_outcome='failure',
outcome_map={'success':{'SEG2':'success', 'SEG5':'success'}})
with step_b_state_3rd_con:
for i in range(2, num_segments, 3):
seg_key = 'seg' + str(i)
Concurrence.add(seg_key.upper(), step_b_state_dict[seg_key])
# Create a SMACH state machine
centipede_fsm_node = StateMachine(outcomes=['success'])
# Open the SMACH state machine
with centipede_fsm_node:
# Add these instances to the top-level StateMachine
StateMachine.add('START_1ST', start_state_1st_con, transitions={'failure':'START_1ST', 'success': 'START_2ND'})
StateMachine.add('START_2ND', start_state_2nd_con, transitions={'failure':'START_2ND', 'success': 'START_3RD'})
StateMachine.add('START_3RD', start_state_3rd_con, transitions={'failure':'START_3RD', 'success': 'STEP_B_1ST'})
# Main loop
StateMachine.add('STEP_B_1ST', step_b_state_1st_con, transitions={'failure':'STEP_B_1ST', 'success': 'STEP_A_1ST'})
StateMachine.add('STEP_A_1ST', step_a_state_1st_con, transitions={'failure':'STEP_A_1ST', 'success': 'STEP_B_2ND'})
StateMachine.add('STEP_B_2ND', step_b_state_2nd_con, transitions={'failure':'STEP_B_2ND', 'success': 'STEP_A_2ND'})
StateMachine.add('STEP_A_2ND', step_a_state_2nd_con, transitions={'failure':'STEP_A_2ND', 'success': 'STEP_B_3RD'})
StateMachine.add('STEP_B_3RD', step_b_state_3rd_con, transitions={'failure':'STEP_B_3RD', 'success': 'STEP_A_3RD'})
StateMachine.add('STEP_A_3RD', step_a_state_3rd_con, transitions={'failure':'STEP_A_3RD', 'success': 'STEP_B_1ST'})
# Create and start the introspection server - for visualization / debugging
sis = smach_ros.IntrospectionServer('centipede_sym_staggered_triplets_fsm_node' + str(rospy.get_name()), centipede_fsm_node, '/SM_ROOT') #+ str(rospy.get_name()))
sis.start()
# Give Gazebo some time to start up
user_input = raw_input("Please press the 'Return/Enter' key to start executing - type: centipede_sym_staggered_triplets_fsm_node.py | node: " + str(rospy.get_name()) + "\n")
# Execute SMACH state machine
print("Input received. Executing - type: centipede_sym_staggered_triplets_fsm_node.py | node: " + str(rospy.get_name()) + "...\n")
outcome = centipede_fsm_node.execute()
# spin() simply keeps python from exiting until this node is stopped
rospy.spin()
rospy.init_node("interactive_static_transform_publisher")
br = tf.TransformBroadcaster()
tf_position = Point(0.0, 0.0, 0.0)
tf_orientation = Quaternion(0.0, 0.0, 0.0, 1.0)
marker_position = Point(tf_position.x + marker_x_offset, tf_position.y, tf_position.z)
if rospy.has_param('~child_frame'):
tf_child_frame = rospy.get_param('~child_frame')
if rospy.has_param('~parent_frame'):
tf_parent_frame = rospy.get_param('~parent_frame')
if rospy.has_param('~tf_publish_period_in_sec'):
tf_update_period = rospy.get_param('~tf_publish_period_in_sec')
server = InteractiveMarkerServer(rospy.get_name()+"/transform_control")
menu_handler.insert( "Generate the static transform command", callback=processFeedback )
make6DofMarker( True, InteractiveMarkerControl.MENU, marker_position, True)
server.applyChanges()
# create a timer to update the published transforms
rospy.Timer(rospy.Duration(tf_update_period), frameCallback)
rospy.spin()
def feedforward(self, obs_traj, obs_traj_rel, seq_start_end):
"""
obs_traj: torch.Tensor([4, num_agents, 2])
obs_traj_rel: torch.Tensor([4, num_agents, 2])
seq_start_end: torch.Tensor([batch_size, 2]) #robot+#pedstrains
goals_rel: torch.Tensor([1, num_agents, 2])
"""
with torch.no_grad():
pred_traj_fake_rel = self.generator(obs_traj, obs_traj_rel,
seq_start_end)
pred_traj_fake = relative_to_abs(pred_traj_fake_rel, obs_traj[0])
return pred_traj_fake
if __name__ == '__main__':
try:
rospy.get_master().getPid()
except:
print("roscore is offline, exit")
sys.exit(-1)
CHECKPOINT = '/home/asus/SocialNavigation/src/sgan/models/sgan-models/eth_12_model.pt'
rospy.init_node('sgan_local_planner')
model = SGan(torch.load(CHECKPOINT))
try:
rospy.spin()
except rospy.ROSInterruptException:
rospy.loginfo('[{}] Shutting down...'.format(rospy.get_name()))
def grabAudioLevel(self):
monitor = PeakMonitor(self.sink_name, self.meter_rate)
for sample in monitor:
if not self._as.is_active():
rospy.logdebug("No active goal")
continue
lights = float(sample) / float(
self.max_sample_value
) * 20 #Magic number. Lets the light look good while speaking.
self.switchLEDs(lights)
self._feedback.output_level = sample
self._feedback.remaining_runtime = self.end_time - rospy.get_time(
) if self.end_time > 0 else float('Inf')
self._as.publish_feedback(self._feedback)
if rospy.get_time() > self.end_time and self.end_time > 0:
rospy.loginfo(
"Execution time has been reached. Goal terminated successfully"
)
self.spinningLEDs()
self._result.expired = True
self._as.set_succeeded(self._result)
if __name__ == '__main__':
rospy.init_node("sound_to_light")
sl = SoundLights(rospy.get_name())
thread.start_new_thread(sl.grabAudioLevel, ())
#sl.grabAudioLevel()
rospy.spin()
def __init__(self, xyz_map, frame_id, temp_lowpass, contact_threshold):
topic = 'takktile'
# Set up node & topics
rospy.init_node(topic, anonymous=True)
rospy.loginfo(rospy.get_name() + " node initialized")
# fast topics
raw_pub = rospy.Publisher(topic + '/raw', Raw)
calibrated_pub = rospy.Publisher(topic + '/calibrated', Touch)
contact_pub = rospy.Publisher(topic + '/contact', Contact)
# slow topic
info_pub = rospy.Publisher(topic + '/sensor_info', Info)
# initialize connection to TakkTile
tk = TakkTile()
# get static values once
self.alive = tk.alive
num_alive = len(tk.alive)
self.calibration = np.zeros(len(
tk.alive)) # start with zero-order calibration
# start rospy service for zeroing sensors
rospy.Service(topic + '/zero', Empty, self.zero_callback)
# publish sensor data at 60 Hz
r = rospy.Rate(60)
tk.startSampling()
# initialize temperature lowpass with actual data
data = tk.getDataRaw()
# print 'self.getDataRaw():', data
# unpack the values
# first - extract the values from the dictionary
# second - unzip
[self.pressure, self.temp] = zip(*data.values())
self.pressure = np.array(self.pressure)
self.temp = np.array(self.temp)
self.calibration = -np.array(self.pressure) # zero values at startup
i = 0
#k = 0
while not rospy.is_shutdown():
i += 1
if i >= 100: # downsample static info
info_pub.publish(frame_id, xyz_map, self.alive)
i = 0
#k += 100
#print k
# publish sensor vals at 100 Hz
calibrated = [0.0] * num_alive
contact = [False] * num_alive
data = tk.getDataRaw()
# unpack the values
# first - extract the values from the dictionary
# second - unzip
# print "type(data.values()) ->", type(data.values())
# dataValues=data.values()
# print "zip(*dataValues) ->", zip(*dataValues)
[self.pressure, temp_new] = zip(*data.values())
#print self.pressure
# lowpass filter temperature
self.temp = TEMPERATURE_LOWPASS * np.array(temp_new) + (
1 - TEMPERATURE_LOWPASS) * self.temp
raw_pub.publish(self.pressure, self.temp)
calibrated = np.array(self.pressure) + self.calibration
for j in range(len(self.alive)):
contact[j] = abs(calibrated[j]) > contact_threshold
calibrated_pub.publish(calibrated)
contact_pub.publish(contact)
# print "published Pressure ->", self.pressure
r.sleep()
# switch things off
print "switching off"
tk.stopSampling()
cmd_vel_publisher1.publish(twist)
else:
global flag_send
twist = Twist()
gtg_flag = Int32()
gtg_flag = 1
twist.linear.x = 0
twist.linear.y = 0
twist.linear.z = 0
twist.angular.x = 0
twist.angular.y = 0
twist.angular.z = 0
#print("i",i)
cmd_vel_publisher1.publish(twist)
if (flag_send == 0):
cmd_vel_publisher2.publish(gtg_flag)
flag_send = 1
rospy.loginfo('Reached Final Goal. GTG Flag raised')
""
if __name__ == '__main__':
try:
gtg_talker()
except rospy.ROSInterruptException:
pass
finally:
rospy.loginfo("%s closed" % rospy.get_name())
self._condition.acquire()
if self._last_rubble is None:
result.rubble_size = 'large'
else:
if 'small' in self._last_rubble:
result.rubble_size = 'small'
elif 'medium' in self._last_rubble:
result.rubble_size = 'medium'
else:
result.rubble_size = 'large'
self._condition.release()
self._as.set_succeeded(result)
def qr_cb(self, msg):
""" Callback for qr topic.
Args:
msg: A string with decoded qr code
"""
self._condition.acquire()
if 'rubble' in msg.data:
self._last_rubble = msg.data
self._condition.release()
if __name__ == '__main__':
rospy.init_node('rubble_check')
server = RubbleCheckServer(rospy.get_name())
rospy.spin()
def __init__(self, rpcport=11611, do_retry=True, ipv6=False):
'''
Initialize method. Creates an XML-RPC server on given port and starts this
in its own thread.
:param rpcport: the port number for the XML-RPC server
:type rpcport: int
:param do_retry: retry to create XML-RPC server
:type do_retry: bool
:param ipv6: Use ipv6
:type ipv6: bool
'''
self._state_access_lock = threading.RLock()
self._create_access_lock = threading.RLock()
self._lock = threading.RLock()
self.__masteruri = masteruri_from_ros()
self.__new_master_state = None
self.__masteruri_rpc = None
self.__mastername = None
self.__cached_nodes = dict()
self.__cached_services = dict()
self.ros_node_name = str(rospy.get_name())
if rospy.has_param('~name'):
self.__mastername = rospy.get_param('~name')
self.__mastername = self.getMastername()
rospy.set_param('/mastername', self.__mastername)
self.__master_state = None
'''the current state of the ROS master'''
self.rpcport = rpcport
'''the port number of the RPC server'''
self._printed_errors = dict()
self._last_clearup_ts = time.time()
self._master_errors = list()
# Create an XML-RPC server
self.ready = False
while not self.ready and (not rospy.is_shutdown()):
try:
RPCClass = RPCThreading
if ipv6:
RPCClass = RPCThreadingV6
self.rpcServer = RPCClass(('', rpcport),
logRequests=False,
allow_none=True)
rospy.loginfo("Start RPC-XML Server at %s",
self.rpcServer.server_address)
self.rpcServer.register_introspection_functions()
self.rpcServer.register_function(self.getListedMasterInfo,
'masterInfo')
self.rpcServer.register_function(
self.getListedMasterInfoFiltered, 'masterInfoFiltered')
self.rpcServer.register_function(self.getMasterContacts,
'masterContacts')
self.rpcServer.register_function(self.getMasterErrors,
'masterErrors')
self.rpcServer.register_function(self.getCurrentTime,
'getCurrentTime')
self.rpcServer.register_function(self.setTime, 'setTime')
self.rpcServer.register_function(self.getTopicsMd5sum,
'getTopicsMd5sum')
self._rpcThread = threading.Thread(
target=self.rpcServer.serve_forever)
self._rpcThread.setDaemon(True)
self._rpcThread.start()
self.ready = True
except socket.error as e:
if not do_retry:
raise Exception(
"Error while start RPC-XML server on port %d: %s\nIs a Node Manager already running?"
% (rpcport, e))
rospy.logwarn(
"Error while start RPC-XML server on port %d: %s\nTry again..."
% (rpcport, e))
time.sleep(1)
except:
print traceback.format_exc()
if not do_retry:
raise
self._master = xmlrpclib.ServerProxy(self.getMasteruri())
# === UPDATE THE LAUNCH URIS Section ===
# subscribe to get parameter updates
rospy.loginfo("Subscribe to parameter `/roslaunch/uris`")
self.__mycache_param_server = rospy.impl.paramserver.get_param_server_cache(
)
# HACK: use own method to get the updates also for parameters in the subgroup
self.__mycache_param_server.update = self.__update_param
# first access, make call to parameter server
self._update_launch_uris_lock = threading.RLock()
self.__launch_uris = {}
code, msg, value = self._master.subscribeParam(self.ros_node_name,
rospy.get_node_uri(),
'/roslaunch/uris')
# the new timer will be created in self._update_launch_uris()
self._timer_update_launch_uris = None
if code == 1:
for k, v in value.items():
self.__launch_uris[roslib.names.ns_join('/roslaunch/uris',
k)] = v
self._update_launch_uris()
msg.pose.pose.orientation.z = feedback.pose.orientation.z
msg.pose.pose.orientation.w = feedback.pose.orientation.w
self.init_pose_client.setPose(msg)
rospy.loginfo('setPoseCB: setting pose')
return
## @brief Stops the current route if it's started
def stop(self, feedback):
self.planner_client.cancel()
return
if __name__=="__main__":
rospy.init_node("poi_markers")
_name = rospy.get_name().replace('/','')
arg_defaults = {
'frame_id': 'map',
'goto_planner': 'move_base',
'init_pose_topic_name': 'initialpose',
'has_safety_laser': False,
'safety_laser_client_name': 'safety_module/set_laser_mode'
}
args = {}
for name in arg_defaults:
try:
def zero_callback(self, msg):
# global cc and current_values
self.calibration = -np.array(self.pressure)
rospy.loginfo(rospy.get_name() + ' zeroed sensors')
return []
def __init__(self, xyz_map, frame_id, temp_lowpass, contact_threshold):
self.tks = None
topic = 'takktile'
# Set up node & topics
rospy.init_node(topic, anonymous=True)
rospy.loginfo(rospy.get_name()+" node initialized")
# fast topics
raw_pub = rospy.Publisher(topic + '/raw', Raw, queue_size=1)
calibrated_pub = rospy.Publisher(topic + '/calibrated', Touch, queue_size=1)
contact_pub = rospy.Publisher(topic + '/contact', Contact, queue_size=1)
# slow topicg
info_pub = rospy.Publisher(topic + '/sensor_info', Info, queue_size=1)
# initialize connection to TakkTile
tks = []
tks.append(TakkTile())
print "Number of boards found = ", len(tks[0].UIDs)
print "UIDs ", tks[0].UIDs
# check if there are multiple interfaces that are connected
if len(tks[0].UIDs)>1:
for i in range(1, len(tks[0].UIDs)):
tks.append(TakkTile(i)) # start a new instance for each board
# get static map of populated live sensors
self.alive=[]
for tk in tks:
self.alive.append(tk.getAlive())
num_alive = len(self.alive)
self.calibration = np.zeros(num_alive) # start with zero-order calibration
# start rospy service for zeroing sensors
rospy.Service(topic + '/zero', Empty, self.zero_callback)
# publish sensor data at 60 Hz
r = rospy.Rate(60)
self.tks = tks
rospy.on_shutdown(self.shutdown)
for tk in tks:
tk.startSampling()
# initialize temperature lowpass with actual data
data = {}
for tk in tks:
data.update(tk.getDataRaw()) # read data from all the connected boards
# unpack the values
# first - extract the values from the dictionary
# second - unzip
[self.pressure, self.temp] = zip(*data.values())
self.pressure = np.array(self.pressure)
self.temp = np.array(self.temp)
self.calibration = -np.array(self.pressure) # zero values at startup
i = 0
#k = 0
while not rospy.is_shutdown():
i += 1
if i >= 100: # downsample static info
info_pub.publish(frame_id, xyz_map, self.alive)
i = 0
#k += 100
#print k
# publish sensor vals at 100 Hz
calibrated = [0.0] * num_alive
data = {}
for tk in tks:
data.update(tk.getDataRaw()) # read data from all the connected boards
# unpack the values
# first - extract the values from the dictionary
# second - unzip
# print "type(data.values()) ->", type(data.values())
# dataValues=data.values()
# print "zip(*dataValues) ->", zip(*dataValues)
[self.pressure, temp_new] = zip(*data.values())
#print self.pressure
# lowpass filter temperature
self.temp = TEMPERATURE_LOWPASS * np.array(temp_new) + (1 - TEMPERATURE_LOWPASS) * self.temp
# check if this is TakkArray, if yes, then remap the data
if (TAKKARRAY_FLAG):
self.pressure= [self.pressure[i] for i in TAKKARRAY_MAPPING]
raw_pub.publish(self.pressure, self.temp)
calibrated = np.array(self.pressure) + self.calibration
contact = [abs(cal) > contact_threshold for cal in calibrated]
calibrated_pub.publish(calibrated)
contact_pub.publish(contact)
# print "published Pressure ->", self.pressure
r.sleep()
def publish_feedback(self, msg):
rospy.loginfo("[%s] %s" % (rospy.get_name(), msg))
self.feedback_pub.publish(msg)
return count % 2 == 1
if __name__ == '__main__':
rospy.init_node("move_base")
AREA_PARAMETER = 'move_base/legal_area'
BOUND_TOLERANCE_PARAMETER = 'move_base/bounds_tolerance_scale'
bounds_tolerance_scale = 1.05
try:
bounds_tolerance_scale = rospy.get_param(BOUND_TOLERANCE_PARAMETER)
except KeyError: # Parameters were not defined.
rospy.loginfo(BOUND_TOLERANCE_PARAMETER +
" was not defined. Assuming " +
str(bounds_tolerance_scale))
if bounds_tolerance_scale < 1.0:
rospy.logwarn(
BOUND_TOLERANCE_PARAMETER +
" should be >= 1.0 to ensure hard bounds outside the defined polygon. Assume 1.0"
)
bounds_tolerance_scale = 1.0
try:
bounds = rospy.get_param(AREA_PARAMETER)
server = MoveAction(rospy.get_name(), bounds, bounds_tolerance_scale)
rospy.spin()
except KeyError: # Parameters were not defined.
rospy.logerr(
"No legal area was defined. Please define a convex, obstacle-free area in the "
+ AREA_PARAMETER + " parameter.")
def callback(data):
rospy.loginfo(rospy.get_name() + "I heard %s", data.name[0])
def _instantiate_monitors(self):
if self.is_instantiated: return True
try:
msg_class, real_topic, _ = rostopic.get_topic_class(
self.topic_name, blocking=False)
topic_type, _, _ = rostopic.get_topic_type(self.topic_name,
blocking=False)
except rostopic.ROSTopicException as e:
self.event_callback(
"Topic %s type cannot be determined, or ROS master cannot be contacted"
% self.topic_name, "warn")
return False
if real_topic is None:
self.event_callback("Topic %s is not published" % self.topic_name,
"warn")
if self.signal_when_cfg["signal_when"].lower(
) == 'not published' and self.signal_when_cfg["safety_critical"]:
self.signal_when_is_safe = False
return False
# if rate > 0 set in config then throttle topic at that rate
if self.rate > 0:
COMMAND_BASE = ["rosrun", "topic_tools", "throttle"]
subscribed_topic = "/sentor/monitoring/" + str(
self.thread_num) + real_topic
command = COMMAND_BASE + [
"messages", real_topic,
str(self.rate), subscribed_topic
]
subprocess.Popen(command, stdout=open(os.devnull, "wb"))
else:
subscribed_topic = real_topic
# find out topic publishing nodes
master = rosgraph.Master(rospy.get_name())
try:
pubs, _ = rostopic.get_topic_list(master=master)
# filter based on topic
pubs = [x for x in pubs if x[0] == real_topic]
nodes = []
for _, _, _nodes in pubs:
nodes += _nodes
self.nodes = nodes
except socket.error:
self.event_callback(
"Could not retrieve nodes for topic %s" % self.topic_name,
"warn")
# Do we need a hz monitor?
hz_monitor_required = False
if self.signal_when_cfg["signal_when"].lower() == 'not published':
hz_monitor_required = True
for signal_lambda in self.signal_lambdas_config:
if "when_published" in signal_lambda:
if signal_lambda["when_published"]:
hz_monitor_required = True
if hz_monitor_required:
self.hz_monitor = self._instantiate_hz_monitor(
subscribed_topic, self.topic_name, msg_class)
if self.signal_when_cfg["signal_when"].lower() == 'published':
print("Signaling 'published' for " + bcolors.OKBLUE +
self.topic_name + bcolors.ENDC + " initialized")
self.pub_monitor = self._instantiate_pub_monitor(
subscribed_topic, self.topic_name, msg_class)
self.pub_monitor.register_published_cb(self.published_cb)
if self.signal_when_cfg["safety_critical"]:
self.signal_when_is_safe = False
elif self.signal_when_cfg["signal_when"].lower() == 'not published':
print("Signaling 'not published' for " + bcolors.BOLD +
str(self.signal_when_cfg["timeout"]) + " seconds" +
bcolors.ENDC + " for " + bcolors.OKBLUE + self.topic_name +
bcolors.ENDC + " initialized")
if len(self.signal_lambdas_config):
print("Signaling expressions for " + bcolors.OKBLUE +
self.topic_name + bcolors.ENDC + ":")
self.lambda_monitor_list = []
for signal_lambda in self.signal_lambdas_config:
lambda_fn_str = signal_lambda["expression"]
lambda_config = self.process_lambda_config(signal_lambda)
if lambda_fn_str != "":
print("\t" + bcolors.OKGREEN + lambda_fn_str +
bcolors.ENDC + " (" + bcolors.BOLD +
"timeout: %s seconds" % lambda_config["timeout"] +
bcolors.ENDC + ")")
lambda_monitor = self._instantiate_lambda_monitor(
subscribed_topic, msg_class, lambda_fn_str,
lambda_config)
# register cb that notifies when the lambda function is True
lambda_monitor.register_satisfied_cb(
self.lambda_satisfied_cb)
lambda_monitor.register_unsatisfied_cb(
self.lambda_unsatisfied_cb)
self.lambda_monitor_list.append(lambda_monitor)
print("")
self.is_instantiated = True
return True
def __init__(self):
self.gui_init()
self.rospy_init()
rospy.loginfo(rospy.get_name() + ' -- Initialization complete')
rospy.loginfo("Command list aborted (preempted).")
last_finished_id = None
if last_msg: self._result.result = last_msg
self._result.result.result_code = Result.FAILURE_EXECUTION
self._as.set_preempted(self._result)
return
elif last_msg and last_msg.result_code != Result.SUCCESS:
rospy.loginfo(
"Command list aborted (error executing trajectory).")
last_finished_id = None
if last_msg: self._result.result = last_msg
self._result.result.result_code = last_msg.result_code
self._as.set_aborted(self._result)
return
elif goal_id == last_finished_id:
rospy.logdebug("Last trajectory command reached.")
if last_msg: self._result.result = last_msg
self._as.set_succeeded(self._result)
return
rospy.sleep(0.05)
if __name__ == '__main__':
rospy.init_node('commands_action_server')
rospy.Subscriber('/command_result', Result, result_callback)
actionizerInstance = Actionizer(rospy.get_name())
rospy.loginfo("Action server started with name '/commands_action_server'")
rospy.spin()
self.heading_offset = rospy.get_param('~heading_offsets', 5.)
self.goal_tolerance = rospy.get_param('~goal_tolerance', 5.)
self.base_frame = rospy.get_param('~base_frame',
"lolo_auv_1/base_link")
self.nav_goal = None
self.listener = tf.TransformListener()
rospy.Timer(rospy.Duration(2), self.timer_callback)
self.rpm_pub = rospy.Publisher('/uavcan_rpm_command',
ThrusterRPMs,
queue_size=10)
self._as = actionlib.SimpleActionServer(self._action_name,
MoveBaseAction,
execute_cb=self.execute_cb,
auto_start=False)
self._as.start()
rospy.loginfo("Announced action server with name: %s",
self._action_name)
r = rospy.Rate(10) # 10hz
rospy.spin()
if __name__ == '__main__':
rospy.init_node('p2p_planner')
planner = P2PPlanner(rospy.get_name())
def _log(self, msg):
print(str(rospy.get_name()) + ": " + str(msg))
def __init__(self, args):
self.node_name = rospy.get_name() #.replace('/','')
self.desired_freq = args['desired_freq']
self.desired_bias = args['desired_bias']
self.desired_stoppedtime = args['desired_stoppedtime']
self.desired_gyrotime = args['desired_gyrotime']
self.desired_standbytime = args['desired_standbytime']
self.desired_temperaturevariation = args[
'desired_temperaturevariation']
self.calibrationEnds = True
self.temperature = 0.0
self.bias = [0, 0]
rospack = rospkg.RosPack()
self.path = rospack.get_path('imu_calibrator')
# Checks value of freq
if self.desired_freq <= 0.0 or self.desired_freq > MAX_FREQ:
rospy.loginfo(
'%s::init: Desired freq (%f) is not possible. Setting desired_freq to %f'
% (self.node_name, self.desired_freq, DEFAULT_FREQ))
self.desired_freq = DEFAULT_FREQ
if self.desired_bias <= 0.0:
rospy.loginfo(
'%s::init: Desired bias (%f) is not possible. Setting desired_bias to %f'
% (self.node_name, self.desired_bias, DEFAULT_BIAS))
self.desired_bias = DEFAULT_BIAS
if self.desired_stoppedtime <= 0.0:
rospy.loginfo(
'%s::init: Desired stopped time (%f) is not possible. Setting desired_stoppedtime to %f'
% (self.node_name, self.desired_stoppedtime,
DEFAULT_STOPPEDTIME))
self.desired_stoppedtime = DEFAULT_STOPPEDTIME
if self.desired_gyrotime <= 0.0:
rospy.loginfo(
'%s::init: Desired gyro time (%f) is not possible. Setting desired_gyrotime to %f'
% (self.node_name, self.desired_gyrotime, DEFAULT_GYROTIME))
self.desired_gyrotime = DEFAULT_GYROTIME
if self.desired_standbytime <= 0.0:
rospy.loginfo(
'%s::init: Desired standby time (%f) is not possible. Setting desired_standbytime to %f'
% (self.node_name, self.desired_standbytime,
DEFAULT_STANDBYTIME))
self.desired_standbytime = DEFAULT_STANDBYTIME
if self.desired_temperaturevariation <= 0.0:
rospy.loginfo(
'%s::init: Desired temperature variation (%f) is not possible. Setting desired_temperaturevariation to %f'
% (self.node_name, self.desired_temperaturevariation,
DEFAULT_TEMPERATUREVARIATION))
self.desired_temperaturevariation = DEFAULT_TEMPERATUREVARIATION
self.status = CalibratorStatus()
self.status.state = "idle"
self.status.remaining_time = self.desired_stoppedtime
self.real_freq = 0.0
# Saves the state of the component
self.state = State.INIT_STATE
# Saves the previous state
self.previous_state = State.INIT_STATE
# flag to control the initialization of the component
self.initialized = False
# flag to control the initialization of ROS stuff
self.ros_initialized = False
# flag to control that the control loop is running
self.running = False
# Variable used to control the loop frequency
self.time_sleep = 1.0 / self.desired_freq
# State msg to publish
self.msg_state = State()
# Timer to publish state
self.publish_state_timer = 1
# The robot is changing his position
self.mov = True
# Position of the robot (linear.x, angular.z)
self.pos = [0, 0, 0, 0, 0, 0]
# The robot needs calibration
self.calibrate = False
# Last inclination of the robot
self.incMin = [100, 100, 100]
self.incMax = [-100, -100, -100]
# True if the robot has been stopped the required time
self.checkedMovement = False
# Time when the search has started
self.t1 = 0.0
