def __init__(self):
Thread.__init__(self)
self.should_run = True
rospy.logout('servo_node: Initializing')
rospy.init_node('servo_node')
# Unfold "ears" to servo positions
p_left = rr.ROS_Robotis_Client('left_pan')
t_left = rr.ROS_Robotis_Client('left_tilt')
p_left.move_angle(math.radians(-40), math.radians(10), blocking=False)
t_left.move_angle(0.0, math.radians(10), blocking=False)
p_right = rr.ROS_Robotis_Client('right_pan')
t_right = rr.ROS_Robotis_Client('right_tilt')
p_right.move_angle(math.radians(40), math.radians(10), blocking=False)
t_right.move_angle(0.0, math.radians(10), blocking=False)
self.reader = rmc.ROS_M5e_Client('ears')
self.reader.track_mode('person ')
# Use assisted teleop as pseudo-callback to see if valid movement:
self.check_pub = rospy.Publisher('rfid_cmd_vel_check', Twist)
self.sub = rospy.Subscriber("assisted_teleop_response", Twist,
self.callback)
self.command_pub = rospy.Publisher("rfid_cmd_vel", Twist)
self.moving = True
time.sleep(3.0) # give antennas time to settle
self.start()
def __init__( self ):
rospy.logout('servo_node: Initializing')
try:
rospy.init_node('servo_node')
except: # Node probably already initialized elsewhere
pass
# Create servos.
self.p_left = rr.ROS_Robotis_Client( 'left_pan' )
self.t_left = rr.ROS_Robotis_Client( 'left_tilt' )
self.p_right = rr.ROS_Robotis_Client( 'right_pan' )
self.t_right = rr.ROS_Robotis_Client( 'right_tilt' )
# Create Costmap Services obj
self.cs = costmap.CostmapServices( accum = 3 )
# Note: After moving, this will require accum * -1's before stopping.
# Publish move_base command
self._pub = rospy.Publisher( 'rfid_cmd_vel', Twist )
# Alterative ways to call servoing using ROS services / actionlib
self._service_servo = rospy.Service( '/rfid_servo/servo', ServoSrv, self.service_request )
self._as = actionlib.SimpleActionServer( '/rfid_servo/servo_act',
ServoAction, execute_cb = self.action_request )
self._as.start()
rospy.logout( 'servo_node: Service ready and waiting' )
def __init__(self):
rospy.logout('servo_node: Initializing')
try:
rospy.init_node('servo_node')
except:
pass
# Unfold "ears" to servo positions
self.p_left = rr.ROS_Robotis_Client('left_pan')
self.t_left = rr.ROS_Robotis_Client('left_tilt')
self.p_right = rr.ROS_Robotis_Client('right_pan')
self.t_right = rr.ROS_Robotis_Client('right_tilt')
# Use assisted teleop as pseudo-callback to see if valid movement:
self.scores = deque()
self.og_pub = rospy.Publisher('assisted_teleop_check', Twist)
self.og_sub = rospy.Subscriber('assisted_teleop_score', Float64,
self.og_score_cb)
self.command_pub = rospy.Publisher('rfid_cmd_vel', Twist)
self._service_servo = rospy.Service('/rfid_servo/servo',
StringInt32_Int32, self.run)
self._service_stop = rospy.Service('/rfid_servo/stop', Empty,
self.servo_stop)
self._service_stop = rospy.Service('/rfid_servo/abort', Empty,
self.servo_abort)
self._service_stop_next_obs = rospy.Service(
'/rfid_servo/stop_next_obs', Int32_Int32, self.stop_request)
self.stop_next_obs = False # run forever
self.should_run = True
self.abort = False
rospy.logout('servo_node: Service ready and waiting')
def __init__(self):
rospy.logout('orient_node: Initializing')
rospy.init_node('orient_node')
# After calling "flap ears", data will look something like this:
# { 'TagID1': [[ang,rssi], [ang,rssi], ...]
# 'TagID2': ... }
# * All angles are in /base_link and rssi's from both antennas
self.data = {}
# Will be transformed into base frame to determine best turn angle -- results in approximately 5-degrees (max) error for small angle assumption
self.tag_gt = {
'EleLeftEar': PointStamped(),
'EleRightEar': PointStamped()
}
self.tag_gt['EleLeftEar'].header.frame_id = '/ear_antenna_left'
self.tag_gt['EleLeftEar'].header.stamp = rospy.Time.now()
self.tag_gt['EleLeftEar'].point.x = 10.0
self.tag_gt['EleRightEar'].header.frame_id = '/ear_antenna_right'
self.tag_gt['EleRightEar'].header.stamp = rospy.Time.now()
self.tag_gt['EleRightEar'].point.x = 10.0
self.listener = tf.TransformListener()
self.listener.waitForTransform('/base_link',
'/ear_antenna_left',
rospy.Time(0),
timeout=rospy.Duration(100))
self.listener.waitForTransform('/base_link',
'/ear_antenna_right',
rospy.Time(0),
timeout=rospy.Duration(100))
rospy.logout('orient_node: Transforms ready')
# For movement...
self.rotate_backup_client = rb.RotateBackupClient()
# "Ears" Setup
self.p_left = rr.ROS_Robotis_Client('left_pan')
self.t_left = rr.ROS_Robotis_Client('left_tilt')
self.p_right = rr.ROS_Robotis_Client('right_pan')
self.t_right = rr.ROS_Robotis_Client('right_tilt')
self.EX_1 = 1.350
self.EX_2 = 0.920
self.p_left.move_angle(self.EX_1, math.radians(10), blocking=False)
self.p_right.move_angle(-1.0 * self.EX_1,
math.radians(10),
blocking=True)
self.t_left.move_angle(0.0, math.radians(10), blocking=False)
self.t_right.move_angle(0.0, math.radians(10), blocking=True)
while self.p_left.is_moving() or self.p_right.is_moving():
time.sleep(0.01)
self.bag_pid = None
self.r = rmc.ROS_M5e_Client('ears')
self.__service_flap = rospy.Service('/rfid_orient/flap', FlapEarsSrv,
self.flap_ears)
self.__service_bag = rospy.Service('/rfid_orient/bag', StringArr_Int32,
self.bag_cap)
self.__service_orient = rospy.Service('/rfid_orient/orient',
String_Int32, self.orient)
self.tag_arr_sub = rospy.Subscriber('/rfid/ears_reader_arr',
RFIDreadArr, self.add_tags)
rospy.logout('orient_node: Waiting for service calls.')
help='tag groundtruth pose in z')
p.add_option('-d',
action='store',
type='string',
dest='date',
default=None,
help='date (yyyymmdd)')
opt, args = p.parse_args()
rospy.init_node('flapper')
if opt.x == None or opt.y == None or opt.z == None or opt.date == None:
print 'Requires <x,y,z> and date arguments!'
exit()
p_left = rr.ROS_Robotis_Client('left_pan')
t_left = rr.ROS_Robotis_Client('left_tilt')
p_right = rr.ROS_Robotis_Client('right_pan')
t_right = rr.ROS_Robotis_Client('right_tilt')
EX_1 = 1.350
EX_2 = 0.520 # 30 deg.
# EX_2 = -1.000
tgt = TagGroundTruth(opt.x, opt.y, opt.z, '/map', opt.date)
p_left.move_angle(-1.0 * EX_1, math.radians(10), blocking=False)
t_left.move_angle(0.0, math.radians(10), blocking=False)
p_right.move_angle(EX_1, math.radians(10), blocking=False)