我们从Python开源项目中，提取了以下50个代码示例，用于说明如何使用rospy.Rate()。
def talker(): pub = rospy.Publisher('chatter', String, queue_size=10) rospy.init_node('talker', anonymous=True) rate = rospy.Rate(10) # 10hz while not rospy.is_shutdown(): hello_str = "hello world %s" % rospy.get_time() rospy.loginfo(hello_str) pub.publish(hello_str) rate.sleep() # rate = rospy.Rate(10) # hello_str = "hello world" # rospy.loginfo(hello_str) # pub.publish(hello_str) # rospy.spin() # exit(0)
def run_driver(): # init moveit commander moveit_commander.roscpp_initialize(sys.argv) # specify move group group = moveit_commander.MoveGroupCommander("arm") # init ros node rospy.init_node('real_servo_driver', anonymous=True) print "============ Waiting for RVIZ..." rospy.sleep(2) # move grasper to init position init_position(group) # set ros publisher rate, 10hz = 10 seconds for a circle rate = rospy.Rate(50) while True: group.set_random_target() plan_msg = group.plan() group.execute(plan_msg=plan_msg, wait=False) rospy.sleep(5) if is_exit: break # shutdown moveit commander moveit_commander.roscpp_shutdown()
def init(self): pygame.init() clock = pygame.time.Clock() screen = pygame.display.set_mode((250, 250)) rospy.init_node('highway_teleop') self.rate = rospy.Rate(rospy.get_param('~hz', 10)) self.acc = rospy.get_param('~acc', 5) self.yaw = rospy.get_param('~yaw', 0) self.robot_pub = rospy.Publisher('robot_0/cmd_vel', Twist, queue_size=1) print "Usage: \n \ up arrow: accelerate \n \ down arrow: decelerate \n \ left arrow: turn left \n \ right arrow: turn right"
def init(self): pygame.init() clock = pygame.time.Clock() screen = pygame.display.set_mode((250, 250)) rospy.init_node('teleop') self.rate = rospy.Rate(rospy.get_param('~hz', 20)) self.acc = rospy.get_param('~acc', 1) self.yaw = rospy.get_param('~yaw', 0.25) self.robot_pub = rospy.Publisher('control_command', controlCommand, queue_size=1) self.path_record_pub = rospy.Publisher('record_state', \ RecordState, queue_size=1) self.highway_game_start_pub = rospy.Publisher('highway_game_start', RecordState, queue_size=1) print "Usage: \n \ up arrow: accelerate \n \ down arrow: decelerate \n \ left arrow: turn left \n \ right arrow: turn right"
def attach_springs(self): """ Switches to joint torque mode and attached joint springs to current joint positions. """ # record initial joint angles self._start_angles = self._limb.joint_angles() # set control rate control_rate = rospy.Rate(self._rate) # for safety purposes, set the control rate command timeout. # if the specified number of command cycles are missed, the robot # will timeout and return to Position Control Mode self._limb.set_command_timeout((1.0 / self._rate) * self._missed_cmds) # loop at specified rate commanding new joint torques while not rospy.is_shutdown(): if not self._rs.state().enabled: rospy.logerr("Joint torque example failed to meet " "specified control rate timeout.") break self._update_forces() control_rate.sleep()
def wobble(self): self.set_neutral() """ Performs the wobbling """ command_rate = rospy.Rate(1) control_rate = rospy.Rate(100) start = rospy.get_time() while not rospy.is_shutdown() and (rospy.get_time() - start < 10.0): angle = random.uniform(-2.0, 0.95) while (not rospy.is_shutdown() and not (abs(self._head.pan() - angle) <= intera_interface.HEAD_PAN_ANGLE_TOLERANCE)): self._head.set_pan(angle, speed=0.3, timeout=0) control_rate.sleep() command_rate.sleep() self._done = True rospy.signal_shutdown("Example finished.")
def robot_listener(self): ''' rospy.wait_for_service('spawn') spawner = rospy.ServiceProxy('spawn', turtlesim.srv.Spawn) spawner(4, 2, 0, 'turtle2') ''' robot_vel_pub = rospy.Publisher('robot_0/cmd_vel', geometry_msgs.msg.Twist, queue_size=1) rate = rospy.Rate(10.0) while not rospy.is_shutdown(): try: (trans,rot) = self.listener.lookupTransform('/robot_3', '/robot_0', rospy.Time(0)) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): continue angular = 4 * math.atan2(trans[1], trans[0]) linear = 0.5 * math.sqrt(trans[0] ** 2 + trans[1] ** 2) cmd = geometry_msgs.msg.Twist() cmd.linear.x = linear cmd.angular.z = angular robot_vel_pub.publish(cmd) rate.sleep()
def water_potential_hydrogen_callback(msg): # float -1 ~ 1 command = msg.data # reset state to idle actuator_state["pump_3_ph_up_1"] = False actuator_state["pump_4_ph_down_1"] = False # Set actuator_state based on command if command > 0: actuator_state["pump_3_ph_up_1"] = True elif command < 0: actuator_state["pump_4_ph_down_1"] = True # nutrient_flora_duo_a is a "Rate" of dosage, so we can just change the dosage # without resetting to "idle state" since that doesn't exist.
def compute_static_stability_thread(): rate = rospy.Rate(60) global kron_com_vertices while not rospy.is_shutdown(): if 'COM-static' in support_area_handles \ and not gui.show_com_support_area: delete_support_area_display('COM-static') if not motion_plan or not motion_plan.started \ or 'COM-static' in support_area_handles \ or not gui.show_com_support_area: rate.sleep() continue color = (0.1, 0.1, 0.1, 0.5) req = contact_stability.srv.StaticAreaRequest( contacts=convert_contacts_to_ros(motion_plan.cur_stance.contacts), mass=robot.mass, z_out=motion_plan.com_height) try: res = compute_com_area(req) vertices = [array([v.x, v.y, v.z]) for v in res.vertices] update_support_area_display('COM-static', vertices, [], color) except rospy.ServiceException: delete_support_area_display('COM-static') rate.sleep()
def run(self): r=rospy.Rate(2) while self.is_looping(): if self.pub_audio_.get_num_connections() == 0: if self.isSubscribed: rospy.loginfo('Unsubscribing from audio bridge as nobody listens to the topics.') self.release() continue if not self.isSubscribed: self.reconfigure(self.config, 0) r.sleep() if self.isSubscribed: self.release() self.myBroker.shutdown()
def __init__(self): """Constructor for the class initialize topic subscription and instance variables """ self.r = rospy.Rate(5) self.publisher = rospy.Publisher('/cmd_vel', Twist, queue_size=10) rospy.Subscriber('/scan', LaserScan, self.process_scan) # user chooses which parking mode to be performed self.is_parallel = rospy.get_param('~parallel', False) # Instance Variables self.timestamp1 = None self.timestamp2 = None self.dist2Neato = None self.dist2Wall = None self.widthOfSpot = None self.twist = None self.radius = None # Adjusment to be made before moving along the arc self.adjustment = 0 self.isAligned = False
def run(self, distance, angle, snap_to, options): """ Run our code """ rospy.loginfo("Start test code") self.task_subscriber = rospy.Subscriber("/srcsim/finals/task", Task, self.task_status) rate = rospy.Rate(1) # 10hz if self.task_subscriber.get_num_connections() == 0: rospy.loginfo('waiting for task publisher...') while self.task_subscriber.get_num_connections() == 0: rate.sleep() if distance > 0.0001 or distance < -0.005: self.zarj_os.walk.forward(distance, True) while not self.zarj_os.walk.walk_is_done(): rospy.sleep(0.01) if abs(angle) > 0.0 or "turn" in options: align = "align" in options small_forward = 0.005 if align else None self.zarj_os.walk.turn(angle, snap_to, align_to_snap = align, small_forward = small_forward) while not self.zarj_os.walk.walk_is_done(): rospy.sleep(0.01)
def get_cylinder_status(self): self.cylinder_laser_client.wait_for_service() self.cylinder_opencv_client.wait_for_service() flag = 0 r = rospy.Rate(2) while not rospy.is_shutdown() and flag != 1: res_opencv = self.cylinder_opencv_client() res_laser = self.cylinder_laser_client() x_laser = res_laser.x theta_laser = res_laser.theta theta_opencv = res_opencv.theta if abs(theta_laser - theta_opencv) < 0.01: flag = 1 break r.sleep() return (x_laser,theta_laser)
def __init__(self): rospy.loginfo('[robot_move_pkg]->linear_move is initial') #??????????????? self.robot_state = robot_state.robot_position_state() self.cmd_move_pub = rospy.Publisher('/cmd_move', g_msgs.Twist, queue_size = 100) self.rate = rospy.Rate(150) #??????????? self.stop_tolerance = config.high_speed_stop_tolerance self.angular_tolerance = config.high_turn_angular_stop_tolerance #????????????? self.accurate_turn_an_angular = turn_an_angular.turn_an_angular() self.x_speed = 0.0 self.y_speed = 0.0 self.w_speed = config.high_turn_angular_speed #??????? self.linear_sp = spline_func.growth_curve() self.amend_speed = 0.12
def __init__(self): #?????????? m/s self.move_cmd_pub = rospy.Publisher('cmd_move_robot',g_msgs.Twist,queue_size=100) self.move_speed = config.go_along_circle_speed self.get_position = robot_state.robot_position_state() #????????? rad self.stop_tolerance = config.go_along_circle_angular_tolerance #???????? rospy.on_shutdown(self.brake) # ??sleep ??? ??????????? self.rate = 100.0 self.R = rospy.Rate(int(self.rate)) self.MODE = { 1:(-1, 1), 2:( 1,-1), 3:( 1, 1), 4:(-1,-1)}
def correct_angle(self): # rospy.loginfo("sadasdafasf") (x,theta,if_close_line) = self.close_line_cmd.find_line() r = rospy.Rate(50) move_velocity = g_msgs.Twist() if theta > 0: move_velocity.angular.z = -0.10 else: move_velocity.angular.z = 0.10 while not rospy.is_shutdown(): (x,theta,if_close_line) = self.close_line_cmd.find_line() self.cmd_move_pub.publish(move_velocity) #???????????????? if theta < 0.02 and theta > -0.02: rospy.loginfo("will Stop!!!!!!!!!!") self.brake() break r.sleep() #??3??????
def go_to_home(self): (x,theta,if_close_line) = self.close_line_cmd.find_line() r = rospy.Rate(50) move_velocity = g_msgs.Twist() while not rospy.is_shutdown(): (x,theta,if_close_line) = self.close_line_cmd.find_line() move_velocity.linear.y = -0.3 self.cmd_move_pub.publish(move_velocity) rospy.loginfo("python: y = %s",move_velocity.linear.y) if if_close_line != 0: rospy.loginfo("will Stop!!!!!!!!!!") self.brake() break r.sleep() #????????
def __init__(self): rospy.loginfo('[robot_move_pkg]->linear_move is initial') #??????????????? self.robot_state = robot_state.robot_position_state() self.cmd_move_pub = rospy.Publisher('/cmd_move', g_msgs.Twist, queue_size = 100) self.rate = rospy.Rate(150) #??????????? self.stop_tolerance = config.high_speed_stop_tolerance self.angular_tolerance = config.high_turn_angular_stop_tolerance #????????????? self.accurate_turn_an_angular = turn_an_angular.turn_an_angular() self.x_speed = 0.0 self.y_speed = 0.0 self.w_speed = config.high_turn_angular_speed #??????? self.linear_sp = spline_func.growth_curve() self.amend_speed = 0.18
def turn(self, goal_angular): # ???????????????,?????????????? # ????,???? rospy.loginfo('[robot_move_pkg]->move_in_robot.turn will turn %s'%goal_angular) current_angular = start_angular = self.robot_state.get_robot_current_w()#?????????? r = rospy.Rate(100) delta_angular = current_angular - start_angular move_velocity = g_msgs.Twist() while not rospy.is_shutdown() : if abs(delta_angular - abs(goal_angular)) < self.turn_move_stop_tolerance: break current_angular = self.robot_state.get_robot_current_w() move_velocity.angular.z = math.copysign(self.w_speed, goal_angular) delta_angular += abs(abs(current_angular) - abs(start_angular) ) start_angular = current_angular self.cmd_move_pub.publish(move_velocity) #??????????? r.sleep() self.brake()
def find_volleyball(self): self.find_ball_client.wait_for_service() res = self.find_ball_client(False) x = res.z y = -res.x theta = -res.theta if_volleyball = res.if_volleyball r = rospy.Rate(50) while not if_volleyball == True: res = self.find_ball_client(False) x = res.z y = -res.x theta = -res.theta if_volleyball = res.if_volleyball if if_volleyball == True: return (x,y,theta) #?????????????????????? #??????????
def simulate(self): loginfo("Beginning simulation") rate = Rate(2) self.temperature = 23.0 while not self.increased and not self.finished: self.temperature += 0.1 rate.sleep() loginfo(self.temperature) while not self.decreased and not self.finished: self.temperature -= 0.1 rate.sleep() loginfo(self.temperature) while self.temperature < 22.5 and not self.finished: self.temperature += 0.1 rate.sleep() loginfo(self.temperature) loginfo("Simulation ended.")
def start_simulation(self): loginfo("Beginning simulation") rate = rospy.Rate(1) while not self.on_fire: rate.sleep() loginfo("{} persons in building!".format(self.persons)) while not self.persons <= 0: self.persons -= 1 loginfo("{} persons in building!".format(self.persons)) rate.sleep() loginfo("Building is empty!") while not self.fire_department_arrived: rate.sleep() loginfo("Fire department has arrived!") rate.sleep() self.on_fire = False loginfo("Simulation ended.")
def __init__(self, loop_time = 0.1, pubs = {}, subs = {}): """init args: pubs: dict with topic / [type,], subs: dict with topic / [type, callback]""" smp_thread.__init__(self, loop_time = loop_time) # now init ros node rospy.init_node(self.name, anonymous=True) # loop frequency / sampling rate self.rate = rospy.Rate(1./self.loop_time) # local pub / sub self.default_queue_size_pub = 2 self.default_queue_size_sub = 2 if len(pubs) == 0 and len(subs) == 0: self.pub_sub_local_legacy() else: self.pub_sub_local(pubs, subs) # print "smp_thread_ros pubs", self.pub # print "smp_thread_ros subs", self.sub
def attach_springs(self): """ Switches to joint torque mode and attached joint springs to current joint positions. """ # record initial joint angles self._des_angles = self._limb.joint_angles() # set control rate control_rate = rospy.Rate(self._rate) # for safety purposes, set the control rate command timeout. # if the specified number of command cycles are missed, the robot # will timeout and disable self._limb.set_command_timeout((1.0 / self._rate) * self._missed_cmds) # loop at specified rate commanding new joint torques while not rospy.is_shutdown(): if not self._rs.state().enabled: rospy.logerr("Joint torque example failed to meet " "specified control rate timeout.") break self._update_forces() control_rate.sleep()
def execute(): # define publisher and its topic pub = rospy.Publisher('write_angles',Angles,queue_size = 10) rospy.init_node('write_angles_node',anonymous = True) rate = rospy.Rate(10) # write 4 angles if len(sys.argv) == 5: s1 = int(sys.argv[1]) s2 = int(sys.argv[2]) s3 = int(sys.argv[3]) s4 = int(sys.argv[4]) pub.publish(s1,s2,s3,s4) else: raiseError() rate.sleep() # main function
def zero_sensor(self): rospy.loginfo("Zeroing sensor...") # Wait a little bit until we get a message from the sensor rospy.sleep(1) self.tip_offset, self.inside_offset = (np.zeros_like(self.tip), np.zeros_like(self.inside)) inside_vals, tip_vals = [], [] r = rospy.Rate(10) while not rospy.is_shutdown() and len(inside_vals) < 10: inside, tip = self.inside, self.tip # If there are zero values (most likely becase a message # has not yet been received), skip that. We could also # initialize them with nans to find out if there's a # problem if all(inside) and all(tip): inside_vals.append(inside) tip_vals.append(tip) r.sleep() # Center around 5000, so ranges are similar to when not centering self.inside_offset = np.min(inside_vals, axis=0) - 5000 self.tip_offset = np.min(tip_vals, axis=0) - 5000 rospy.loginfo("Zeroing finished")
def limb_pose(limb_name): """Get limb pose at time of OK cuff button press.""" button = CuffOKButton(limb_name) rate = rospy.Rate(20) # rate at which we check whether button was # pressed or not rospy.loginfo( 'Waiting for %s OK cuff button press to save pose' % limb_name) while not button.pressed and not rospy.is_shutdown(): rate.sleep() joint_pose = baxter_interface.Limb(limb_name).joint_angles() # Now convert joint coordinates to end effector cartesian # coordinates using forward kinematics. kinematics = baxter_kinematics(limb_name) endpoint_pose = kinematics.forward_position_kinematics(joint_pose) # How is this different from # baxter_interface.Limb(limb_name).endpoint_pose() print() print('baxter_interface endpoint pose:') print(baxter_interface.Limb(limb_name).endpoint_pose()) print('pykdl forward kinematics endpoint pose:') print(endpoint_pose) print() return endpoint_pose
def attach_springs(self): self._start_angles = self._get_cmd_positions() control_rate = rospy.Rate(self._rate) # for safety purposes, set the control rate command timeout. # if the specified number of command cycles are missed, the robot # will timeout and disable self._limb.set_command_timeout((1.0 / self._rate) * self._missed_cmds) error = [self._limb.joint_angles()[key] - self._start_angles[key] for key in self._limb.joint_angles().keys()] self.sum_sqr_error = sum([error[i]**2 for i in range(len(error))]) print ('-------new set of joint position---------') print (self.sum_sqr_error) tolerance = 0.020 # loop at specified rate commanding new joint torques while self.sum_sqr_error>tolerance and not rospy.is_shutdown(): if not self._rs.state().enabled: rospy.logerr("Joint torque example failed to meet " "specified control rate timeout.") break self._update_forces() control_rate.sleep()
def limb_pose(limb_name): """Get limb pose at time of OK cuff button press.""" button = CuffOKButton(limb_name) rate = rospy.Rate(20) # rate at which we check whether button was # pressed or not rospy.loginfo( 'Waiting for %s OK cuff button press to save pose' % limb_name) while not button.pressed and not rospy.is_shutdown(): rate.sleep() joint_pose = baxter_interface.Limb(limb_name).joint_angles() # Now convert joint coordinates to end effector cartesian # coordinates using forward kinematics. kinematics = baxter_kinematics(limb_name) endpoint_pose = kinematics.forward_position_kinematics(joint_pose) #print (endpoint_pose) return endpoint_pose
def goto_cup(self): # self.calibrate_obj_det_pub.publish(True) # # while self.calibrated == False: # pass # # print("Finger Sensors calibrated") rate = rospy.Rate(100) while not rospy.is_shutdown(): try: trans = self.tfBuffer.lookup_transform('root', 'teaCup_position', rospy.Time()) break except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): rate.sleep() # continue translation = [trans.transform.translation.x, trans.transform.translation.y, trans.transform.translation.z] rotation = [trans.transform.rotation.x, trans.transform.rotation.y, trans.transform.rotation.z, trans.transform.rotation.w] pose_value = translation + rotation #second arg=0 (absolute movement), arg = '-r' (relative movement) self.cmmnd_CartesianPosition(pose_value, 0)
def cmmnd_makeContact_USBPlug(self, sensitivity): rate = rospy.Rate(100) while (self.bump_finger_1 < sensitivity)and not rospy.is_shutdown(): print (self.bump_finger_1) self.cmmnd_CartesianVelocity([-0.03,0,0,0,0,0,1]) rate.sleep() print ("contact made with the ground") # def pick_USBlight_1(self, current_finger_position): # ii = 0 # rate = rospy.Rate(100) # while self.touch_finger_3 != True and not rospy.is_shutdown(): # current_finger_position[0] += 5 # slowly close finger_1 until contact is made # print (current_finger_position[0]) # self.cmmnd_FingerPosition([current_finger_position[0], current_finger_position[1], current_finger_position[2]]) # rate.sleep() # self.touch_finger_1 = False # return current_finger_position
def cmmd_touchBlock(self,current_finger_position): ii = 0 rate = rospy.Rate(100) while self.touch_finger_1 != True and not rospy.is_shutdown(): current_finger_position[0] += 5 # slowly close finger_1 until contact is made # print (current_finger_position[0]) self.cmmnd_FingerPosition([current_finger_position[0], current_finger_position[1], current_finger_position[2]]) rate.sleep() self.touch_finger_1 = False while self.touch_finger_2 != True and not rospy.is_shutdown(): current_finger_position[1] += 5 # slowly close finger_1 until contact is made # print (current_finger_position[0]) self.cmmnd_FingerPosition([current_finger_position[0], current_finger_position[1], current_finger_position[2]]) rate.sleep() self.touch_finger_2 = False return current_finger_position
def lift_spoon(self): rate = rospy.Rate(100) # NOTE to publish cmmds to velocity_pub at 100Hz while self.touch_finger_3 != True: self.cmmnd_CartesianVelocity([0,0.025,0,0,0,0,1]) rate.sleep() self.touch_finger_3 = False # p.cmmnd_CartesianPosition([0.02,0,0,0,0,0,1],'-r') self.cmmnd_FingerPosition([0, 0, 60]) self.cmmnd_FingerPosition([100, 0, 60]) self.cmmnd_FingerPosition([100, 0, 100]) self.cmmnd_FingerPosition([100, 100, 100]) self.cmmnd_CartesianPosition([0, 0, 0.13, 0, 0, 0, 1],'-r') # self.cmmnd_FingerPosition([100, 100, 100])
def search_USBlight(self): if self.listen.frameExists("/j2n6a300_end_effector") and self.listen.frameExists("/root"): # print ("we are in the search spoon fucntion") self.listen.waitForTransform('/j2n6a300_end_effector','/root',rospy.Time(),rospy.Duration(100.0)) t = self.listen.getLatestCommonTime("/j2n6a300_end_effector","/root") translation, quaternion = self.listen.lookupTransform("/j2n6a300_end_effector","/root",t) matrix1=self.listen.fromTranslationRotation(translation,quaternion) counter=0 rate=rospy.Rate(100) while not self.obj_det: counter = counter + 1 if(counter < 200): cart_velocities = np.dot(matrix1[:3,:3],np.array([0,0,0.05])[np.newaxis].T) #change in y->x, z->y, x->z cart_velocities = cart_velocities.T[0].tolist() self.cmmnd_CartesianVelocity(cart_velocities + [0,0,0,1]) else: cart_velocities = np.dot(matrix1[:3,:3],np.array([0,0,-0.05])[np.newaxis].T) cart_velocities = cart_velocities.T[0].tolist() self.cmmnd_CartesianVelocity(cart_velocities + [0,0,0,1]) rate.sleep() if(counter >400): counter=0
def search_straw(self): if self.listen.frameExists("/j2n6a300_end_effector") and self.listen.frameExists("/root"): # print ("we are in the search spoon fucntion") self.listen.waitForTransform('/j2n6a300_end_effector','/root',rospy.Time(),rospy.Duration(100.0)) t = self.listen.getLatestCommonTime("/j2n6a300_end_effector","/root") translation, quaternion = self.listen.lookupTransform("/j2n6a300_end_effector","/root",t) matrix1=self.listen.fromTranslationRotation(translation,quaternion) counter=0 rate=rospy.Rate(100) while not self.obj_det: counter = counter + 1 if(counter < 200): cart_velocities = np.dot(matrix1[:3,:3],np.array([00,0,0.05])[np.newaxis].T) #change in y->x, z->y, x->z cart_velocities = cart_velocities.T[0].tolist() self.cmmnd_CartesianVelocity(cart_velocities + [0,0,0,1]) else: cart_velocities = np.dot(matrix1[:3,:3],np.array([0.0,0,-.05])[np.newaxis].T) cart_velocities = cart_velocities.T[0].tolist() self.cmmnd_CartesianVelocity(cart_velocities + [0,0,0,1]) rate.sleep() if(counter >400): counter=0
def searchSpoon(self): if self.listen.frameExists("/j2n6a300_end_effector") and self.listen.frameExists("/root"): # print ("we are in the search spoon fucntion") self.listen.waitForTransform('/j2n6a300_end_effector','/root',rospy.Time(),rospy.Duration(100.0)) t = self.listen.getLatestCommonTime("/j2n6a300_end_effector","/root") translation, quaternion = self.listen.lookupTransform("/j2n6a300_end_effector","/root",t) matrix1=self.listen.fromTranslationRotation(translation,quaternion) counter=0 rate=rospy.Rate(100) while not self.obj_det: counter = counter + 1 if(counter < 200): cart_velocities = np.dot(matrix1[:3,:3],np.array([0,0,0.05])[np.newaxis].T) #change in y->x, z->y, x->z cart_velocities = cart_velocities.T[0].tolist() self.cmmnd_CartesianVelocity(cart_velocities + [0,0,0,1]) else: cart_velocities = np.dot(matrix1[:3,:3],np.array([0,0,-0.05])[np.newaxis].T) cart_velocities = cart_velocities.T[0].tolist() self.cmmnd_CartesianVelocity(cart_velocities + [0,0,0,1]) rate.sleep() if(counter >400): counter=0
def back_forth_search_xy(self): rate = rospy.Rate(100) for i in range(100): if np.all(np.array(self.finger_detect) == np.array(self.detect_goal)): return 'found' msg = self.create_pose_velocity_msg([0.0,-0.05,0.0,0.0,0.0,0.0]) self.jn.kinematic_control(msg) rate.sleep() for i in range(200): if np.all(np.array(self.finger_detect)[[self.fingers]] == np.array(self.detect_goal)): return 'found' msg = self.create_pose_velocity_msg([0.0,0.05,0.0,0.0,0.0,0.0]) self.jn.kinematic_control(msg) rate.sleep() for i in range(100): if np.all(np.array(self.finger_detect) == np.array(self.detect_goal)): return 'found' msg = self.create_pose_velocity_msg([0.0,-0.05,0.0,0.0,0.0,0.0]) self.jn.kinematic_control(msg) rate.sleep() return 'not_found'
def main(): rospy.init_node('issue_com') pub = rospy.Publisher(POS_COM_TOPIC, PositionCommand, queue_size=10) test_pub = rospy.Publisher(TEST_TOPIC, Empty, queue_size=10) sub = rospy.Subscriber(POS_COM_TOPIC, PositionCommand, listen) #sub = rospy.Subscriber('/joint_states', JointState, listen) pc = PositionCommand() pc.mode = JOINT_SPACE #pc.arm = PositionCommand.LEFT_ARM pc.arm = 1#PositionCommand.RIGHT_ARM pc.data = np.zeros(7) r = rospy.Rate(1) #while not rospy.is_shutdown(): # pub.publish(pc) # r.sleep() # print 'published!' r.sleep() test_pub.publish(Empty()) pub.publish(pc)
def depth_callback(self, ros_image): try: inImg = self.bridge.imgmsg_to_cv2(ros_image) except CvBridgeError, e: print e inImgarr = np.array(inImg, dtype=np.uint16) # inImgarr = cv2.GaussianBlur(inImgarr, (3, 3), 0) # cv2.normalize(inImgarr, inImgarr, 0, 1, cv2.NORM_MINMAX) self.outImg, self.num_fingers = self.process_depth_image(inImgarr) # outImg = self.process_depth_image(inImgarr) # rate = rospy.Rate(10) self.num_pub.publish(self.num_fingers) # self.img_pub.publish(self.bridge.cv2_to_imgmsg(self.outImg, "bgr8")) # rate.sleep() cv2.imshow("Hand Gesture Recognition", self.outImg) cv2.waitKey(3)
def __init__(self): self.joy_pub = rospy.Publisher('sensors/joystick/1', Joy, queue_size=1) self.joy_pub2 = rospy.Publisher('sensors/joystick/2', Joy, queue_size=1) self.rospack = RosPack() self.rate = rospy.Rate(20) count = len(devices.gamepads) if count < 2: rospy.logerr("Sensors: Expecting 2 joysticks but found only {}, exiting".format(count)) sys.exit(-1) gamepads = sorted(devices.gamepads, key=lambda pad: int(str(pad.name)[-1])) rospy.loginfo(gamepads) self.joysticks = [JoystickThread(pad) for pad in gamepads] [joystick.start() for joystick in self.joysticks]
def __init__(self): # Load parameters and hack the tuple conversions so that OpenCV is happy self.rospack = RosPack() with open(join(self.rospack.get_path('apex_playground'), 'config', 'environment.json')) as f: self.params = json.load(f) self.params['tracking']['ball']['lower'] = tuple(self.params['tracking']['ball']['lower']) self.params['tracking']['ball']['upper'] = tuple(self.params['tracking']['ball']['upper']) self.params['tracking']['arena']['lower'] = tuple(self.params['tracking']['arena']['lower']) self.params['tracking']['arena']['upper'] = tuple(self.params['tracking']['arena']['upper']) self.tracking = BallTracking(self.params) self.conversions = EnvironmentConversions() self.ball_pub = rospy.Publisher('environment/ball', CircularState, queue_size=1) self.light_pub = rospy.Publisher('environment/light', UInt8, queue_size=1) self.image_pub = rospy.Publisher('environment/image', Float32MultiArray, queue_size=1) self.sound_pub = rospy.Publisher('environment/sound', Float32, queue_size=1) self.rate = rospy.Rate(self.params['rate'])
def __init__(self): self.rospack = RosPack() with open(join(self.rospack.get_path('apex_playground'), 'config', 'torso.json')) as f: self.params = json.load(f) self.publish_rate = rospy.Rate(self.params['publish_rate']) self.demo = rospy.get_param('demo_mode') # Protected resources self.in_rest_pose = False self.robot_lock = RLock() # Used services self.torso = TorsoServices(self.params['robot_name']) # Proposed services self.reset_srv_name = 'torso/reset' self.reset_srv = None
def wait_for_human_interaction(self, arm_threshold=1, joystick_threshold=0.15): rospy.loginfo("We are waiting for human interaction...") def detect_arm_variation(): new_effort = np.array(self.topics.torso_l_j.effort) delta = np.absolute(effort - new_effort) return np.amax(delta) > arm_threshold def detect_joy_variation(): return np.amax(np.abs(self.topics.joy1.axes)) > joystick_threshold effort = np.array(self.topics.torso_l_j.effort) rate = rospy.Rate(50) is_joystick_demo = None while not rospy.is_shutdown(): if detect_arm_variation(): is_joystick_demo = False break elif detect_joy_variation(): is_joystick_demo = True break rate.sleep() return is_joystick_demo ################################# Service callbacks
def wait_for_server(self): """Waits until interoperability server is reachable.""" reachable = False rate = rospy.Rate(1) while not reachable and not rospy.is_shutdown(): try: response = requests.get( self.url, timeout=self.timeout, verify=self.verify) response.raise_for_status() reachable = response.ok except requests.ConnectionError: rospy.logwarn_throttle(5.0, "Waiting for server: {}".format( self.url)) except Exception as e: rospy.logerr_throttle( 5.0, "Unexpected error waiting for server: {}, {}".format( self.url, e)) if not reachable: rate.sleep()
def spin(self): # @todo: is this excessively hitting the master? r = rospy.Rate(10.0) while not rospy.is_shutdown(): for srv in self._local_services: srv_uri = self._local_manager.lookup_service(srv) if srv_uri: self._foreign_manager.advertise_service(srv, srv_uri) else: self._foreign_manager.unadvertise_service(srv) for srv in self._foreign_services: srv_uri = self._foreign_manager.lookup_service(srv) if srv_uri: self._local_manager.advertise_service(srv, srv_uri) else: self._local_manager.unadvertise_service(srv) r.sleep() if self._local_manager: self._local_manager.unsubscribe_all() if self._foreign_manager: self._foreign_manager.unsubscribe_all()
def __init__(self,dis=0.0): State.__init__(self, outcomes=['succeeded','preempted','aborted'],input_keys=['target']) rospy.on_shutdown(self.shutdown) self.test_distance = target.y-dis self.rate = 200 self.r = rospy.Rate(self.rate) self.speed = rospy.get_param('~speed',0.08) self.tolerance = rospy.get_param('~tolerance', 0.01) self.cmd_vel = rospy.Publisher('/mobile_base_controller/smooth_cmd_vel',Twist,queue_size=5) self.base_frame = rospy.get_param('~base_frame', '/base_link') self.odom_frame = rospy.get_param('~odom_frame', '/odom') self.tf_listener = tf.TransformListener() rospy.sleep(1) self.tf_listener.waitForTransform(self.odom_frame,self.base_frame,rospy.Time(),rospy.Duration(60.0)) #define a bianliang self.flag = rospy.get_param('~flag', True) rospy.loginfo(self.test_distance) #tf get position #publish cmd_vel
def __init__(self,angle=0): State.__init__(self, outcomes=['succeeded','aborted','preempted'],input_keys=["angle_in"]) self.speed = rospy.get_param('~speed', 0.10) self.secretindigal = 1.0 self.tolerance = rospy.get_param('tolerance', math.radians(5)) self.start = True self.cmd_vel = rospy.Publisher('mobile_base_controller/smooth_cmd_vel', Twist, queue_size=5) self.base_frame = rospy.get_param('~base_frame', '/base_link') self.odom_frame = rospy.get_param('~odom_frame', '/odom') rospy.on_shutdown(self.shutdown) self.rate = 30 self.start_test = True self.r = rospy.Rate(self.rate) self.tf_listener = tf.TransformListener() rospy.sleep(0.5) self.tf_listener.waitForTransform(self.odom_frame, self.base_frame, rospy.Time(), rospy.Duration(60))
def __init__(self, angle=0): State.__init__(self, outcomes=['succeeded', 'aborted', 'preempted'], input_keys=["angle_in"]) self.speed = rospy.get_param('~speed', 0.03) self.secretindigal = 1.0 self.tolerance = rospy.get_param('tolerance', math.radians(5)) self.start = True self.cmd_vel = rospy.Publisher('mobile_base_controller/smooth_cmd_vel', Twist, queue_size=5) self.base_frame = rospy.get_param('~base_frame', '/base_link') self.odom_frame = rospy.get_param('~odom_frame', '/odom') rospy.on_shutdown(self.shutdown) self.rate = 30 self.start_test = True self.r = rospy.Rate(self.rate) self.angle=angle self.tf_listener = tf.TransformListener() rospy.sleep(0.5) self.tf_listener.waitForTransform(self.odom_frame, self.base_frame, rospy.Time(), rospy.Duration(60))
