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)
python类Float32()的实例源码
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 _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) + (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)
