我们从Python开源项目中，提取了以下32个代码示例，用于说明如何使用std_msgs.msg.Float32()。
def pub_sub_local_legacy(self): self.pub["motor"] = rospy.Publisher("/motor", Float32MultiArray) # learning signals # FIXME: change these names to /learner/... self.pub["learn_dw"] = rospy.Publisher("/learner/dw", Float32MultiArray) self.pub["learn_w"] = rospy.Publisher("/learner/w", Float32MultiArray) self.pub["learn_perf"] = rospy.Publisher("/learner/perf", reservoir) self.pub["learn_perf_lp"] = rospy.Publisher("/learner/perf_lp", reservoir) # learning control self.sub["ctrl_eta"] = rospy.Subscriber("/learner/ctrl/eta", Float32, self.sub_cb_ctrl) self.sub["ctrl_theta"] = rospy.Subscriber("/learner/ctrl/theta", Float32, self.sub_cb_ctrl) self.sub["ctrl_target"] = rospy.Subscriber("/learner/ctrl/target", Float32, self.sub_cb_ctrl) # state self.pub["learn_x_raw"] = rospy.Publisher("/learner/x_raw", reservoir) self.pub["learn_x"] = rospy.Publisher("/learner/x", reservoir) self.pub["learn_r"] = rospy.Publisher("/learner/r", reservoir) self.pub["learn_y"] = rospy.Publisher("/learner/y", reservoir)
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.topics = { "ball": {"topic": "environment/ball", "sub": None, "data": CircularState(), "type": CircularState}, "light": {"topic": "environment/light", "sub": None, "data": UInt8(), "type": UInt8}, "sound": {"topic": "environment/sound", "sub": None, "data": Float32(), "type": Float32}, "ergo_state": {"topic": "ergo/state", "sub": None, "data": CircularState(), "type": CircularState}, "joy1": {"topic": "sensors/joystick/1", "sub": None, "data": Joy(), "type": Joy}, "joy2": {"topic": "sensors/joystick/2", "sub": None, "data": Joy(), "type": Joy}, "torso_l_j": {"topic": "{}/joint_state".format("poppy_torso"), "sub": None, "data": JointState(), "type": JointState}, "torso_l_eef": {"topic": "{}/left_arm/end_effector_pose".format("poppy_torso"), "sub": None, "data": PoseStamped(), "type": PoseStamped}, "torso_r_eef": {"topic": "{}/right_arm/end_effector_pose".format("poppy_torso"), "sub": None, "data": PoseStamped(), "type": PoseStamped} } self.topics["ball"]["sub"] = rospy.Subscriber(self.topics["ball"]["topic"], self.topics["ball"]["type"], self.cb_ball) self.topics["light"]["sub"] = rospy.Subscriber(self.topics["light"]["topic"], self.topics["light"]["type"], self.cb_light) self.topics["sound"]["sub"] = rospy.Subscriber(self.topics["sound"]["topic"], self.topics["sound"]["type"], self.cb_sound) self.topics["ergo_state"]["sub"] = rospy.Subscriber(self.topics["ergo_state"]["topic"], self.topics["ergo_state"]["type"], self.cb_ergo) self.topics["joy1"]["sub"] = rospy.Subscriber(self.topics["joy1"]["topic"], self.topics["joy1"]["type"], self.cb_joy1) self.topics["joy2"]["sub"] = rospy.Subscriber(self.topics["joy2"]["topic"], self.topics["joy2"]["type"], self.cb_joy2) self.topics["torso_l_j"]["sub"] = rospy.Subscriber(self.topics["torso_l_j"]["topic"], self.topics["torso_l_j"]["type"], self.cb_torso_l_j) self.topics["torso_l_eef"]["sub"] = rospy.Subscriber(self.topics["torso_l_eef"]["topic"], self.topics["torso_l_eef"]["type"], self.cb_torso_l_eef) self.topics["torso_r_eef"]["sub"] = rospy.Subscriber(self.topics["torso_r_eef"]["topic"], self.topics["torso_r_eef"]["type"], self.cb_torso_r_eef)
def __init__(self): self.petrone = Petrone() # subscriber self.sub_flight = rospy.Subscriber('cmd_fly', Int8, self.cb_fly, queue_size=1) self.sub_cmd = rospy.Subscriber('cmd_vel_raw', Twist, self.cb_cmd, queue_size=1) self.sub_color = rospy.Subscriber('led_color', String, self.cb_color, queue_size=1) # publisher self.pub_battery = rospy.Publisher('battery', Float32, queue_size=1) self.pub_status_flight = rospy.Publisher('status/flight', Int8, queue_size=1) self.pub_status_system = rospy.Publisher('status/system', Int8, queue_size=1) self.pub_imu = rospy.Publisher('imu', Imu, queue_size=1) # cache self.is_disconnected = True self.last_values = defaultdict(lambda: 0) # flight parameters self.twist = Twist() self.twist_at = 0
def __init__(self): self.rate = rospy.get_param('~rate', 100.0) self.imu_name = rospy.get_param('~imu_name', 'imu_steer') self.topic_name = rospy.get_param('~topic_name', 'topic_name') self.roll = 0.0 self.pitch = 0.0 self.yaw = 0.0 self.pub_imu_roll_msg = Float32() self.pub_imu_pitch_msg = Float32() self.pub_imu_yaw_msg = Float32() self.pub_imu_roll = rospy.Publisher('/' + self.imu_name +'/roll', Float32, queue_size=1) self.pub_imu_pitch = rospy.Publisher('/' + self.imu_name +'/pitch', Float32, queue_size=1) self.pub_imu_yaw = rospy.Publisher('/' + self.imu_name +'/yaw', Float32, queue_size=1) self.sub = rospy.Subscriber(self.topic_name, Imu, self.process_imu_message, queue_size=1) rate = rospy.Rate(self.rate) while not rospy.is_shutdown(): rate.sleep()
def publish_angles(self): if self.enable_reference and self.enable_angle: self.pub_imu_roll_msg = Float32() self.pub_imu_roll_msg.data = self.roll_frame - self.roll_steer self.pub_imu_roll.publish(self.pub_imu_roll_msg) self.pub_imu_pitch_msg = Float32() self.pub_imu_pitch_msg.data = self.pitch_frame - self.pitch_steer self.pub_imu_pitch.publish(self.pub_imu_pitch_msg) self.pub_imu_yaw_msg = Float32() self.pub_imu_yaw_msg.data = self.yaw_frame - self.yaw_steer self.pub_imu_yaw.publish(self.pub_imu_yaw_msg) # Main function.
def speed_converter(): rospy.init_node('wheel_speed', anonymous=True) pub1 = rospy.Publisher('lwheel_vtarget', Float32, queue_size=10) pub2 = rospy.Publisher('rwheel_vtarget', Float32, queue_size=10) rate = rospy.Rate(10) while not rospy.is_shutdown(): rospy.Subscriber('cmd_vel', Twist, callback) s1 = "The left wheel's target speed is %f m/s." % lv s2 = "The right wheel's target speed is %f m/s." % rv rospy.loginfo(s1) rospy.loginfo(s2) pub1.publish(lv) pub2.publish(rv) rate.sleep()
def publish(): header = Header() header.stamp = rospy.Time.now() header.frame_id = 'map' for role, robot_id in WorldModel.assignments.items(): if robot_id is not None: command = WorldModel.commands[role] if command.velocity_control_enable: msg = TwistStamped() msg.header = header msg.twist = command.target_velocity pubs_velocity[robot_id].publish(msg) else: send_pose = WorldModel.tf_listener.transformPose("map",command.target_pose) send_pose.header.stamp = rospy.Time.now() pubs_position[robot_id].publish(send_pose) pubs_kick_velocity[robot_id].publish(Float32(command.kick_power)) status = AIStatus() # TODO(Asit) use navigation_enable instead avoidBall. status.avoidBall = command.navigation_enable status.do_chip = command.chip_enable status.dribble_power = command.dribble_power pubs_ai_status[robot_id].publish(status) command.reset_adjustments()
def __init__(self, limb = "right"): # control parameters self._rate = 1000.0 # Hz self._missed_cmds = 20.0 # Missed cycles before triggering timeout # create our limb instance self._limb = intera_interface.Limb(limb) # initialize parameters self._springs = dict() self._damping = dict() self._des_angles = dict() # create cuff disable publisher cuff_ns = 'robot/limb/' + limb + '/suppress_cuff_interaction' self._pub_cuff_disable = rospy.Publisher(cuff_ns, Empty, queue_size=1) # verify robot is enabled print("Getting robot state... ") self._rs = intera_interface.RobotEnable(CHECK_VERSION) self._init_state = self._rs.state().enabled print("Enabling robot... ") self._rs.enable() print("Running. Ctrl-c to quit") rospy.Subscriber("desired_joint_pos", JointState, self._set_des_pos) rospy.Subscriber("release_spring", Float32, self._release) rospy.Subscriber("imp_ctrl_active", Int64, self._imp_ctrl_active) self.max_stiffness = 20 self.time_to_maxstiffness = .3 ######### 0.68 self.t_release = rospy.get_time() self._imp_ctrl_is_active = True for joint in self._limb.joint_names(): self._springs[joint] = 30 self._damping[joint] = 4
def run(self): rospy.Subscriber("apex_playground/environment/sound", Float32, self.cb_sound) rospy.spin() self.stream.stop_stream() self.stream.close() self.p.terminate()
def update_sound(self, state): self.sound_pub.publish(Float32(self.conversions.ball_to_sound(state))) # TODO rescale
def cb_get_interests(self, request): interests_array = self.learning.get_normalized_interests_evolution() interests = GetInterestsResponse() if self.learning is not None: interests.names = self.learning.get_space_names() interests.num_iterations = UInt32(len(interests_array)) interests.interests = [Float32(val) for val in interests_array.flatten()] return interests
def run_gripper_driver(): # init moveit commander moveit_commander.roscpp_initialize(sys.argv) # specify move group arm_group = moveit_commander.MoveGroupCommander("arm") gripper_group = moveit_commander.MoveGroupCommander("gripper") # init publisher arm_pub = rospy.Publisher('servo_write', JointStatus, queue_size=1) gripper_pub = rospy.Publisher('gripper_write', Float32, queue_size=1) # 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(arm_group, arm_pub) init_gripper(gripper_group, gripper_pub) # set ros publisher rate, 10hz = 10 seconds for a circle rate = rospy.Rate(100) # set position close_gripper = {"finger_joint1": 0, "finger_joint2": 0} open_gripper = {"finger_joint1": 0.015, "finger_joint2": -0.015} switch = True while True: if switch: plan_msg = gripper_group.plan(joints=open_gripper) else: plan_msg = gripper_group.plan(joints=close_gripper) switch = not switch gripper_group.execute(plan_msg=plan_msg, wait=False) # servo talker gripper_talker(gripper_pub, plan_msg, rate) rospy.sleep(5) if is_exit: break # shutdown moveit commander moveit_commander.roscpp_shutdown()
def __init__(self): self.joint_status = UInt16MultiArray() self.gripper_status = Float32() self.is_exit = False self.run_real = False # init moveit commander moveit_commander.roscpp_initialize(sys.argv) # specify move group self.arm_group = moveit_commander.MoveGroupCommander("arm") self.gripper_group = moveit_commander.MoveGroupCommander("gripper") # init publisher self.arm_pub = rospy.Publisher('servo_write', UInt16MultiArray, queue_size=10) self.gripper_pub = rospy.Publisher('gripper_write', Float32, queue_size=10) # init ros node rospy.init_node('real_servo_driver', anonymous=True) # init robot and scene self.robot = moveit_commander.RobotCommander() self.scene = moveit_commander.PlanningSceneInterface() # set ros publisher rate, 10hz = 10 seconds for a circle self.rate = rospy.Rate(50) print "============ Waiting for RVIZ..." rospy.sleep(2) # move grasper to init position self.init_position() self.init_gripper()
def _init_motor_pubs(self): self._motor_pubs = [] for i in range(12): if i % 2 == 0: bbb, board_id, sub_index = i + 2, 0x71, 0x2 else: bbb, board_id, sub_index = i + 1, 0x1, 0x1 self._motor_pubs.append( rospy.Publisher('/bbb%d/0x%x_0x2040_0x%x' % (bbb, board_id, sub_index), Float32, queue_size=1))
def _set_motor_positions(self, pos): gain = 1 / 0.009 msg = Float32() if not USE_F32: msg.header.stamp = rospy.rostime.get_rostime() if (pos - 0.95).all() and self._hyperparams['constraint']: pos = self._constraint.find_nearest_valid_values(pos) for i in range(12): msg.data = min(max(7.5, ((0.95 - pos[i]) * gain)), 40) self._motor_pubs[i].publish(msg)
def _set_motor_velocities(self, vel): pos = self._sensor_readings[MOTOR_POSITIONS].copy() pos += vel * self._hyperparams['dt'] msg = Float32() if not USE_F32: msg.header.stamp = rospy.rostime.get_rostime() if self._hyperparams['constraint']: pos = self._constraint.find_nearest_valid_values(pos) for i in range(12): msg.data = min(max(0, ((0.95 - pos[i]) / 0.009)), 45) self._motor_pubs[i].publish(msg)
def _set_motor_positions(self, pos): gain = 1 / 0.009 msg = Float32() if not USE_F32: msg.header.stamp = rospy.rostime.get_rostime() if (pos - 0.95).all() and self._hyperparams['constraint']: pos = self._constraint.find_nearest_valid_values(pos) for i in range(12): msg.data = min(max(7.5, ((0.95 - pos[i]) * gain) + (7.5 / gain)), 40) self._motor_pubs[i].publish(msg)
def __init__(self): self.pub = {} self.pub["red"] = rospy.Publisher("/robot/sonar/head_sonar/lights/set_red_level",Float32,queue_size = 1) self.pub["green"] = rospy.Publisher("/robot/sonar/head_sonar/lights/set_green_level",Float32,queue_size = 1) self.post = Post(self)
def setLed(self,color, intensity): """ Set an intensity value for the halo led :param color: Color of the led (red, green) :type color: str :param intensity: Float value for the intensity between 0.0 and 100.0 :type intensity: float """ try: self.pub[color].publish(Float32(intensity)) except Exception,e: rospy.logwarn("%s",str(e))
def __init__(self, joy_topic): rospy.loginfo("waiting for petrone") rospy.wait_for_message('battery', Float32) rospy.loginfo("found petrone") # fly control publisher self.pub_fly = rospy.Publisher('cmd_fly', Int8, queue_size=1) self.pub_led = rospy.Publisher('led_color', String, queue_size=1) # subscribe to the joystick at the end to make sure that all required # services were found self._buttons = None rospy.Subscriber(joy_topic, Joy, self.cb_joy)
def publish_angles(self): self.pub_imu_roll_msg = Float32() self.pub_imu_roll_msg.data = self.roll self.pub_imu_roll.publish(self.pub_imu_roll_msg) self.pub_imu_pitch_msg = Float32() self.pub_imu_pitch_msg.data = self.pitch self.pub_imu_pitch.publish(self.pub_imu_pitch_msg) self.pub_imu_yaw_msg = Float32() self.pub_imu_yaw_msg.data = self.yaw self.pub_imu_yaw.publish(self.pub_imu_yaw_msg) # Main function.
def __init__(self): self.rate = rospy.get_param('~rate', 100.0) self.topic_name_angle = rospy.get_param('~topic_name_angle', 'topic_name_angle') self.topic_name_reference = rospy.get_param('~topic_name_reference', 'topic_name_reference') self.imu_name = rospy.get_param('~imu_name', 'imu_steer') self.roll_steer = 0.0 self.pitch_steer = 0.0 self.yaw_steer = 0.0 self.roll_frame = 0.0 self.pitch_frame = 0.0 self.yaw_frame = 0.0 self.enable_reference = False self.enable_angle = False self.pub_imu_roll_msg = Float32() self.pub_imu_pitch_msg = Float32() self.pub_imu_yaw_msg = Float32() self.pub_imu_roll = rospy.Publisher('/' + self.imu_name +'/roll', Float32, queue_size=1) self.pub_imu_pitch = rospy.Publisher('/' + self.imu_name +'/pitch', Float32, queue_size=1) self.pub_imu_yaw = rospy.Publisher('/' + self.imu_name +'/yaw', Float32, queue_size=1) self.sub = rospy.Subscriber(self.topic_name_angle, Imu, self.process_imu_message_angle, queue_size=1) self.sub = rospy.Subscriber(self.topic_name_reference, Imu, self.process_imu_message_reference, queue_size=1) rate = rospy.Rate(self.rate) while not rospy.is_shutdown(): rate.sleep()
def __init__(self): #Loads the robot model, which contains the robot's kinematics information self.robot = URDF.from_parameter_server() #Subscribes to information about what the current joint values are. rospy.Subscriber("/joint_states", JointState, self.joint_callback) #Subscribes to command for end-effector pose rospy.Subscriber("/cartesian_command", Transform, self.command_callback) #Subscribes to command for redundant dof rospy.Subscriber("/redundancy_command", Float32, self.redundancy_callback) # Publishes desired joint velocities self.pub_vel = rospy.Publisher("/joint_velocities", JointState, queue_size=1) #This is where we hold the most recent joint transforms self.joint_transforms = [] self.q_current = [] self.x_current = tf.transformations.identity_matrix() self.R_base = tf.transformations.identity_matrix() self.x_target = tf.transformations.identity_matrix() self.q0_desired = 0 self.last_command_time = 0 self.last_red_command_time = 0 # Initialize timer that will trigger callbacks self.mutex = Lock() self.timer = rospy.Timer(rospy.Duration(0.1), self.timer_callback)
def __init__(self): #Loads the robot model, which contains the robot's kinematics information self.robot = URDF.from_parameter_server() # Publishes Cartesian goals self.pub_command = rospy.Publisher("/cartesian_command", geometry_msgs.msg.Transform, queue_size=1) # Publishes Redundancy goals self.pub_red = rospy.Publisher("/redundancy_command", Float32, queue_size=1) # Publisher to set robot position self.pub_reset = rospy.Publisher("/joint_command", JointState, queue_size=1) #This is where we hold the most recent joint transforms self.joint_transforms = [] self.x_current = tf.transformations.identity_matrix() self.R_base = tf.transformations.identity_matrix() #Create "Interactive Marker" that we can manipulate in RViz self.init_marker() self.ee_tracking = 0 self.red_tracking = 0 self.q_current = [] self.x_target = tf.transformations.identity_matrix() self.q0_desired = 0 self.mutex = Lock() self.timer = rospy.Timer(rospy.Duration(0.3), self.timer_callback) #Subscribes to information about what the current joint values are. rospy.Subscriber("joint_states", JointState, self.joint_callback) #Resets the robot to a known pose
def timer_callback(self, event): if self.ee_tracking: msg = JointState() self.mutex.acquire() msg = convert_to_trans_message(self.x_target) self.pub_command.publish(msg) self.mutex.release() if self.red_tracking: red = Float32() self.mutex.acquire() red.data = self.q0_desired self.pub_red.publish(red) self.mutex.release()
def __init__(self): # Initialize the node rospy.init_node('pid_control', anonymous=True) # Set varibles self.l_enc = 0.0 self.l_enc_prev = 0.0 self.lwheel_number = 0 self.lwheel_dis = 0.0 self.r_enc = 0.0 self.r_enc_prev = 0.0 self.rwheel_number = 0 self.rwheel_dis = 0.0 self.l_then = rospy.Time.now() self.lwheel_dis_prev = 0.0 self.r_then = rospy.Time.now() self.rwheel_dis_prev = 0.0 self.l_error_int = 0.0 self.l_pid_duration_prev = rospy.Time.now() self.lwheel_vel = 0.0 self.l_error_int = 0.0 self.l_target = 0.0 self.r_pid_duration_prev = rospy.Time.now() self.rwheel_vel = 0.0 self.r_error_int = 0.0 self.r_target = 0.0 # Set parameters self.kp = rospy.get_param('Kp',500) self.ki = rospy.get_param('ki',200) self.kd = rospy.get_param('kd',10) self.rate = rospy.get_param('rate', 10) self.ticks_per_meter = rospy.get_param('ticks_per_meter', 4685) #4685 self.encoder_min = rospy.get_param('encoder_min', -4294967296) self.encoder_max = rospy.get_param('encoder_max', +4294967296) self.encoder_low_wrap = rospy.get_param('encoder_low_wrap', 0.2*(self.encoder_max-self.encoder_min)+self.encoder_min) self.encoder_high_wrap = rospy.get_param('encode_high_wrap', 0.8*(self.encoder_max-self.encoder_max)+self.encoder_max) self.control_max = rospy.get_param('out_max', 255) self.control_min = rospy.get_param('out_min', -255) # Subscrption and publishment message rospy.Subscriber('lwheel_vtarget', Float32, self.l_pid_func) rospy.Subscriber('rwheel_vtarget', Float32, self.r_pid_func) rospy.Subscriber('lwheel', Int64, self.l_distance) rospy.Subscriber('rwheel', Int64, self.r_distance) rospy.Subscriber('str', String, self.stop) self.l_speed_pub = rospy.Publisher('lwheel_vel', Float32, queue_size=10) self.r_speed_pub = rospy.Publisher('rwheel_vel', Float32, queue_size=10) self.l_control_pub = rospy.Publisher('left_wheel_control', Float32, queue_size=10) self.r_control_pub = rospy.Publisher('right_wheel_control', Float32, queue_size=10) # Calculate the odom accroding to the encoder data from launchpad_node
def __init__(self): rospy.init_node('sec_control', anonymous=True) # Set varibles self.l_enc = 0.0 self.l_enc_prev = 0.0 self.lwheel_number = 0 self.lwheel_dis = 0.0 self.r_enc = 0.0 self.r_enc_prev = 0.0 self.rwheel_number = 0 self.rwheel_dis = 0.0 self.l_then = rospy.Time.now() self.lwheel_dis_prev = 0.0 self.r_then = rospy.Time.now() self.rwheel_dis_prev = 0.0 self.l_error_int = 0.0 self.l_pid_duration_prev = rospy.Time.now() self.lwheel_vel = 0.0 self.r_error_int = 0.0 self.r_pid_duration_prev = rospy.Time.now() self.rwheel_vel = 0.0 self.r_error_int = 0.0 self.lwheel_control = 0 self.rwheel_control = 0 self.l_number = 0 self.r_number = 0 self.l_flag = 1 self.r_flag = 1 self.left = (59.42, 61.05, 63.05, 65.78, 67.75, 69.16, 70.31, 71.63, 73.13, 74.15, 75.22, 76.50, 79.19, 80.00, 80.73, 82.18, 83.63, 84.82, 86.19, 87.90, 88.92, 89.05, 89.86, 91.31, 92.38, 93.83, 94.94, 95.58, 96.52, 97.89, 96.82, 99.04, 99.59, 100.49, 102.28, 103.27, 104.72, 106.13, 106.04, 107.07, 108.22, 108.52, 108.30, 110.61, 110.27, 110.27, 111.38, 111.85, 113.98, 114.24, 112.96, 112.70, 113.55, 116.37, 117.57, 118.76, 117.14, 117.74, 118.89, 120.38, 121.32, 122.09, 122.90, 123.16, 124.31, 125.04, 125.55, 126.02, 125.55, 125.34, 126.40, 126.92, 128.41, 128.11, 128.41, 129.52, 129.69, 130.16, 130.42, 131.31, 131.48, 132.51, 134.00, 133.40, 134.60, 134.77, 135.75, 136.05, 135.80, 136.39, 136.22, 136.31, 136.56, 136.99, 138.06, 138.36, 139.08, 139.09, 140.06, 141.13, 141.55, 141.62, 141.88, 141.94, 141.52, 141.13, 142.37, 143.56, 142.97, 142.97, 143.78, 144.12, 144.97, 145.57, 145.78, 145.66, 145.87, 146.21, 146.30, 147.07, 147.28, 147.58, 147.53, 148.09, 148.35, 148.73, 148.39, 148.86, 149.46, 149.28, 150.01, 150.22, 150.35, 151.12, 151.59, 151.63, 152.06, 152.15, 152.74, 152.36, 152.66, 152.79, 153.51, 153.77, 154.11, 153.68, 154.19, 154.71, 155.60, 155.60, 155.77, 155.99, 156.20, 156.41, 156.67, 156.41, 156.50, 156.54, 156.20, 156.97, 157.10, 157.52, 157.61, 157.87, 158.34, 158.93, 159.02, 159.74, 159.74, 159.96, 160.34, 160.43, 160.51, 160.60, 160.98, 160.94, 160.30, 160.64, 160.94, 161.41, 162.69, 164.31, 165.12) #the left motor doesn't rotate steadily until pwm = 72, and corresponding speed is 59.42mm/s. self.right = (59.89, 69.07, 69.20, 69.71, 70.57, 73.85, 76.07, 78.25, 79.87, 80.81, 81.92, 83.37, 84.70, 85.68, 86.66, 89.61, 90.59, 92.42, 93.40, 94.47, 96.05, 98.48, 99.77, 101.30, 101.60, 102.97, 103.78, 104.50, 106.04, 107.62, 108.22, 107.32, 109.11, 110.74, 110.69, 111.63, 113.17, 112.96, 111.97, 112.74, 113.77, 115.05, 115.47, 116.37, 118.16, 118.46, 117.82, 118.72, 120.26, 120.55, 121.02, 120.90, 122.35, 122.52, 123.80, 124.06, 124.35, 125.21, 125.98, 127.39, 127.51, 127.17, 128.11, 128.62, 128.84, 129.73, 130.07, 130.16, 129.73, 130.80, 131.14, 131.87, 131.87, 132.21, 132.72, 133.66, 133.87, 135.11, 134.94, 135.24, 135.41, 136.44, 136.99, 136.39, 136.99, 136.99, 137.59, 137.59, 138.44, 138.14, 137.80, 138.19, 138.61, 139.08, 139.12, 139.72, 139.89, 140.02, 139.94, 140.36, 140.53, 141.17, 141.56, 141.81, 142.37, 142.92, 143.95, 144.72, 145.23, 145.36, 145.31, 145.83, 145.96, 146.30, 146.94, 147.71, 147.79, 148.26, 147.96, 148.18, 148.05, 148.86, 148.43, 148.64, 149.11, 149.46, 149.50, 149.97, 150.18, 150.65, 150.95, 150.95, 151.50, 151.63, 151.80, 151.97, 152.32, 152.53, 152.02, 151.72, 152.15, 152.19, 151.80, 151.80, 152.87, 153.60, 153.04, 153.90, 153.64, 153.90, 153.72, 153.72, 153.98, 154.49, 154.75, 154.15, 154.41, 151.55, 151.55, 153.68, 153.68, 155.09, 155.05, 155.22, 155.65, 156.07, 156.80, 156.46, 156.07, 156.63, 156.97, 157.31, 157.27, 157.01, 157.69, 157.95, 157.31, 157.31, 157.27, 157.10, 157.48, 157.10, 157.57, 158.46, 159.70, 160.51, 160.77) #the right motor doesn't rotate steadily until pwm = 68, and corresponding speed is 58.89mm/s. self.rate = rospy.get_param('rate', 10) self.vel_threshold = rospy.get_param('~vel_threshold', 0.001) self.ticks_per_meter = rospy.get_param('ticks_per_meter', 4685) self.encoder_min = rospy.get_param('encoder_min', -4294967296) self.encoder_max = rospy.get_param('encoder_max', +4294967296) self.encoder_low_wrap = rospy.get_param('encoder_low_wrap', 0.2*(self.encoder_max-self.encoder_min)+self.encoder_min) self.encoder_high_wrap = rospy.get_param('encode_high_wrap', 0.8*(self.encoder_max-self.encoder_max)+self.encoder_max) rospy.Subscriber('lwheel_vtarget', Float32, self.l_sec_func, queue_size=1) rospy.Subscriber('rwheel_vtarget', Float32, self.r_sec_func, queue_size=1) rospy.Subscriber('lwheel', Int64, self.l_distance, queue_size=1) rospy.Subscriber('rwheel', Int64, self.r_distance, queue_size=1) rospy.Subscriber('str', String, self.stop) self.l_speed_pub = rospy.Publisher('lwheel_vel', Float32, queue_size=10) self.r_speed_pub = rospy.Publisher('rwheel_vel', Float32, queue_size=10) self.l_control_pub = rospy.Publisher('left_wheel_control', Float32, queue_size=10) self.r_control_pub = rospy.Publisher('right_wheel_control', Float32, queue_size=10)
def run(self): self.start_simulation() rospy.Service('vrep/reset_ball', Empty, self.cb_reset_ball) try: self.reset_ball() while not rospy.is_shutdown(): self.joints.update() self.objects.update() joints = self.joints.get() def map_joy(x): h_joy = 2. return min(max(h_joy*sin(x), -1), 1) x = map_joy(joints[self.joystick_left_joints[0]]['position']) y = map_joy(joints[self.joystick_left_joints[1]]['position']) # Publishers self.publish_joy(x, y, self.joy_pub) x = map_joy(joints[self.joystick_right_joints[0]]['position']) y = map_joy(joints[self.joystick_right_joints[1]]['position']) self.publish_joy(x, y, self.joy_pub2) objects = self.objects.get() if 'position' in objects[self.ball_name] and 'position' in objects[self.arena_name]: ring_radius = self.env_params['tracking']['arena']['radius'] / self.env_params['tracking']['ring_divider'] # There is no visual tracking so the true arena diameter is assumed ball_state = self.conversions.get_state(objects[self.ball_name]['position'], # Although returned by V-Rep, dimension "z" is ignored objects[self.arena_name]['position'], ring_radius) self.ball_pub.publish(ball_state) color = self.conversions.ball_to_color(ball_state) self.light_pub.publish(UInt8(color)) sound = self.conversions.ball_to_sound(ball_state) self.sound_pub.publish(Float32(sound)) distance = norm(array(objects[self.ball_name]['position']) - array(objects[self.arena_name]['position'])) if distance > 0.25: self.reset_ball() self.rate.sleep() finally: self.stop_simulation() sleep(0.5) vrep.simxFinish(self.simulation_id)
def __init__(self, max_random_start, observation_dims, data_format, display): self.max_random_start = max_random_start self.action_size = 28 self.display = display self.data_format = data_format self.observation_dims = observation_dims self.screen = np.zeros((self.observation_dims[0],self.observation_dims[1]), np.uint8) ##################### # Followings are not used yet self.syncLaser = False self.syncPose = False self.syncGoal = False self.syncDir = False self.sentTime = 0 ##################### self.poseX = 0.0 self.poseY = 0.0 self.prevPoseX = 0.0 self.prevPoseY = 0.0 self.goalX = 0.0 self.goalY = 0.0 self.dir = 0.0 self.prevDist = 0.0 self.terminal = 0 self.sendTerminal = 0 self.clock = Clock() self.lastClock = Clock() rospy.wait_for_service('reset_positions') self.resetStage = rospy.ServiceProxy('reset_positions', EmptySrv) # Publishers: self.pub_action_ = rospy.Publisher("dqn/selected_action",Int8,queue_size=1) self.pub_new_goal_ = rospy.Publisher("dqn/new_goal",EmptyMsg,queue_size=1) self.pub_rew_ = rospy.Publisher("dqn/lastreward",Float32,queue_size=1) # Subscribers: rospy.Subscriber('bridge/laser_image', Image, self.laserCB,queue_size=1) rospy.Subscriber('bridge/current_dir', Float32, self.directionCB,queue_size=1) rospy.Subscriber('bridge/impact', EmptyMsg, self.impactCB,queue_size=1) rospy.Subscriber('bridge/current_pose', Pose, self.positionCB,queue_size=1) rospy.Subscriber('bridge/goal_pose', Pose, self.targetCB,queue_size=1) rospy.Subscriber('clock', Clock, self.stepCB,queue_size=1)
def step(self, action, is_training=False): self.prevPoseX = self.poseX self.prevPoseY = self.poseY if action == -1: # Step with random action action = int(random.random()*(self.action_size)) msg = Int8() msg.data = action self.pub_action_.publish( msg) if self.display: cv2.imshow("Screen", self.screen) #cv2.waitKey(9) dist = (self.poseX - self.goalX)**2 + (self.poseY - self.goalY)**2 reward = (self.prevDist - dist)/10.0 self.prevDist = dist if self.terminal == 1: reward -= 900 #self.new_random_game() if dist < 0.9: reward += 300 newStateMSG = EmptyMsg() self.pub_new_goal_.publish( newStateMSG) # cv2.waitKey(30) # Add whatever info you want info = "" #rospy.loginfo("Episede ended, reward: %g", reward) while(self.clock == self.lastClock): pass self.lastClock = self.clock if self.terminal == 2: self.sendTerminal = 1 if self.terminal == 1: # rewd = Float32() # rewd.data = reward # self.pub_rew_.publish( rewd) self.terminal = 2 return self.screen, reward, self.sendTerminal, info #observation, reward, terminal, info = self.env.step(action) #return self.preprocess(observation), reward, terminal, info
