def magnetic_field_callback(self, magnetometer_msg):
# Correct magnetic filed
raw_mag = np.array([magnetometer_msg.vector.x,
magnetometer_msg.vector.y,
magnetometer_msg.vector.z])
# corrected_mag = compensation * (raw_mag - offset)
corrected_mag = np.dot(self._calibration_compensation,
raw_mag - self._calibration_offset)
# compute yaw angle using corrected magnetometer measurements
# and ASSUMING ZERO pitch and roll of the magnetic sensor!
# adapted from
# https://github.com/KristofRobot/razor_imu_9dof/blob/indigo-devel/src/Razor_AHRS/Compass.ino
corrected_mag = corrected_mag / np.linalg.norm(corrected_mag)
mag_bearing = math.atan2(corrected_mag[1], -corrected_mag[0])
# add declination and constant bearing offset
mag_bearing = mag_bearing + self._declination + self._bearing_offset
# publish unfiltered bearing, degrees only for debug purposes
self._pub_bearing_raw.publish(Float64(math.degrees(mag_bearing)))
# compute mean
self._latest_bearings[self._num_magnetometer_reads] = mag_bearing
self._num_magnetometer_reads += 1
if self._num_magnetometer_reads >= self._number_samples_average:
self._num_magnetometer_reads = 0 # delete oldest samples
self._received_enough_samples = True
if self._received_enough_samples:
bearing_avg = self.angular_mean(self._latest_bearings)
else:
# not enough samples, use latest value
bearing_avg = mag_bearing
# WARNING: we assume zero roll and zero pitch!
q_avg = tf.quaternion_from_euler(0.0, 0.0, bearing_avg);
imu_msg = Imu()
imu_msg.orientation.w = q_avg[3]
imu_msg.orientation.x = q_avg[0]
imu_msg.orientation.y = q_avg[1]
imu_msg.orientation.z = q_avg[2]
self._pub_bearing_avg.publish(Float64(math.degrees(bearing_avg)))
self._pub_imu_bearing_avg.publish(imu_msg)
# debug
if self._verbose:
rospy.loginfo("bearing_avg : " +
str(math.degrees(bearing_avg)) + " deg")
mag_corrected_msg = magnetometer_msg
mag_corrected_msg.vector.x = corrected_mag[0]
mag_corrected_msg.vector.y = corrected_mag[1]
mag_corrected_msg.vector.z = corrected_mag[2]
self._pub_mag_corrected.publish(mag_corrected_msg)
python类Float64()的实例源码
def __init__(self):
self.recording = False
self.names = ('sair', 'sail', 'fai', 'faii')
self.data = {n: [] for n in self.names}
# TODO hardcoded for left arm
# self.acc_sub = rospy.Subscriber(
# '/robot/accelerometer/left_accelerometer/state',
# Imu,
# self.handle_acc)
self.sensor_sub = rospy.Subscriber(
'/sensor_values',
Int32MultiArray,
self.handle_sensor)
# self.kb_sub = rospy.Subscriber('/keyboard/keyup',
# Key,
# self.keyboard_cb, queue_size=10)
# Visualising the below pulished signals in rqt_plot is recommended
self.sai_pub = rospy.Publisher(
'/finger_sensor/sai',
FingerSAI,
queue_size=5)
self.fai_pub = rospy.Publisher(
'/finger_sensor/fai',
FingerFAI,
queue_size=5)
self.faii_pub = rospy.Publisher(
'/finger_sensor/faii',
Float64,
queue_size=5)
self.sensor_values = []
self.sensor_values = deque(maxlen=80)
self.acc_t = deque(maxlen=400)
self.acc = deque(maxlen=400)
# 0.66pi rad/sample (cutoff frequency over nyquist frequency
# (ie, half the sampling frequency)). For the wrist, 33 Hz /
# (100 Hz/ 2). TODO Double check arguments
self.b1, self.a1 = signal.butter(1, 0.66, 'high', analog=False)
# 0.5p rad/sample. For the tactile sensor, 5 Hz / (20 Hz / 2).
self.b, self.a = signal.butter(1, 0.5, 'high', analog=False)
def step(self, action, is_training):
if self.terminal == 0:
if action == -1:
# Step with random action
action = int(random.random()*(self.action_size))
self.actionToVel( action)
self.readyForNewData = True
if self.display:
cv2.imshow("Screen", self.screen)
cv2.waitKey(3)
dist = np.sqrt( (self.robotPose.position.x - self.goalPose.position.x)**2 + (self.robotPose.position.y - self.goalPose.position.y)**2)
#near obstacle penalty factor:
#nearObstPenalty = self.minFrontDist - 1.5
#reward = max( 0, ((self.prevDist - dist + 1.8)*3/dist)+min( 0, nearObstPenalty))
#self.prevDist = dist
reward = 0
if dist < 0.3:
reward += 1
#Select a new goal
theta = self.ang*random.random()
self.goalPose.position.x = self.r*np.cos( theta)
self.goalPose.position.y = self.r*np.sin( theta)
self.pub_goal_.publish( self.goalPose)
rwd = Float64()
rwd.data = 10101.963
self.pub_rew_.publish( rwd)
self.numWins += 1
if self.numWins == 99:
if self.ang < np.pi:
self.r += 1
self.ang += float(int(self.r/20)/10.0)
self.r %=20
self.nimWins = 0
self.resetStage()
# Add whatever info you want
info = ""
self.ep_reward += reward
if self.terminal == 1:
reward = -1
rewd = Float64()
rewd.data = self.ep_reward
self.pub_rew_.publish( rewd)
self.sendTerminal = 1
while( self.readyForNewData == True):
pass
if self.use_cumulated_reward:
return self.screen, self.ep_reward, self.sendTerminal, info
else:
return self.screen, reward, self.sendTerminal, info
#observation, reward, terminal, info = self.env.step(action)
#return self.preprocess(observation), reward, terminal, info
def __init__(self, max_random_start,
observation_dims, data_format, display, use_cumulated_reward):
self.max_random_start = max_random_start
self.action_size = 28
self.use_cumulated_reward = use_cumulated_reward
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)
#self.dirToTarget = 0
self.robotPose = Pose()
self.goalPose = Pose()
self.robotRot = 0.0
self.prevDist = 0.0
self.boom = False
self.numWins = 0
self.ep_reward = 0.0
self.terminal = 0
self.sendTerminal = 0
self.readyForNewData = True
rospy.wait_for_service('reset_positions')
self.resetStage = rospy.ServiceProxy('reset_positions', EmptySrv)
# Subscribers:
# rospy.Subscriber('base_scan', LaserScan, self.scanCB,queue_size=1)
# rospy.Subscriber('base_pose_ground_truth', Odometry, self.poseUpdateCB, queue_size=1)
# trying time sync:
sub_scan_ = message_filters.Subscriber('base_scan', LaserScan)
sub_pose_ = message_filters.Subscriber('base_pose_ground_truth', Odometry)
ts = message_filters.TimeSynchronizer( [sub_scan_, sub_pose_], 1)
ts.registerCallback( self.syncedCB)
# publishers:
self.pub_vel_ = rospy.Publisher('cmd_vel', Twist, queue_size = 1)
self.pub_rew_ = rospy.Publisher("lastreward",Float64,queue_size=10)
self.pub_goal_ = rospy.Publisher("goal", Pose, queue_size = 15)
def step(self, action, is_training):
if self.terminal == 0:
if action == -1:
# Step with random action
action = int(random.random()*(self.action_size))
self.actionToVel( action)
self.readyForNewData = True
if self.display:
cv2.imshow("Screen", self.screen)
cv2.waitKey(3)
dist = np.sqrt( (self.robotPose.position.x - self.goalPose.position.x)**2 + (self.robotPose.position.y - self.goalPose.position.y)**2)
#near obstacle penalty factor:
nearObstPenalty = self.minFrontDist - 1.5
reward = max( 0, ((self.prevDist - dist + 1.8)*3/dist)+min( 0, nearObstPenalty))
self.prevDist = dist
if dist < 0.9:
reward += 300
#Select a new goal
d = -1
while d < 3.9:
theta = 2.0 * np.pi * random.random()
r = random.random()*19.5
self.goalPose.position.x = r*np.cos( theta)
self.goalPose.position.y = r*np.sin( theta)
d = np.sqrt( (self.robotPose.position.x - self.goalPose.position.x)**2 + (self.robotPose.position.y - self.goalPose.position.y)**2)
self.pub_goal_.publish( self.goalPose)
self.prevDist = d
rwd = Float64()
rwd.data = 10101.963
self.pub_rew_.publish( rwd)
self.numWins += 1
if self.numWins == 99:
reward += 9000
self.terminal = 1
# Add whatever info you want
info = ""
self.ep_reward += reward
if self.terminal == 1:
rewd = Float64()
rewd.data = self.ep_reward
self.pub_rew_.publish( rewd)
self.sendTerminal = 1
while( self.readyForNewData == True):
pass
if self.use_cumulated_reward:
return self.screen, self.ep_reward, self.sendTerminal, info
else:
return self.screen, reward, self.sendTerminal, info
#observation, reward, terminal, info = self.env.step(action)
#return self.preprocess(observation), reward, terminal, info
def __init__(self):
# Give the node a name
rospy.init_node('quat_to_angle', anonymous=False)
# Publisher to control the robot's speed
self.turtlebot_angle = rospy.Publisher('/turtlebot_angle', Float64, queue_size=5)
self.turtlebot_posex = rospy.Publisher('/turtlebot_posex', Float64, queue_size=5)
self.turtlebot_posey = rospy.Publisher('/turtlebot_posey', Float64, queue_size=5)
#goal.target_pose.pose = Pose(Point(float(data["point"]["x"]), float(data["point"]["y"]), float(data["point"]["z"])), Quaternion(float(data["quat"]["x"]), float(data["quat"]["y"]), float(data["quat"]["z"]), float(data["quat"]["w"])))
# How fast will we update the robot's movement?
rate = 20
# Set the equivalent ROS rate variable
r = rospy.Rate(rate)
# Initialize the tf listener
self.tf_listener = tf.TransformListener()
# Give tf some time to fill its buffer
rospy.sleep(2)
# Set the odom frame
self.odom_frame = '/odom'
# Find out if the robot uses /base_link or /base_footprint
try:
self.tf_listener.waitForTransform(self.odom_frame, '/base_footprint', rospy.Time(), rospy.Duration(1.0))
self.base_frame = '/base_footprint'
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
try:
self.tf_listener.waitForTransform(self.odom_frame, '/base_link', rospy.Time(), rospy.Duration(1.0))
self.base_frame = '/base_link'
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
rospy.loginfo("Cannot find transform between /odom and /base_link or /base_footprint")
rospy.signal_shutdown("tf Exception")
# Initialize the position variable as a Point type
position = Point()
while not rospy.is_shutdown():
(position, rotation) = self.get_odom()
print(position)
self.turtlebot_angle.publish(rotation)
#print(str(position).split('x: ')[1].split('\ny:')[0])
x = float(str(position).split('x: ')[1].split('\ny:')[0])
y = float(str(position).split('y: ')[1].split('\nz:')[0])
self.turtlebot_posex.publish(x)
self.turtlebot_posey.publish(y)
#print(rotation)
rospy.sleep(5)
