Python rospy.Rate() Examples
The following are 30
code examples of rospy.Rate().
You can vote up the ones you like or vote down the ones you don't like,
and go to the original project or source file by following the links above each example.
You may also want to check out all available functions/classes of the module
rospy
, or try the search function
.
Example #1
Source File: move_circle.py From ROS-Robotics-By-Example with MIT License | 10 votes |
def move_circle(): # Create a publisher which can "talk" to Turtlesim and tell it to move pub = rospy.Publisher('turtle1/cmd_vel', Twist, queue_size=1) # Create a Twist message and add linear x and angular z values move_cmd = Twist() move_cmd.linear.x = 1.0 move_cmd.angular.z = 1.0 # Save current time and set publish rate at 10 Hz now = rospy.Time.now() rate = rospy.Rate(10) # For the next 6 seconds publish cmd_vel move commands to Turtlesim while rospy.Time.now() < now + rospy.Duration.from_sec(6): pub.publish(move_cmd) rate.sleep()
Example #2
Source File: wheel_scaler.py From differential-drive with GNU General Public License v3.0 | 7 votes |
def scaleWheel(): rospy.init_node("wheel_scaler") rospy.loginfo("wheel_scaler started") scale = rospy.get_param('distance_scale', 1) rospy.loginfo("wheel_scaler scale: %0.2f", scale) rospy.Subscriber("lwheel", Int16, lwheelCallback) rospy.Subscriber("rwheel", Int16, rwheelCallback) lscaled_pub = rospy.Publisher("lwheel_scaled", Int16, queue_size=10) rscaled_pub = rospy.Publisher("rwheel_scaled", Int16, queue_size=10) ### testing sleep CPU usage r = rospy.Rate(50) while not rospy.is_shutdown: r.sleep() rospy.spin()
Example #3
Source File: input_velodyne.py From FL3D with GNU General Public License v3.0 | 7 votes |
def publish_pc2(pc, obj): """Publisher of PointCloud data""" pub = rospy.Publisher("/points_raw", PointCloud2, queue_size=1000000) rospy.init_node("pc2_publisher") header = std_msgs.msg.Header() header.stamp = rospy.Time.now() header.frame_id = "velodyne" points = pc2.create_cloud_xyz32(header, pc[:, :3]) pub2 = rospy.Publisher("/points_raw1", PointCloud2, queue_size=1000000) header = std_msgs.msg.Header() header.stamp = rospy.Time.now() header.frame_id = "velodyne" points2 = pc2.create_cloud_xyz32(header, obj) r = rospy.Rate(0.1) while not rospy.is_shutdown(): pub.publish(points) pub2.publish(points2) r.sleep()
Example #4
Source File: rudder_control_heading.py From usv_sim_lsa with Apache License 2.0 | 7 votes |
def talker_ctrl(): global rate_value rospy.init_node('usv_simple_ctrl', anonymous=True) rate = rospy.Rate(rate_value) # 10h # publishes to thruster and rudder topics pub_motor = rospy.Publisher('thruster_command', JointState, queue_size=10) pub_rudder = rospy.Publisher('joint_setpoint', JointState, queue_size=10) pub_result = rospy.Publisher('move_usv/result', Float64, queue_size=10) # subscribe to state and targer point topics rospy.Subscriber("state", Odometry, get_pose) # get usv position (add 'gps' position latter) rospy.Subscriber("move_usv/goal", Odometry, get_target) # get target position while not rospy.is_shutdown(): try: pub_motor.publish(thruster_ctrl_msg()) pub_rudder.publish(rudder_ctrl_msg()) pub_result.publish(verify_result()) rate.sleep() except rospy.ROSInterruptException: rospy.logerr("ROS Interrupt Exception! Just ignore the exception!") except rospy.ROSTimeMovedBackwardsException: rospy.logerr("ROS Time Backwards! Just ignore the exception!")
Example #5
Source File: twist_to_motors.py From ROS-Programming-Building-Powerful-Robots with MIT License | 7 votes |
def spin(self): ############################################################# r = rospy.Rate(self.rate) idle = rospy.Rate(10) then = rospy.Time.now() self.ticks_since_target = self.timeout_ticks ###### main loop ###### while not rospy.is_shutdown(): while not rospy.is_shutdown() and self.ticks_since_target < self.timeout_ticks: self.spinOnce() r.sleep() idle.sleep() #############################################################
Example #6
Source File: navigation_pub.py From usv_sim_lsa with Apache License 2.0 | 7 votes |
def navigation(): pub = rospy.Publisher('usv_position_setpoint', Odometry, queue_size=10) # only create a rostopic, navigation system TODO rospy.init_node('navigation_publisher') rate = rospy.Rate(60) # 10h x = -20.0 y = -20.0 msg = Odometry() # msg.header = Header() msg.header.stamp = rospy.Time.now() msg.header.frame_id = "navigation" msg.pose.pose.position = Point(x, y, 0.) while not rospy.is_shutdown(): pub.publish(msg) rate.sleep()
Example #7
Source File: rudder_control_heading_old.py From usv_sim_lsa with Apache License 2.0 | 7 votes |
def talker_ctrl(): global rate_value # publishes to thruster and rudder topics pub_motor = rospy.Publisher('thruster_command', JointState, queue_size=10) pub_rudder = rospy.Publisher('joint_setpoint', JointState, queue_size=10) rospy.init_node('usv_simple_ctrl', anonymous=True) rate = rospy.Rate(rate_value) # 10h # subscribe to state and targer point topics rospy.Subscriber("state", Odometry, get_pose) # get usv position (add 'gps' position latter) rospy.Subscriber("usv_position_setpoint", Odometry, get_target) # get target position while not rospy.is_shutdown(): pub_motor.publish(thruster_ctrl_msg()) pub_rudder.publish(rudder_ctrl_msg()) rate.sleep()
Example #8
Source File: script.py From RAFCON with Eclipse Public License 1.0 | 6 votes |
def execute(self, inputs, outputs, gvm): turtle_name = inputs["turtle_name"] x = inputs["x_vel"] phi = inputs["phi_vel"] rate = rospy.Rate(10) position_vector = Vector3(x, 0, 0) rotation_vector = Vector3(0, 0, phi) twist_msg = Twist(position_vector, rotation_vector) self.logger.info("moving turtle {} {}".format(str(x), str(phi))) self.logger.info("publish twist to turtle {}".format(turtle_name)) turtle_vel_publisher = rospy.Publisher("/" + turtle_name + "/cmd_vel", Twist, queue_size=10, latch=True) turtle_vel_publisher.publish(twist_msg) rate.sleep() return 0
Example #9
Source File: diff_control_heading.py From usv_sim_lsa with Apache License 2.0 | 6 votes |
def talker_ctrl(): global rate_value global result # publishes to thruster and rudder topics pub_motor = rospy.Publisher('thruster_command', JointState, queue_size=10) pub_result = rospy.Publisher('move_usv/result', Float64, queue_size=10) rospy.init_node('usv_simple_ctrl', anonymous=True) rate = rospy.Rate(rate_value) # 10h # subscribe to state and targer point topics rospy.Subscriber("state", Odometry, get_pose) # get usv position (add 'gps' position latter) rospy.Subscriber("move_usv/goal", Odometry, get_target) # get target position while not rospy.is_shutdown(): try: pub_motor.publish(thruster_ctrl_msg()) pub_result.publish(verify_result()) rate.sleep() except rospy.ROSInterruptException: rospy.logerr("ROS Interrupt Exception! Just ignore the exception!") except rospy.ROSTimeMovedBackwardsException: rospy.logerr("ROS Time Backwards! Just ignore the exception!")
Example #10
Source File: airboat_control_heading.py From usv_sim_lsa with Apache License 2.0 | 6 votes |
def talker_ctrl(): global rate_value rospy.init_node('usv_simple_ctrl', anonymous=True) rate = rospy.Rate(rate_value) # 10h # publishes to thruster and rudder topics pub_motor = rospy.Publisher('thruster_command', JointState, queue_size=10) pub_rudder = rospy.Publisher('joint_setpoint', JointState, queue_size=10) pub_result = rospy.Publisher('move_usv/result', Float64, queue_size=10) # subscribe to state and targer point topics rospy.Subscriber("state", Odometry, get_pose) # get usv position (add 'gps' position latter) rospy.Subscriber("move_usv/goal", Odometry, get_target) # get target position while not rospy.is_shutdown(): try: pub_motor.publish(thruster_ctrl_msg()) pub_rudder.publish(rudder_ctrl_msg()) pub_result.publish(verify_result()) rate.sleep() except rospy.ROSInterruptException: rospy.logerr("ROS Interrupt Exception! Just ignore the exception!") except rospy.ROSTimeMovedBackwardsException: rospy.logerr("ROS Time Backwards! Just ignore the exception!")
Example #11
Source File: sailboat_control_heading.py From usv_sim_lsa with Apache License 2.0 | 6 votes |
def talker_ctrl(): global rate_value global currentHeading global windDir global isTacking global heeling global spHeading rospy.init_node('usv_simple_ctrl', anonymous=True) rate = rospy.Rate(rate_value) # 10h # publishes to thruster and rudder topics #pub_sail = rospy.Publisher('angleLimits', Float64, queue_size=10) pub_rudder = rospy.Publisher('joint_setpoint', JointState, queue_size=10) pub_result = rospy.Publisher('move_usv/result', Float64, queue_size=10) pub_heading = rospy.Publisher('currentHeading', Float64, queue_size=10) pub_windDir = rospy.Publisher('windDirection', Float64, queue_size=10) pub_heeling = rospy.Publisher('heeling', Float64, queue_size=10) pub_spHeading = rospy.Publisher('spHeading', Float64, queue_size=10) # subscribe to state and targer point topics rospy.Subscriber("state", Odometry, get_pose) # get usv position (add 'gps' position latter) rospy.Subscriber("move_usv/goal", Odometry, get_target) # get target position while not rospy.is_shutdown(): try: pub_rudder.publish(rudder_ctrl_msg()) #pub_sail.publish(sail_ctrl()) pub_result.publish(verify_result()) pub_heading.publish(currentHeading) pub_windDir.publish(windDir) pub_heeling.publish(heeling) pub_spHeading.publish(spHeading) rate.sleep() except rospy.ROSInterruptException: rospy.logerr("ROS Interrupt Exception! Just ignore the exception!") except rospy.ROSTimeMovedBackwardsException: rospy.logerr("ROS Time Backwards! Just ignore the exception!")
Example #12
Source File: wheel_loopback.py From differential-drive with GNU General Public License v3.0 | 6 votes |
def spin(self): ############################################### r = rospy.Rate self.secs_since_target = self.timeout_secs self.then = rospy.Time.now() self.latest_msg_time = rospy.Time.now() rospy.loginfo("-D- spinning") ###### main loop ######### while not rospy.is_shutdown(): while not rospy.is_shutdown() and self.secs_since_target < self.timeout_secs: self.spinOnce() r.sleep self.secs_since_target = rospy.Time.now() - self.latest_msg_time self.secs_since_target = self.secs_since_target.to_sec() #rospy.loginfo(" inside: secs_since_target: %0.3f" % self.secs_since_target) # it's been more than timeout_ticks since we recieved a message self.secs_since_target = rospy.Time.now() - self.latest_msg_time self.secs_since_target = self.secs_since_target.to_sec() # rospy.loginfo(" outside: secs_since_target: %0.3f" % self.secs_since_target) self.velocity = 0 r.sleep ###############################################
Example #13
Source File: widowx_controller.py From visual_foresight with MIT License | 6 votes |
def __init__(self, robot_name, print_debug, email_cred_file='', log_file='', control_rate=100, gripper_attached='default'): super(WidowXController, self).__init__(robot_name, print_debug, email_cred_file, log_file, control_rate, gripper_attached) self._redist_rate = rospy.Rate(50) self._arbotix = ArbotiX('/dev/ttyUSB0') assert self._arbotix.syncWrite(MAX_TORQUE_L, [[servo_id, 255, 0] for servo_id in SERVO_IDS]) != -1, "failure during servo configuring" assert self._arbotix.syncWrite(TORQUE_LIMIT, [[servo_id, 255, 0] for servo_id in SERVO_IDS]) != -1, "failure during servo configuring" self._joint_lock = Lock() self._angles, self._velocities = {}, {} rospy.Subscriber("/joint_states", JointState, self._joint_callback) time.sleep(1) self._joint_pubs = [rospy.Publisher('/{}/command'.format(name), Float64, queue_size=1) for name in JOINT_NAMES[:-1]] self._gripper_pub = rospy.Publisher('/gripper_prismatic_joint/command', Float64, queue_size=1) p.connect(p.DIRECT) widow_x_urdf = '/'.join(__file__.split('/')[:-1]) + '/widowx/widowx.urdf' self._armID = p.loadURDF(widow_x_urdf, useFixedBase=True) p.resetBasePositionAndOrientation(self._armID, [0, 0, 0], p.getQuaternionFromEuler([np.pi, np.pi, np.pi])) self._n_errors = 0
Example #14
Source File: wheel_loopback.py From ROS-Programming-Building-Powerful-Robots with MIT License | 6 votes |
def spin(self): ############################################### r = rospy.Rate self.secs_since_target = self.timeout_secs self.then = rospy.Time.now() self.latest_msg_time = rospy.Time.now() rospy.loginfo("-D- spinning") ###### main loop ######### while not rospy.is_shutdown(): while not rospy.is_shutdown() and self.secs_since_target < self.timeout_secs: self.spinOnce() r.sleep self.secs_since_target = rospy.Time.now() - self.latest_msg_time self.secs_since_target = self.secs_since_target.to_sec() #rospy.loginfo(" inside: secs_since_target: %0.3f" % self.secs_since_target) # it's been more than timeout_ticks since we recieved a message self.secs_since_target = rospy.Time.now() - self.latest_msg_time self.secs_since_target = self.secs_since_target.to_sec() # rospy.loginfo(" outside: secs_since_target: %0.3f" % self.secs_since_target) self.velocity = 0 r.sleep ###############################################
Example #15
Source File: twist_to_motors.py From ROS-Programming-Building-Powerful-Robots with MIT License | 6 votes |
def spin(self): ############################################################# r = rospy.Rate(self.rate) idle = rospy.Rate(10) then = rospy.Time.now() self.ticks_since_target = self.timeout_ticks ###### main loop ###### while not rospy.is_shutdown(): while not rospy.is_shutdown() and self.ticks_since_target < self.timeout_ticks: self.spinOnce() r.sleep() idle.sleep() #############################################################
Example #16
Source File: wheel_loopback.py From ROS-Programming-Building-Powerful-Robots with MIT License | 6 votes |
def spin(self): ############################################### r = rospy.Rate self.secs_since_target = self.timeout_secs self.then = rospy.Time.now() self.latest_msg_time = rospy.Time.now() rospy.loginfo("-D- spinning") ###### main loop ######### while not rospy.is_shutdown(): while not rospy.is_shutdown() and self.secs_since_target < self.timeout_secs: self.spinOnce() r.sleep self.secs_since_target = rospy.Time.now() - self.latest_msg_time self.secs_since_target = self.secs_since_target.to_sec() #rospy.loginfo(" inside: secs_since_target: %0.3f" % self.secs_since_target) # it's been more than timeout_ticks since we recieved a message self.secs_since_target = rospy.Time.now() - self.latest_msg_time self.secs_since_target = self.secs_since_target.to_sec() # rospy.loginfo(" outside: secs_since_target: %0.3f" % self.secs_since_target) self.velocity = 0 r.sleep ###############################################
Example #17
Source File: twist_to_motors.py From ROS-Programming-Building-Powerful-Robots with MIT License | 6 votes |
def spin(self): ############################################################# r = rospy.Rate(self.rate) idle = rospy.Rate(10) then = rospy.Time.now() self.ticks_since_target = self.timeout_ticks ###### main loop ###### while not rospy.is_shutdown(): while not rospy.is_shutdown() and self.ticks_since_target < self.timeout_ticks: self.spinOnce() r.sleep() idle.sleep() #############################################################
Example #18
Source File: wheel_loopback.py From ROS-Programming-Building-Powerful-Robots with MIT License | 6 votes |
def spin(self): ############################################### r = rospy.Rate self.secs_since_target = self.timeout_secs self.then = rospy.Time.now() self.latest_msg_time = rospy.Time.now() rospy.loginfo("-D- spinning") ###### main loop ######### while not rospy.is_shutdown(): while not rospy.is_shutdown() and self.secs_since_target < self.timeout_secs: self.spinOnce() r.sleep self.secs_since_target = rospy.Time.now() - self.latest_msg_time self.secs_since_target = self.secs_since_target.to_sec() #rospy.loginfo(" inside: secs_since_target: %0.3f" % self.secs_since_target) # it's been more than timeout_ticks since we recieved a message self.secs_since_target = rospy.Time.now() - self.latest_msg_time self.secs_since_target = self.secs_since_target.to_sec() # rospy.loginfo(" outside: secs_since_target: %0.3f" % self.secs_since_target) self.velocity = 0 r.sleep ###############################################
Example #19
Source File: pr2_target_policy.py From visual_dynamics with MIT License | 6 votes |
def main(): parser = argparse.ArgumentParser() parser.add_argument("--lr", default="r") args = parser.parse_args() rospy.init_node("pr2_target_policy") with open('config/environment/pr2.yaml') as yaml_string: pr2_env = utils.from_yaml(yaml_string) tool_link_name = "%s_gripper_tool_frame" % args.lr pol = policies.Pr2TargetPolicy(pr2_env, tool_link_name, np.zeros(3)) rate = rospy.Rate(30.0) while not rospy.is_shutdown(): state = pr2_env.get_state() target_state = pol.get_target_state() action = (target_state - state) / pr2_env.dt pr2_env.step(action) # pr2_env.reset(pol.get_target_state()) rate.sleep()
Example #20
Source File: joint_torque_springs.py From intera_sdk with Apache License 2.0 | 6 votes |
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()
Example #21
Source File: head_wobbler.py From intera_sdk with Apache License 2.0 | 6 votes |
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.")
Example #22
Source File: control_thread.py From flyingros with GNU General Public License v3.0 | 6 votes |
def sendSetpoint(): # Input data global setpoint, yawSetPoint, run, position_control # Output data local_setpoint_pub = rospy.Publisher('/mavros/setpoint_position/local', PoseStamped, queue_size=0) rate = rospy.Rate(5) while run: q = tf.transformations.quaternion_from_euler(0, 0, deg2radf(yawSetPoint), axes="sxyz") msg = PoseStamped() msg.header.stamp = rospy.Time.now() msg.pose.position.x = float(setpoint.x) msg.pose.position.y = float(setpoint.y) msg.pose.position.z = float(setpoint.z) msg.pose.orientation.x = q[0] msg.pose.orientation.y = q[1] msg.pose.orientation.z = q[2] msg.pose.orientation.w = q[3] local_setpoint_pub.publish(msg) rate.sleep()
Example #23
Source File: script.py From RAFCON with Eclipse Public License 1.0 | 6 votes |
def set_velocity(x, phi, turtle_name, logger): position_vector = Vector3(x, 0, 0) rotation_vector = Vector3(0, 0, phi) twist_msg = Twist(position_vector, rotation_vector) try: logger.info("move_to_position: publish twist to turtle".format(turtle_name)) turtle_vel_publisher = rospy.Publisher("/" + turtle_name + "/cmd_vel", Twist, queue_size=10, latch=True) turtle_vel_publisher.publish(twist_msg) rate = rospy.Rate(10) rate.sleep() except rospy.ROSInterruptException as e: logger.error("Failed to send a velocity command to turtle {}: {}".format(turtle_name, str(e)))
Example #24
Source File: script.py From RAFCON with Eclipse Public License 1.0 | 6 votes |
def execute(self, inputs, outputs, gvm): turtle_name = inputs["turtle_name"] x = inputs["x_pos"] y = inputs["y_pos"] phi = inputs["phi"] service = "/spawn" rospy.wait_for_service(service) spawn_turtle_service = rospy.ServiceProxy(service, Spawn) resp1 = spawn_turtle_service(x, y, phi, turtle_name) self.logger.info("ROS external module: executed the {} service".format(service)) turtle_pos_subscriber = rospy.Subscriber("/" + turtle_name + "/pose", Pose, check_turtle_pose) r = rospy.Rate(10) global pose_received # wait until the first pose message was received while not pose_received: r.sleep() return 0
Example #25
Source File: twist_to_motors.py From differential-drive with GNU General Public License v3.0 | 6 votes |
def spin(self): ############################################################# r = rospy.Rate(self.rate) idle = rospy.Rate(10) then = rospy.Time.now() self.ticks_since_target = self.timeout_ticks ###### main loop ###### while not rospy.is_shutdown(): while not rospy.is_shutdown() and self.ticks_since_target < self.timeout_ticks: self.spinOnce() r.sleep() idle.sleep() #############################################################
Example #26
Source File: cmdControl.py From Cherry-Autonomous-Racecar with MIT License | 5 votes |
def __init__(self): self.throttleInitialized = False self.joy_timeout = 2.0 self.lid_timeout = 0.30 self.cnn_timeout = 0.1 self.joy_time = time.time() self.lid_time = time.time() self.cnn_time = time.time() self.controller = XBox360() self.joy_cmd = TwistStamped() self.lid_cmd = TwistStamped() self.cnn_cmd = TwistStamped() self.cruiseControl = False self.cruiseThrottle = 0.5 self.steeringAngle = 0.5 self.throttle = 0.5 self.trim = 0.0 ##self.throttleLock = Lock() print "cmd_control" rospy.Subscriber("/imu", Imu, self.imuCB, queue_size=5) rospy.Subscriber("/lidar_twist", TwistStamped, self.lidarTwistCB, queue_size=5) rospy.Subscriber("/neural_twist", TwistStamped, self.neuralTwistCB, queue_size=5) rospy.Subscriber("/joy", Joy, self.joyCB, queue_size=5) self.vel_pub = rospy.Publisher("/cmd_vel", TwistStamped, queue_size = 1) #self.state_pub = rospy.Publisher("/joint_states", JointState, queue_size=1) self.sound = rospy.Publisher("/sound_server/speech_synth", String, queue_size=1) rospy.init_node ('cmd_control',anonymous=True) rate = rospy.Rate(66) while not rospy.is_shutdown(): self.cmdRouter() rate.sleep()
Example #27
Source File: control_simplepub.py From usv_sim_lsa with Apache License 2.0 | 5 votes |
def talker(): pub = rospy.Publisher('/barco_auv/thruster_command', JointState, queue_size=10) rospy.init_node('ctrl_jointState_publisher', anonymous=True) rate = rospy.Rate(10) # 10hz hello_str = JointState() hello_str.header = Header() hello_str.name = ['fwd_left'] hello_str.position = [10] hello_str.velocity = [] hello_str.effort = [] while not rospy.is_shutdown(): rospy.loginfo(hello_str) pub.publish(hello_str) rate.sleep()
Example #28
Source File: cozmo_driver.py From cozmo_driver with Apache License 2.0 | 5 votes |
def run(self, update_rate=10): """ Publish data continuously with given rate. :type update_rate: int :param update_rate: The update rate. """ r = rospy.Rate(update_rate) while not rospy.is_shutdown(): self._publish_tf(update_rate) self._publish_image() self._publish_objects() self._publish_joint_state() self._publish_imu() self._publish_battery() self._publish_odometry() self._publish_diagnostics() # send message repeatedly to avoid idle mode. # This might cause low battery soon # TODO improve this! self._cozmo.drive_wheels(*self._wheel_vel) # sleep r.sleep() # stop events on cozmo self._cozmo.stop_all_motors()
Example #29
Source File: tasks.py From flyingros with GNU General Public License v3.0 | 5 votes |
def __init__(self, name, sleep = 5): self.sleep = sleep self.rate = rospy.Rate(1.0/sleep) super(init_UAV, self).__init__("init_UAV", name)
Example #30
Source File: joint_velocity_wobbler.py From intera_sdk with Apache License 2.0 | 5 votes |
def wobble(self): self.set_neutral() """ Performs the wobbling of both arms. """ rate = rospy.Rate(self._rate) start = rospy.Time.now() def make_v_func(): """ returns a randomly parameterized cosine function to control a specific joint. """ period_factor = random.uniform(0.3, 0.5) amplitude_factor = random.uniform(0.1, 0.2) def v_func(elapsed): w = period_factor * elapsed.to_sec() return amplitude_factor * math.cos(w * 2 * math.pi) return v_func v_funcs = [make_v_func() for _ in self._right_joint_names] def make_cmd(joint_names, elapsed): return dict([(joint, v_funcs[i](elapsed)) for i, joint in enumerate(joint_names)]) print("Wobbling. Press Ctrl-C to stop...") while not rospy.is_shutdown(): self._pub_rate.publish(self._rate) elapsed = rospy.Time.now() - start cmd = make_cmd(self._right_joint_names, elapsed) self._right_arm.set_joint_velocities(cmd) rate.sleep()