def _publish_odom(self, timestamp):
'''
See base class comment
'''
# One of the kalman filters have not been initialized yet, don't publish
if not self._chassis_kf or not self._imu_kf:
return
if self._should_reset():
rospy.logwarn("Odometry diverged, resetting kalman filters...")
self._reset_kf()
return
odom_msg = Odometry()
odom_msg.header.frame_id = "odom"
odom_msg.child_frame_id = "base_center" # fitting 2 frame transforms into 1 odom msg
odom_msg.header.stamp = timestamp
chassis_state = self._chassis_kf.statePost[:, 0]
imu_state = self._imu_kf.statePost[:, 0]
# Translation
(odom_msg.pose.pose.position.x,
odom_msg.pose.pose.position.y) = chassis_state[0:2]
odom_msg.pose.pose.position.z = 0
# Rotation
mod = 2 * math.pi
mcb_imu_quat = tf_conversions.transformations.quaternion_from_euler(
imu_state[0] % mod, imu_state[1] % mod, imu_state[2] % mod)
pose_quat = convert_quat_to_frame(mcb_imu_quat, "base_center",
"mcb_imu")
(odom_msg.pose.pose.orientation.x, odom_msg.pose.pose.orientation.y,
odom_msg.pose.pose.orientation.z,
odom_msg.pose.pose.orientation.w) = pose_quat
# Set pose covariances
chassis_cov = self._chassis_kf.errorCovPost
imu_cov = self._imu_kf.errorCovPost
pose_cov_matrix = np.zeros((6, 6), dtype=np.float32)
pose_cov_matrix[0:2, 0:2] = chassis_cov[0:2, 0:2]
pose_cov_matrix[3:6, 3:6] = imu_cov[0:3, 0:3]
odom_msg.pose.covariance = tuple(pose_cov_matrix.flatten())
# Translational velocity
(odom_msg.twist.twist.linear.x,
odom_msg.twist.twist.linear.y) = chassis_state[2:4]
odom_msg.twist.twist.linear.z = 0
# Angular velocities
twist_euler = convert_angv_to_frame(imu_state[3:6], "base_center",
"mcb_imu")
(odom_msg.twist.twist.angular.x, odom_msg.twist.twist.angular.y,
odom_msg.twist.twist.angular.z) = twist_euler[0:3]
# Set velocity covariances
twist_cov_matrix = np.zeros((6, 6), dtype=np.float32)
twist_cov_matrix[0:2, 0:2] = chassis_cov[2:4, 2:4]
twist_cov_matrix[3:6, 3:6] = imu_cov[3:6, 3:6]
odom_msg.twist.covariance = tuple(twist_cov_matrix.flatten())
self._odom_pub.publish(odom_msg)
def __init__(self,
state_collector,
ns,
robot_radius=0,
robot_width=0.58,
robot_height=0.89):
self.__robot_radius = robot_radius # robot radius
self.__robot_height = robot_height # robot width
self.__robot_width = robot_width # robot heigth
self.__odom = Odometry(
) # most recently published odometry of the robot
self.__path = Path() # most recently published path
self.__done = False # is episode done?
self.__new_task_started = False # has a new task started?
self.__state_collector = state_collector # for getting relevant state values of the robot
self.__rl_agent_path = rospkg.RosPack().get_path(
'rl_agent') # absolute path rl_agent-package
self.__flatland_topics = [] # list of flatland topics
self.__timestep = 0 # actual timestemp of training
self.__NS = ns
self.MODE = rospy.get_param("%s/rl_agent/train_mode", 1)
self.__clock = Clock().clock
self.__task_generator = TaskGenerator(self.__NS,
self.__state_collector, 0.46)
self.__recent_agent_states = []
# Subscriber for getting data
#self.__odom_sub = rospy.Subscriber("%s/odom"%self.__NS, Odometry, self.__odom_callback, queue_size=1)
self.__global_plan_sub = rospy.Subscriber(
"%s/move_base/NavfnROS/plan" % self.__NS,
Path,
self.__path_callback,
queue_size=1)
# self.__path_sub = rospy.Subscriber("%s/move_base/GlobalPlanner/plan" % self.__NS, Path, self.__path_callback, queue_size=1)
self.__done_sub = rospy.Subscriber("%s/rl_agent/done" % self.__NS,
Bool,
self.__done_callback,
queue_size=1)
self.__new_task_sub = rospy.Subscriber('%s/rl_agent/new_task_started' %
self.__NS,
Bool,
self.__new_task_callback,
queue_size=1)
self.__flatland_topics_sub = rospy.Subscriber(
"%s/flatland_server/debug/topics" % self.__NS,
DebugTopicList,
self.__flatland_topic_callback,
queue_size=1)
self.__agent_action_sub = rospy.Subscriber(
'%s/rl_agent/action' % self.__NS, Twist, self.__trigger_callback)
self.__ped_sub = rospy.Subscriber(
'%s/pedsim_simulator/simulated_agents' % self.__NS, AgentStates,
self.__ped_callback)
if self.MODE == 1 or self.MODE == 0:
self.clock_sub = rospy.Subscriber('%s/clock' % self.__NS, Clock,
self.__clock_callback)
# Publisher for generating qualitative image
self.__driven_path_pub = rospy.Publisher('%s/rl_eval/driven_path' %
self.__NS,
Path,
queue_size=1)
self.__driven_path_pub2 = rospy.Publisher('%s/rl_eval/driven_path2' %
self.__NS,
Path,
queue_size=1)
self.__global_path_pub = rospy.Publisher('%s/rl_eval/global_path' %
self.__NS,
Path,
queue_size=1)
self.__agent_pub = rospy.Publisher('%s/rl_eval/viz_agents' % self.__NS,
MarkerArray,
queue_size=1)
def publish_messages():
# pub = rospy.Publisher('ekf_data_fused', Odometry, queue_size=10)
# rospy.init_node('mavlink_manager_in', anonymous=True)
# rate = rospy.Rate(5) # 10hz
# while not rospy.is_shutdown():
# current_time = rospy.Time.now()
# ekf_data_fused = connection_in.recv_match(type='ODOMETRY', blocking=True) # connection_in.messages['Odometry']
# # print("ecco sta merda di msg")
# # print(connection_in)
# ekf_data_fused_ros = Odometry()
# ekf_data_fused_ros.header.stamp = current_time
# ekf_data_fused_ros.header.frame_id = "odom"
# ekf_data_fused_ros.pose.pose.position.x = ekf_data_fused.x
# ekf_data_fused_ros.pose.pose.position.y = ekf_data_fused.y
# ekf_data_fused_ros.pose.pose.orientation.w = ekf_data_fused.q[0]
# ekf_data_fused_ros.pose.pose.orientation.x = ekf_data_fused.q[3]
# ekf_data_fused_ros.twist.twist.linear.x = ekf_data_fused.vx
# ekf_data_fused_ros.twist.twist.linear.y = ekf_data_fused.vy
# pub.publish(ekf_data_fused_ros)
# rate.sleep()
pub_enc = rospy.Publisher('encoder', Odometry, queue_size=10)
pub_mag = rospy.Publisher('magnetometer', Odometry, queue_size=10)
pub_imu = rospy.Publisher('imu_k64', Imu, queue_size=10)
pub_imu2 = rospy.Publisher('imu_ext', Imu, queue_size=10)
global pos_l_old
global pos_l_new
global pos_r_old
global pos_r_new
global psi_old
global oldT
oldT = 0
pos_l_old = 0
pos_l_new = 0
pos_r_old = 0
pos_r_new = 0
psi_old = 0
x_0 = 0
y_0 = 0
r = 0.02
B = 0.185
rospy.init_node('mavlink_manager_in', anonymous=True)
rate = rospy.Rate(20) # 10hz
while not rospy.is_shutdown():
current_time = rospy.Time.now()
# imu = connection_in.recv_match(type='SCALED_IMU', blocking=True)
imu_ros = Imu()
imu_ros.header.stamp = current_time
imu_ros.header.frame_id = "odom" # odom before
imu_ros.linear_acceleration.x = 0
imu_ros.linear_acceleration.y = 0
imu_ros.linear_acceleration.z = 0
imu_ros.linear_acceleration_covariance[0] = 0
imu_ros.linear_acceleration_covariance[4] = 0
imu_ros.linear_acceleration_covariance[8] = 0
# imu_ros.orientation.z = imu.zmag/10000
# imu_ros.orientation.w = sqrt(1-(imu.zmag/10000)**2)
# imu_ros.orientation.x = 0
# imu_ros.orientation.y = 0
imu_ros.orientation_covariance[8] = 0
pub_imu.publish(imu_ros)
# rospy.loginfo("AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA")
# Magnetometer is treated as a separate Odom topic
magneto_ros = Odometry()
magneto_ros.header.stamp = current_time
magneto_ros.header.frame_id = "odom"
magneto_ros.pose.pose.orientation.z = 0
magneto_ros.pose.pose.orientation.w = 0
magneto_ros.pose.pose.orientation.x = 0
magneto_ros.pose.pose.orientation.y = 0
pub_mag.publish(magneto_ros)
# imu_ex = connection_in.recv_match(type='SCALED_IMU2', blocking=True)
imu_ros2 = Imu()
imu_ros2.header.stamp = current_time
imu_ros2.header.frame_id = "odom" # odom before
imu_ros2.linear_acceleration.x = 0
imu_ros2.linear_acceleration.y = 0
imu_ros2.linear_acceleration.z = 0
imu_ros2.linear_acceleration_covariance[0] = 0
imu_ros2.linear_acceleration_covariance[4] = 0
imu_ros2.linear_acceleration_covariance[8] = 0
imu_ros2.angular_velocity.z = 0
imu_ros2.angular_velocity.x = 0
imu_ros2.angular_velocity.y = 0
imu_ros2.orientation.z = 0
imu_ros2.orientation.w = 0
imu_ros2.orientation.x = 0
imu_ros2.orientation.y = 0
imu_ros2.angular_velocity_covariance[0] = 2.5 / 10000
imu_ros2.angular_velocity_covariance[4] = 2.7 / 10000
imu_ros2.angular_velocity_covariance[8] = 2.6 / 10000
pub_imu2.publish(imu_ros2)
# encoders = connection_in.recv_match(type='WHEEL_DISTANCE', blocking=True) # connection_in.messages['Odometry']
# # print(connection_in)
# if pos_l_old== 0:
# pos_l_old = encoders.distance[0]
# pos_r_old = encoders.distance[1]
# psi_old = 0
encoders_ros = Odometry()
encoders_ros.header.stamp = current_time
encoders_ros.header.frame_id = "odom"
rot = R.from_quat([0, 0, 0, 1])
[psi, _, _] = rot.as_euler("zyx", degrees=False)
# Integration of psi based on encoder positions
pos_l_new = 1
pos_r_new = 1
# currentT = (float)encoders_ros.header.stamp
# deltaT = currentT - oldT
# oldT = currentT
deltaPos_l = pos_l_new - pos_l_old
deltaPos_r = pos_r_new - pos_r_old
psi_new = psi_old + (1 / B) * (deltaPos_r - deltaPos_l) # Rad
VR = deltaPos_r
VL = deltaPos_l
pos_l_old = pos_l_new
pos_r_old = pos_r_new
psi_old = psi_new
# if (x_0 == 0) and (y_0 == 0):
# x_0 = (encoders.distance[0]+encoders.distance[1])/2*cos(psi_new)
# y_0 = (encoders.distance[0]+encoders.distance[1])/2*sin(psi_new)
# encoders_ros.pose.pose.position.x = (encoders.distance[0]+encoders.distance[1])/2*cos(psi_new) #-x_0
# encoders_ros.pose.pose.position.y = (encoders.distance[0]+encoders.distance[1])/2*sin(psi_new) #-y_0
encoders_ros.pose.pose.position.x = x_0 + (VR + VL) / 2 * cos(psi_new)
encoders_ros.pose.pose.position.y = y_0 + (VR + VL) / 2 * sin(psi_new)
x_0 = encoders_ros.pose.pose.position.x
y_0 = encoders_ros.pose.pose.position.y
print('Psi value from frdm', psi_new * 180 / pi,
encoders_ros.pose.pose.position.x,
encoders_ros.pose.pose.position.y, (pos_l_new + pos_r_new), x_0,
y_0)
# encoders_ros.pose.pose.position.y = 0
pub_enc.publish(encoders_ros)
rate.sleep()
def _get_obs(self):
"""
Read robot state
:return:
"""
rospy.logdebug("Start Get Observation ==>")
# Pose of the MAV
pose = self.get_gt_odom()
stamp = pose.header.stamp
# Velocity of the MAV
#vel = self.get_velocity(stamp)
#print(vel.twist.twist)
velocity = self.get_tello_velocity()
vel = Odometry()
vel.twist.twist.linear.x = velocity[1]
vel.twist.twist.linear.y = velocity[0]
vel.twist.twist.linear.z = -velocity[2]
print(vel.twist.twist)
# Position of the Target
target = self.get_gt_target(stamp)
if target is None:
target = Odometry()
target.pose.pose.position.x = 0
target.pose.pose.position.y = 0
target.pose.pose.position.z = 0
target.twist.twist.linear.x = 0
target.twist.twist.linear.y = 0
target.twist.twist.linear.z = 0
###################################################################################################
'''
Inputs for Policy Network
'''
###################################################################################################
'''Actor Root in Mav base link frame: Only Translation. Also used for calculating MAV bearing'''
delta_target1 = PoseWithCovarianceStamped()
delta_target_w = np.array([target.pose.pose.position.y-pose.position.y,\
target.pose.pose.position.x-pose.position.x,\
-(target.pose.pose.position.z-pose.position.z)])
TARGET_orientation = np.array([
target.pose.pose.orientation.x, target.pose.pose.orientation.y,
target.pose.pose.orientation.z, target.pose.pose.orientation.w
])
MAV_orientation = np.array([
pose.orientation.x, pose.orientation.y, pose.orientation.z,
pose.orientation.w
])
(r, p,
y) = tf.transformations.euler_from_quaternion(TARGET_orientation)
TARGET_yaw = y
(r, p, y) = tf.transformations.euler_from_quaternion(MAV_orientation)
MAV_yaw = y
homogeneous_matrix = tf.transformations.euler_matrix(0,
0,
y,
axes='sxyz')
inv_homogeneous_matrix = np.linalg.inv(homogeneous_matrix[:3, :3])
delta_target_local = np.dot(inv_homogeneous_matrix, delta_target_w)
#Relative Translation
delta_target1.pose.pose.position.x = delta_target_local[0]
delta_target1.pose.pose.position.y = delta_target_local[1]
delta_target1.pose.pose.position.z = delta_target_local[2]
#Bearing
theta1 = np.arctan2(delta_target_local[1], delta_target_local[0])
'''Actor Root Orientation in Mav base link frame: Only Rotation '''
delta_orient = TARGET_yaw - MAV_yaw
''' Take smaller difference between angles'''
if delta_orient > np.pi: delta_orient += -2 * np.pi
elif delta_orient < -np.pi: delta_orient += 2 * np.pi
else: delta_orient += 0
'''Calculate relative velocity vector in the MAV base link frame'''
relative_vel = np.dot(inv_homogeneous_matrix,
np.array([vel.twist.twist.linear.y - target.twist.twist.linear.x,
vel.twist.twist.linear.x - target.twist.twist.linear.y,\
-vel.twist.twist.linear.z - target.twist.twist.linear.z]))
#Dummy zero inputs to value network during testing.
value_input = [0] * 55
scale = 4
# Policy Inputs + Value Inputs
observations = [scale*delta_target1.pose.pose.position.x, scale*delta_target1.pose.pose.position.y, scale*delta_target1.pose.pose.position.z,\
relative_vel[0],relative_vel[1],relative_vel[2],np.cos(theta1), np.sin(theta1),np.cos(delta_orient), np.sin(delta_orient)]+value_input
#rospy.loginfo("Observations==>"+str(observations))
#rospy.loginfo("END Get Observation ==>")
return observations
cont = 0
#Node and msg initialization
rospy.init_node('path_odom_plotter')
#Rosparams that are set in the launch
#max size of array pose msg from the path
if not rospy.has_param("~max_list_append"):
rospy.logwarn('The parameter max_list_append dont exists')
max_append = rospy.set_param("~max_list_append", 1000)
max_append = 1000
if not (max_append > 0):
rospy.logwarn('The parameter max_list_append not is correct')
sys.exit()
pub = rospy.Publisher('/odompath', Path, queue_size=1)
path = Path() #creamos el mensaje path de tipo path
msg = Odometry()
#Subscription to the topic
msg = rospy.Subscriber('/odom', Odometry, callback)
rate = rospy.Rate(30) # 30hz
try:
while not rospy.is_shutdown():
#rospy.spin()
rate.sleep()
except rospy.ROSInterruptException:
pass
def main(self):
print("starting node")
rospy.init_node('rs_to_odom')
listener = tf.TransformListener()
odom_sub = rospy.Subscriber("/t265/odom/sample", Odometry, self.odom_callback)
initial_sub = rospy.Subscriber("/initialpose", PoseWithCovarianceStamped, self.initial_callback)
odom_pub = rospy.Publisher("/base_pose_ground_truth", Odometry , queue_size=10)
move_base_odom_pub = rospy.Publisher("/odom", Odometry , queue_size=10)
odom_msg = Odometry()
odom_msg.header.frame_id = "odom"
odom_msg.child_frame_id = "base_link"
odom_msg.pose.covariance = [0.01, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.01, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.01, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0001, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0001, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0001]
odom_msg.twist.covariance = [0.01, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.01, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.01, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0001, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0001, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0001]
while not rospy.is_shutdown():
try:
rospy.loginfo("odom info recieved %s"%(str([[lx,ly,lz],[ax,ay,az]])))
rospy.loginfo("beginning odometry transformation")
except NameError:
continue
else:
break
#lx, ly, lz, ax, ay, az = (0,0,0,0,0,0)
#time.sleep(0.1) # allows listener buffer to load + callback variables
rate = rospy.Rate(50)
try:
while not rospy.is_shutdown():
try:
(trans,rot) = listener.lookupTransform('/t265_odom_frame', '/fake_base_link', rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
continue
# Negate to make it start "backwards"
odom_msg.pose.pose.position.x = trans[0] - 0.42545 + self.new_x_pose
odom_msg.pose.pose.position.y = trans[1] + self.new_y_pose
odom_msg.pose.pose.position.z = 0
odom_msg.pose.pose.orientation.x = 0
odom_msg.pose.pose.orientation.y = 0
# Inorder to flip robot 180 degrees you must switch the z and w values: negate values to switch turn direction
odom_msg.pose.pose.orientation.z = rot[2]
odom_msg.pose.pose.orientation.w = rot[3]
#print("z: ",rot[2]," w: ", rot[3])
odom_msg.twist.twist.linear.x = -lx
#odom_msg.twist.twist.linear.y = ly
#odom_msg.twist.twist.linear.z = lz
odom_msg.twist.twist.angular.x = ax
odom_msg.twist.twist.angular.y = ay
odom_msg.twist.twist.angular.z = az
now = rospy.get_rostime()
odom_msg.header.stamp.secs = now.secs
odom_msg.header.stamp.nsecs = now.nsecs
odom_pub.publish(odom_msg)
# For move_base
move_base_odom_pub.publish(odom_msg)
rate.sleep()
except rospy.ROSInterruptException:
pass
def cb_odometry(self, msg):
# Nan check
if (math.isnan(msg.pose.pose.position.x)
or math.isnan(msg.pose.pose.position.y)
or math.isnan(msg.pose.pose.position.z)
or math.isnan(msg.pose.pose.orientation.x)
or math.isnan(msg.pose.pose.orientation.y)
or math.isnan(msg.pose.pose.orientation.z)
or math.isnan(msg.twist.twist.linear.x)
or math.isnan(msg.twist.twist.linear.y)
or math.isnan(msg.twist.twist.linear.z)
or math.isnan(msg.twist.twist.angular.x)
or math.isnan(msg.twist.twist.angular.y)
or math.isnan(msg.twist.twist.angular.z)):
rospy.logwarn('Recieved an odom message with nan values')
return
pos_t265_odombased = np.array([
msg.pose.pose.position.x, msg.pose.pose.position.y,
msg.pose.pose.position.z
])
quat_t265_odombased = Quaternion(
msg.pose.pose.orientation.w, msg.pose.pose.orientation.x,
msg.pose.pose.orientation.y,
msg.pose.pose.orientation.z).normalised
pos_base_odombased = pos_t265_odombased + \
quat_t265_odombased.rotate(self._pos_base_t265based)
quat_base_odombased = quat_t265_odombased * self._quat_base_t265based
pos_dot_t265_t265based = np.array([
msg.twist.twist.linear.x, msg.twist.twist.linear.y,
msg.twist.twist.linear.z
])
omega_t265_t265based = np.array([
msg.twist.twist.angular.x, msg.twist.twist.angular.y,
msg.twist.twist.angular.z
])
pos_dot_base_basebased = self._quat_t265_basebased.rotate(
pos_dot_t265_t265based +
np.cross(omega_t265_t265based, self._pos_base_t265based))
omega_base_basebased = self._quat_t265_basebased.rotate(
omega_t265_t265based)
pub_msg = Odometry()
pub_msg.header.stamp = rospy.Time.now()
pub_msg.header.frame_id = self._frame_id_odom
pub_msg.child_frame_id = self._frame_id_base_link
if self._2d_mode:
quat_base_odombased_2d = Quaternion(
quat_base_odombased.w, 0, 0, quat_base_odombased.z).normalised
pub_msg.pose.pose.position.x = pos_base_odombased[0]
pub_msg.pose.pose.position.y = pos_base_odombased[1]
pub_msg.pose.pose.position.z = 0
pub_msg.pose.pose.orientation.x = quat_base_odombased_2d.x
pub_msg.pose.pose.orientation.y = quat_base_odombased_2d.y
pub_msg.pose.pose.orientation.z = quat_base_odombased_2d.z
pub_msg.pose.pose.orientation.w = quat_base_odombased_2d.w
pub_msg.twist.twist.linear.x = pos_dot_base_basebased[0]
pub_msg.twist.twist.linear.y = pos_dot_base_basebased[1]
pub_msg.twist.twist.linear.z = 0
pub_msg.twist.twist.angular.x = 0
pub_msg.twist.twist.angular.y = 0
pub_msg.twist.twist.angular.z = omega_base_basebased[2]
else:
pub_msg.pose.pose.position.x = pos_base_odombased[0]
pub_msg.pose.pose.position.y = pos_base_odombased[1]
pub_msg.pose.pose.position.z = pos_base_odombased[2]
pub_msg.pose.pose.orientation.x = quat_base_odombased.x
pub_msg.pose.pose.orientation.y = quat_base_odombased.y
pub_msg.pose.pose.orientation.z = quat_base_odombased.z
pub_msg.pose.pose.orientation.w = quat_base_odombased.w
pub_msg.twist.twist.linear.x = pos_dot_base_basebased[0]
pub_msg.twist.twist.linear.y = pos_dot_base_basebased[1]
pub_msg.twist.twist.linear.z = pos_dot_base_basebased[2]
pub_msg.twist.twist.angular.x = omega_base_basebased[0]
pub_msg.twist.twist.angular.y = omega_base_basebased[1]
pub_msg.twist.twist.angular.z = omega_base_basebased[2]
# TODO: tranform covariance
pub_msg.pose.covariance = msg.pose.covariance
pub_msg.twist.covariance = msg.twist.covariance
self._pub_odom.publish(pub_msg)
if self._publish_tf:
t = TransformStamped()
t.header.stamp = rospy.Time.now()
t.header.frame_id = pub_msg.header.frame_id
t.child_frame_id = pub_msg.child_frame_id
t.transform.translation.x = pub_msg.pose.pose.position.x
t.transform.translation.y = pub_msg.pose.pose.position.y
t.transform.translation.z = pub_msg.pose.pose.position.z
t.transform.rotation.x = pub_msg.pose.pose.orientation.x
t.transform.rotation.y = pub_msg.pose.pose.orientation.y
t.transform.rotation.z = pub_msg.pose.pose.orientation.z
t.transform.rotation.w = pub_msg.pose.pose.orientation.w
self._tf_br.sendTransform(t)
def trajectory_in_camera_fov(self, data):
"""generates trajectory in camera workspace"""
#self.end_point = self.end_point + np.array([0, -self.target_velocity*0.05, 0])
odom_msg = Odometry()
odom_msg.pose.pose.position.x = self.end_point[0]
odom_msg.pose.pose.position.y = self.end_point[1]
odom_msg.pose.pose.position.z = self.end_point[2]
odom_msg.header.stamp = rospy.Time.now()
odom_msg.header.frame_id = 'world'
self.target_odom.publish(odom_msg)
start = time.time()
start_point = np.zeros((0,3))
if self.traj_gen_counter != 0:
start_point = np.concatenate((start_point, self.p_eoe), 0) # was self.p_eoe
else:
start_point = np.concatenate((start_point, self.Pglobal[np.newaxis]), 0) # new axis was needed because you dont maintain uniformity
points_in_cone = self.generate_regular_pyramid_grid()
occupied_points = ros_numpy.numpify(data)
#for k in range(len(occupied_points)):
# f7 = open('point_cloud.txt', 'a')
# f7.write('%s, %s, %s, %s, %s, %s\n' %(self.traj_gen_counter, self.RHP_time, len(occupied_points), occupied_points[k][0], occupied_points[k][1], occupied_points[k][2]))
#if self.traj_gen_counter == 0:
# for k in range(len(occupied_points)):
# f7 = open('point_cloud.txt', 'a')
# f7.write('%s, %s, %s, %s\n' %(len(occupied_points), occupied_points[k][0], occupied_points[k][1], occupied_points[k][2]))
if len(occupied_points) != 0:
points_in_cone = self.generate_final_grid(points_in_cone, occupied_points)
"""
points_in_cone = [x[np.newaxis] for x in points_in_cone]
pp1 = [np.dot(self.Rcdoc_cdl[0:3, 0:3].T, x.T) for x in points_in_cone]
# now translate from visensor frame to cg frame
pp2 = [self.Rcg_vibase[0:3, 3:4] + np.dot(self.Rcg_vibase[0:3, 0:3].T, x) for x in pp1]
# now rotate the cloud in world frame
points_in_cone = [self.Pglobal + np.dot(self.Rglobal.T, x).ravel() for x in pp2]
# remove z points if they are going inside the ground, assuming ground is z = 0
points_in_cone = [x for x in points_in_cone if not x[2] <= 0]
points_in_cone = np.concatenate((start_point, points_in_cone), 0)
"""
p1 = [np.dot(self.Rcdoc_cdl[0:3,0:3], x[np.newaxis].T) for x in points_in_cone]
#p1 = [self.Rvibase_cl[0:3, 3:4] + np.dot(self.Rcdoc_cdl[0:3,0:3], x[np.newaxis].T) for x in points_in_cone]
p2 = [self.Rcg_vibase[0:3, 3:4] + np.dot(self.Rcg_vibase[0:3, 0:3].T, x) for x in p1]
#p2 = [np.dot(self.Rcg_vibase[0:3, 0:3].T, x) for x in p1]
points_in_cone = [self.Pglobal + np.dot(self.Rglobal, x).ravel() for x in p2]
points_in_cone = [x for x in points_in_cone if not x[2] <= 0]
points_in_cone = np.concatenate((start_point, points_in_cone), 0)
# publish generated pointcloud
header = std_msgs.msg.Header()
header.stamp = rospy.Time.now()
header.frame_id = 'world'
p = pc2.create_cloud_xyz32(header, points_in_cone)
self.pcl_pub.publish(p)
kdt_points_in_cone = cKDTree(points_in_cone)
closest_to_end = kdt_points_in_cone.query(self.end_point, 1)
#closest_to_end_index = points_in_cone[closest_to_end[1]]
#closest_to_end_point = (closest_to_end_index[0], closest_to_end_index[1], closest_to_end_index[2])
end_point_index = closest_to_end[1]
#one dimension simplicial complex which is basically a graph
rips = gudhi.RipsComplex(points = points_in_cone, max_edge_length = 1.5*self.resolution)
f = rips.create_simplex_tree(max_dimension = 1)
# make graph
G = nx.Graph()
G.add_nodes_from(range(f.num_vertices()))
edge_list = [(simplex[0][0], simplex[0][1], {'weight': simplex[1]}) if len(simplex[0])==2 else None for simplex in f.get_skeleton(1)]
edge_list = [k for k in edge_list if k is not None]
G.add_edges_from(edge_list)
try:
shortest_path = nx.shortest_path(G, source = 0, target = end_point_index, weight = 'weight', method = 'dijkstra')
path = np.array([points_in_cone[j] for j in shortest_path])
except:
print 'No path between start and end'
#f1 = open('path.txt', 'a')
#f1.write('%s, %s\n' %(path, points_in_cone[end_point_index]))
#length = nx.shortest_path_length(G, source = 0, target = end_point_index, weight = 'weight', method='dijkstra')
# planning horizon is a fixed (5 for now) segments, trajectory will be planned only for these segments,
# execution horizon will be just 3 segments
# at current resolution of 0.25m, that means a trajecotory of approximately 1.25m will be planned and 0.75m will be executed
# at each time the quad plans its motion, depending on how fast it can replan, these values would be changed
no_of_segments = 3
no_of_segments_to_track = 1
path = path[:no_of_segments+1] # n segments means n+1 points in the path
path = zip(*path)
# now construct the minimum snap trajectory on the minimum path
waypoint_specified = True
waypoint_bc = False
T, p1, p2, p3 = Minimum_snap_trajetory(self.uav_velocity, path, waypoint_specified, waypoint_bc, self.v_eoe, self.a_eoe, self.j_eoe).construct_polynomial_trajectory()
#self.plot_graph_and_trajectory(points_in_cone, occupied_points, G, path, T, p1, p2, p3)
N = 8
p1 = [p1[i:i + N] for i in range(0, len(p1), N)]
[i.reverse() for i in p1]
p2 = [p2[i:i + N] for i in range(0, len(p2), N)]
[i.reverse() for i in p2]
p3 = [p3[i:i + N] for i in range(0, len(p3), N)]
[i.reverse() for i in p3]
t = []; xx = []; yy = []; zz = []
vx = []; vy = []; vz = []; accx = []; accy = []; accz = []
jx = []; jy = []; jz = []
#print T
traj_frequency = 100
for ii in range(no_of_segments_to_track):
#print 'i m here' , ii, len(T)
#t.append(np.linspace(T[ii], T[ii+1], int((T[ii+1]-T[ii])*traj_frequency)))
t.append(np.linspace(T[ii], T[ii+1], 100))
xx.append(np.poly1d(p1[ii]))
vx.append(np.polyder(xx[-1], 1)); accx.append(np.polyder(xx[-1], 2))
jx.append(np.polyder(xx[-1], 3))#; sx.append(np.polyder(xx[-1], 4))
yy.append(np.poly1d(p2[ii]))
vy.append(np.polyder(yy[-1], 1)); accy.append(np.polyder(yy[-1], 2))
jy.append(np.polyder(yy[-1], 3))#; sy.append(np.polyder(yy[-1], 4))
zz.append(np.poly1d(p3[ii]))
vz.append(np.polyder(zz[-1], 1)); accz.append(np.polyder(zz[-1], 2))
jz.append(np.polyder(zz[-1], 3))#; sz.append(np.polyder(zz[-1], 4))
traj = MultiDOFJointTrajectory()
header = std_msgs.msg.Header()
header.stamp = rospy.Time.now()
header.frame_id = 'frame'
traj.header = header
traj.joint_names = 'nothing' # testing for now
trajectory_start_time = self.RHP_time
for i in range(no_of_segments_to_track): # "changed to no_of_segments_to_track" instead of "no_of_segments"
for j in t[i]:
time.sleep(0.01)
self.RHP_time = j + trajectory_start_time
xdes = xx[i](j); ydes = yy[i](j); zdes = zz[i](j)
vxdes = vx[i](j); vydes = vy[i](j); vzdes = vz[i](j)
axdes = accx[i](j); aydes = accy[i](j); azdes = accz[i](j)
jxdes = jx[i](j); jydes = jy[i](j); jzdes = jz[i](j)
# for now angular acceleration Point msg of MultiDOFJointTrajectory() msg is used to specify the desired direction
#vector = np.array([self.end_point[0]-xdes, self.end_point[1]-ydes, self.end_point[2]-zdes])
#vector = np.array([self.p_eoe[0][0]-xdes, self.p_eoe[0][1]-ydes, self.p_eoe[0][2]-zdes])
vector = np.array([self.end_point[0]-self.Pglobal[0], self.end_point[1]-self.Pglobal[1], self.end_point[2]-self.Pglobal[2]])
#vector = np.array([self.Vglobal[0], self.Vglobal[1], self.Vglobal[2]])
direction = vector/np.linalg.norm(vector)
transforms = Transform(translation = Point(xdes, ydes, zdes), rotation = Quaternion(0,0,0,1))
velocities = Twist(linear = Point(vxdes, vydes, vzdes), angular = Point(0,0,0))
accelerations = Twist(linear = Point(axdes, aydes, azdes), angular = Point(direction[0],direction[1],direction[2]))
#accelerations = Twist(linear = Point(axdes, aydes, azdes), angular = Point(1,0,0))
point = MultiDOFJointTrajectoryPoint([transforms],[velocities],[accelerations],rospy.Duration(self.RHP_time))
traj.points.append(point)
self.uav_traj_pub.publish(traj)
traj.points.pop(0)
self.p_eoe = np.array([[xdes, ydes, zdes]])
self.v_eoe = np.array([[vxdes, vydes, vzdes]])
self.a_eoe = np.array([[axdes, aydes, azdes]])
self.j_eoe = np.array([[jxdes, jydes, jzdes]])
f1 = open('eoe_points.txt', 'a')
f1.write('%s, %s, %s, %s\n' %(self.p_eoe, self.v_eoe, self.a_eoe, self.j_eoe))
#self.uav_traj_pub.publish(traj)
time_taken = time.time()-start
self.traj_gen_counter += 1
#time.sleep(0.1)
#time.sleep(T[no_of_segments_to_track]*0.9-time_taken)
f1 = open('total_time_taken.txt', 'a')
f1.write("%s\n" % (time_taken))
print 'total time taken to execute the callbacak is:', time_taken
theta_array = []
first_scan = True
counter = 0
out_direction_ = 0
OCCUPANCY_GRID_HEIGHT = 3.0 # in meters
OCCUPANCY_GRID_WIDTH = 3.0 # in meters
RESOLUTION = 8.0 # boxes per meter
PIXEL_HEIGHT = int(OCCUPANCY_GRID_HEIGHT*RESOLUTION)
PIXEL_WIDTH = int(OCCUPANCY_GRID_WIDTH*RESOLUTION)
current_occupancy_grid = np.zeros((PIXEL_WIDTH, PIXEL_HEIGHT))
radius_map = np.zeros((PIXEL_WIDTH, PIXEL_HEIGHT))
theta_map = np.zeros((PIXEL_WIDTH, PIXEL_HEIGHT))
current_odom = Odometry()
occupancy_grid_pub = OccupancyGrid()
next_point_ = Point()
def create_theta_array(scan):
theta_min = scan.angle_min
theta_max = scan.angle_max
theta_delta = (theta_max - theta_min)/len(np.array(scan.ranges))
theta = np.arange(theta_min,theta_max,theta_delta)
return theta
def create_radius_array():
global radius_map
global theta_map
for row in range(0, PIXEL_WIDTH):
for col in range(0, PIXEL_HEIGHT):
def poll(self):
now = rospy.Time.now()
if now > self.t_next:
#try:
# left_pidin, right_pidin = self.arduino.get_pidin()
#except:
# rospy.logerr("getpidout exception count: ")
# return
#self.lEncoderPub.publish(left_pidin)
#self.rEncoderPub.publish(right_pidin)
#try:
# left_pidout, right_pidout = self.arduino.get_pidout()
#except:
# rospy.logerr("getpidout exception count: ")
# return
#self.lPidoutPub.publish(left_pidout)
#self.rPidoutPub.publish(right_pidout)
# Read the encoders
try:
left_enc, right_enc = self.arduino.get_encoder_counts()
#rospy.loginfo("left_enc: " + str(left_enc)+"right_enc: " + str(right_enc))
except:
self.bad_encoder_count += 1
rospy.logerr("Encoder exception count: " +
str(self.bad_encoder_count))
return
try:
csb_value1 = self.arduino.get_csb_value()
except:
rospy.logerr("CSB exception ")
try:
csb_value2 = self.arduino.get_csb1_value()
except:
rospy.logerr("CSB exception ")
dt = now - self.then
self.then = now
dt = dt.to_sec()
# Calculate odometry
if self.enc_left == None:
dright = 0
dleft = 0
else:
if (left_enc < self.encoder_low_wrap
and self.enc_left > self.encoder_high_wrap):
self.l_wheel_mult = self.l_wheel_mult + 1
elif (left_enc > self.encoder_high_wrap
and self.enc_left < self.encoder_low_wrap):
self.l_wheel_mult = self.l_wheel_mult - 1
else:
self.l_wheel_mult = 0
if (right_enc < self.encoder_low_wrap
and self.enc_right > self.encoder_high_wrap):
self.r_wheel_mult = self.r_wheel_mult + 1
elif (right_enc > self.encoder_high_wrap
and self.enc_right < self.encoder_low_wrap):
self.r_wheel_mult = self.r_wheel_mult - 1
else:
self.r_wheel_mult = 0
#dright = (right_enc - self.enc_right) / self.ticks_per_meter
#dleft = (left_enc - self.enc_left) / self.ticks_per_meter
dleft = 1.0 * (left_enc + self.l_wheel_mult *
(self.encoder_max - self.encoder_min) -
self.enc_left) / self.ticks_per_meter
dright = 1.0 * (right_enc + self.r_wheel_mult *
(self.encoder_max - self.encoder_min) -
self.enc_right) / self.ticks_per_meter
self.enc_right = right_enc
self.enc_left = left_enc
dxy_ave = (dright + dleft) / 2.0
dth = (dright - dleft) / self.wheel_track
vxy = dxy_ave / dt
vth = dth / dt
if (dxy_ave != 0):
dx = cos(dth) * dxy_ave
dy = -sin(dth) * dxy_ave
self.x += (cos(self.th) * dx - sin(self.th) * dy)
self.y += (sin(self.th) * dx + cos(self.th) * dy)
if (dth != 0):
self.th += dth
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = sin(self.th / 2.0)
quaternion.w = cos(self.th / 2.0)
# Create the odometry transform frame broadcaster.
self.odomBroadcaster.sendTransform(
(self.x, self.y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
rospy.Time.now(), self.base_frame, "odom")
odom = Odometry()
odom.header.frame_id = "odom"
odom.child_frame_id = self.base_frame
odom.header.stamp = now
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.twist.twist.linear.x = vxy
odom.twist.twist.linear.y = 0
odom.twist.twist.angular.z = vth
# todo sensor_state.distance == 0
#if self.v_des_left == 0 and self.v_des_right == 0:
# odom.pose.covariance = ODOM_POSE_COVARIANCE2
# odom.twist.covariance = ODOM_TWIST_COVARIANCE2
#else:
# odom.pose.covariance = ODOM_POSE_COVARIANCE
# odom.twist.covariance = ODOM_TWIST_COVARIANCE
self.odomPub.publish(odom)
self.csbPub.publish(csb_value1)
self.csb1Pub.publish(csb_value2)
if now > (self.last_cmd_vel + rospy.Duration(self.timeout)):
self.v_des_left = 0
self.v_des_right = 0
if self.v_left < self.v_des_left:
self.v_left += self.max_accel
if self.v_left > self.v_des_left:
self.v_left = self.v_des_left
else:
self.v_left -= self.max_accel
if self.v_left < self.v_des_left:
self.v_left = self.v_des_left
if self.v_right < self.v_des_right:
self.v_right += self.max_accel
if self.v_right > self.v_des_right:
self.v_right = self.v_des_right
else:
self.v_right -= self.max_accel
if self.v_right < self.v_des_right:
self.v_right = self.v_des_right
self.lVelPub.publish(self.v_left)
self.rVelPub.publish(self.v_right)
self.csbPub.publish(self.csb_value)
# Set motor speeds in encoder ticks per PID loop
if not self.stopped:
self.arduino.drive(self.v_left, self.v_right)
self.t_next = now + self.t_delta
#!/usr/bin/env python
# license removed for brevity
import rospy
import math
import tf
from sensor_msgs.msg import JointState
from std_msgs.msg import Header
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Quaternion
initial_pose = Odometry()
target_pose = Odometry()
target_distance = 0
def get_pose(initial_pose_tmp):
global initial_pose
initial_pose = initial_pose_tmp
def get_target(target_pose_tmp):
global target_pose
target_pose = target_pose_tmp
def actuator_ctrl():
actuator_vel = 15
return actuator_vel
def thruster_ctrl_msg():
from nav_msgs.msg import Odometry
import rospy
import numpy as np
import matplotlib.pyplot as plt
import rospkg
queue = []
# pub = rospy.Publisher("/gx4_45_imu/data", Imu, queue_size=1)
# pub = rospy.Publisher("/pwm_auv_state", Float64[], queue_size=1)
count = 0
# r = 0.05
path = rospkg.RosPack().get_path('zeabus_sensor') + '/scripts'
rate = 1 / 20.
f = open(path + '/auv_state_pwm_00.csv', 'w+')
pwm = Pwm()
pwm.pwm = [1500] * 8
auv_state = Odometry()
imu = Imu()
get_data = True
pub_pwm = rospy.Publisher("/pwm/sub_sampling", Pwm, queue_size=1)
pub_imu = rospy.Publisher("/imu/sub_sampling", Imu, queue_size=1)
pub_state = rospy.Publisher("/state/sub_sampling", Odometry, queue_size=1)
def change_rate():
global rate, auv_state, pwm, count, f, get_data, imu
pub_imu.publish(imu)
pub_pwm.publish(pwm)
pub_state.publish(auv_state)
string = str(imu.linear_acceleration.x) + ", " + str(
imu.linear_acceleration.y) + "\n"
frame_R = np.zeros((480, 640, 1), np.uint8)
frame_L_prev = np.zeros((480, 640, 1), np.uint8)
frame_R_prev = np.zeros((480, 640, 1), np.uint8)
#ros Images
left_image = Image()
right_image = Image()
left_featured_image = Image()
right_featured_image = Image()
matched_featured_image = Image()
flow_left_matched_featured_image = Image()
flow_left_image = Image()
debug_image = Image()
#relative pose
pose_rel = Odometry()
def pose_estimation():
global f, B
global frame_L, frame_R, frame_L_prev, frame_R_prev
global left_image, right_image, img_L, img_R
global left_featured_image, right_featured_image, matched_featured_image
global img_L, frame_L, frame_L_prev, dt_L
global flow_left_matched_featured_image, flow_left_image
global height, width, scale
frame_L_prev = frame_L
frame_R_prev = frame_R
try:
def poll(self):
now = rospy.Time.now()
if now > self.t_next:
# Read the encoders
try:
self.diagnostics.reads += 1
self.diagnostics.total_reads += 1
left_enc, right_enc = self.arduino.get_encoder_counts()
self.diagnostics.freq_diag.tick()
except:
self.diagnostics.errors += 1
self.bad_encoder_count += 1
rospy.logerr("Encoder exception count: " +
str(self.bad_encoder_count))
return
# Check for jumps in encoder readings
if self.detect_enc_jump_error:
try:
#rospy.loginfo("Left: %d LEFT: %d Right: %d RIGHT: %d", left_enc, self.enc_left, right_enc, self.enc_right)
enc_jump_error = False
if abs(right_enc -
self.enc_right) > self.enc_jump_error_threshold:
self.diagnostics.errors += 1
self.bad_encoder_count += 1
rospy.logerr("RIGHT encoder jump error from %d to %d",
self.enc_right, right_enc)
self.enc_right = right_enc
enc_jump_error = True
if abs(left_enc -
self.enc_left) > self.enc_jump_error_threshold:
self.diagnostics.errors += 1
self.bad_encoder_count += 1
rospy.logerr("LEFT encoder jump error from %d to %d",
self.enc_left, left_enc)
self.enc_left = left_enc
enc_jump_error = True
if enc_jump_error:
return
except:
pass
dt = now - self.then
self.then = now
dt = dt.to_sec()
# Calculate odometry
if self.enc_left == None:
dright = 0
dleft = 0
else:
dright = (right_enc - self.enc_right) / self.ticks_per_meter
dleft = (left_enc - self.enc_left) / self.ticks_per_meter
self.enc_right = right_enc
self.enc_left = left_enc
dxy_ave = self.odom_linear_scale_correction * (dright +
dleft) / 2.0
dth = self.odom_angular_scale_correction * (
dright - dleft) / float(self.wheel_track)
vxy = dxy_ave / dt
vth = dth / dt
if (dxy_ave != 0):
dx = cos(dth) * dxy_ave
dy = -sin(dth) * dxy_ave
self.x += (cos(self.th) * dx - sin(self.th) * dy)
self.y += (sin(self.th) * dx + cos(self.th) * dy)
if (dth != 0):
self.th += dth
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = sin(self.th / 2.0)
quaternion.w = cos(self.th / 2.0)
# Create the odometry transform frame broadcaster.
if self.publish_odom_base_transform:
self.odomBroadcaster.sendTransform(
(self.x, self.y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
rospy.Time.now(), self.base_frame, "odom")
odom = Odometry()
odom.header.frame_id = "odom"
odom.child_frame_id = self.base_frame
odom.header.stamp = now
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.twist.twist.linear.x = vxy
odom.twist.twist.linear.y = 0
odom.twist.twist.angular.z = vth
self.current_speed = Twist()
self.current_speed.linear.x = vxy
self.current_speed.angular.z = vth
"""
Covariance values taken from Kobuki node odometry.cpp at:
https://github.com/yujinrobot/kobuki/blob/indigo/kobuki_node/src/library/odometry.cpp
Pose covariance (required by robot_pose_ekf) TODO: publish realistic values
Odometry yaw covariance must be much bigger than the covariance provided
by the imu, as the later takes much better measures
"""
odom.pose.covariance[0] = 0.1
odom.pose.covariance[7] = 0.1
if self.use_imu_heading:
#odom.pose.covariance[35] = 0.2
odom.pose.covariance[35] = 0.05
else:
odom.pose.covariance[35] = 0.05
odom.pose.covariance[
14] = sys.float_info.max # set a non-zero covariance on unused
odom.pose.covariance[
21] = sys.float_info.max # dimensions (z, pitch and roll); this
odom.pose.covariance[
28] = sys.float_info.max # is a requirement of robot_pose_ekf
self.odomPub.publish(odom)
if now > (self.last_cmd_vel + rospy.Duration(self.timeout)):
self.v_des_left = 0
self.v_des_right = 0
#self.v_left = 0
#self.v_right = 0
if self.v_left < self.v_des_left:
self.v_left += self.max_accel
if self.v_left > self.v_des_left:
self.v_left = self.v_des_left
else:
self.v_left -= self.max_accel
if self.v_left < self.v_des_left:
self.v_left = self.v_des_left
if self.v_right < self.v_des_right:
self.v_right += self.max_accel
if self.v_right > self.v_des_right:
self.v_right = self.v_des_right
else:
self.v_right -= self.max_accel
if self.v_right < self.v_des_right:
self.v_right = self.v_des_right
# Set motor speeds in encoder ticks per PID loop
if not self.stopped:
rospy.loginfo("left: {0}".format(self.v_left))
rospy.loginfo("des_left: {0}".format(self.v_des_left))
self.arduino.drive(self.scale(self.v_left),
self.scale(self.v_right))
self.t_next = now + self.t_delta
from std_msgs.msg import String, Float32, UInt16
from sensor_msgs.msg import Imu, JointState, Joy
from geometry_msgs.msg import Vector3Stamped, Twist
from nav_msgs.msg import Odometry
from DecayFilter import DecayFilter
import csv
CONFIG_ENABLE_LOG = False
CONFIG_DEBUG_PUBLISHER = False
CONFIG_DEBUG_PRINT_DATA_SOURCE = True
CONFIG_TOPIC_WHEELODOM = '/ak1/odom'
CONFIG_TOPIC_REFODOM = '/vive/LHR_0EB0243A_odom' # ak1: /vive/LHR_0EB0243A_odom, ak2: /vive/LHR_08DF7BFF_odom
ak_wheelodom = Odometry()
ak_refodom = Odometry()
def normpdf(x, mu, sigma2):
return np.exp(-(x - mu)**2 / (2 * sigma2)) / np.sqrt(2 * np.pi * sigma2)
def Pr_L_consistent(L):
return normpdf(L, 0, 0.08947) * 2
def Pr_L_diverged(L):
return normpdf(L, 0.40272, 0.10208)
def auto_demo():
rospy.init_node('autonomous_demo')
#Requirements
pub1 = rospy.Publisher('/gps/fix', NavSatFix, queue_size =10)
pub2 = rospy.Publisher('/imu/data', Imu ,queue_size = 10)
pub3 = rospy.Publisher('/odometry/wheel', Odometry, queue_size = 10)
pub4 = rospy.Publisher('/gps_waypoint_handler/status', String, queue_size = 10)
#Autonomous movement
#pub5 = rospy.Publisher('/move_base/status',GoalStatusArray,queue_size=10)
#Image Detect
pub6 = rospy.Publisher('/image_detect_topic',String,queue_size=10)
#Image Reach
pub7 = rospy.Publisher('/image_reach_topic',String,queue_size=10)
rate = rospy.Rate(10) # 10hz
count = 0
while not rospy.is_shutdown():
print("0: All sensors are good.")
print("1: All sensors except encoder are good.")
print("2: Damaged Sensors")
print("3: Got Waypoint")
print("4: Did not get any waypoint")
print("5: Reached to way point")
print("6: Did not Reached to way point")
print("7: Detected Image")
print("8: Did not detect Image")
print("9: Reached Image")
print("10: Did not reached image")
imuMsg = Imu()
imuMsg.orientation.x = 5
imuMsg.orientation.y = 5
gpsMsg = NavSatFix()
gpsMsg.latitude = 5
gpsMsg.longitude = 5
encoderMsg = Odometry()
encoderMsg.pose.pose.position.x = 5
encoderMsg.pose.pose.position.y = 5
wpStatusMsgLow = GoalStatus()
wpStatusMsgLow.status = 3
wpStatusArray =[]
wpStatusArray.append(wpStatusMsgLow)
wpStatusMsg = GoalStatusArray()
wpStatusMsg.status_list = wpStatusArray
userInput = raw_input()
if userInput == "0": #All sensors are good.
pub1.publish(gpsMsg)
pub2.publish(imuMsg)
pub3.publish(encoderMsg)
elif userInput == "1": # All sensors except encoder are good.
pub1.publish(gpsMsg)
pub2.publish(imuMsg)
#pub3.publish("0")
elif userInput == "2": #Damaged Sensors
pub1.publish(gpsMsg)
#pub2.publish("0")
pub3.publish(encoderMsg)
elif userInput == "3": # Got Waypoint
pub4.publish("1")
#elif userInput == "4": #Did not get any waypoint
#pub4.publish("0")
elif userInput == "5": #Reached to way point
#pub5.publish(wpStatusMsg)
pub4.publish("2")
#elif userInput == "6": #Did not reached to way point
#pub5.publish("0")
elif userInput == "7": #Detected Image
pub6.publish("1")
elif userInput == "8": #Did not Detect Image
pub6.publish("0")
elif userInput == "9": #Reached Image
pub7.publish("1")
elif userInput == "10": #Did not reached image
pub7.publish("0")
else:
print("Invalid entry")
rate.sleep();
def pose_update(self, rmp):
"""
Read in the current RMP feedback and publish the pose
:param rmp: the RMP feedback message
"""
rmp_items = rmp.sensor_items
rmp_values = rmp.sensor_values
time_msg_received = rmp.header.stamp.secs + 10**-9 * rmp.header.stamp.nsecs
"""
get the values for the feedback items needed
angles and angle rates are flipped because the RMP's do not
follow the right hand rule convention
"""
for x in range(0, len(rmp_items)):
if rmp_items[x] == 'linear_vel_mps':
lin_vel = rmp_values[x]
elif rmp_items[x] == 'linear_pos_m':
lin_pos = rmp_values[x]
elif rmp_items[x] == 'differential_wheel_vel_rps':
diff_rate = -1 * rmp_values[x]
elif rmp_items[x] == 'angle_target_deg':
ang_targ = (rmp_values[x] - 90) * math.pi / 180
elif rmp_items[x] == 'pse_pitch_deg':
pitch = (-1 * rmp_values[x]) * math.pi / 180
elif rmp_items[x] == 'pse_roll_deg':
roll = (-1 * rmp_values[x]) * math.pi / 180
"""
Segway RMP base tends to drift when stationary, basic
filter to ignore low differences and reduce/stop drift
"""
if diff_rate >= -0.005 and diff_rate <= 0.005:
diff_rate = 0.0
"""
Calculate the new pose based on the feedback from the Segway
and the time difference from the previous calculation
"""
yaw = self.prev_yaw + diff_rate * (time_msg_received - self.prev_time)
x_pos = self.prev_x_pos + (lin_pos - self.prev_lin_pos) * math.cos(yaw)
y_pos = self.prev_y_pos + (lin_pos - self.prev_lin_pos) * math.sin(yaw)
# store the current values to be used in the next iteration
self.prev_lin_pos = lin_pos
self.prev_x_pos = x_pos
self.prev_y_pos = y_pos
self.prev_yaw = yaw
self.prev_time = time_msg_received
# create quaternion array from rmp and IMU data
quaternion = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
# make and publish the odometry message
odom = Odometry()
odom.header.stamp = rospy.Time.now()
odom.header.frame_id = "odom"
odom.child_frame_id = "base_footprint"
odom.pose.pose.position.x = x_pos
odom.pose.pose.position.y = y_pos
odom.pose.pose.position.z = 0.0
odom.pose.pose.orientation.x = quaternion[0]
odom.pose.pose.orientation.y = quaternion[1]
odom.pose.pose.orientation.z = quaternion[2]
odom.pose.pose.orientation.w = quaternion[3]
odom.twist.twist.linear.x = lin_vel * math.cos(ang_targ)
odom.twist.twist.linear.y = lin_vel * math.sin(ang_targ)
odom.twist.twist.angular.z = diff_rate
self.odomPub.publish(odom)
# publish the transform from odom to the base footprint
if self.publish_tf:
self.tfBroadCast.sendTransform((x_pos, y_pos, 0.0), quaternion,
rospy.Time.now(), '/base_footprint',
'/odom')
def __init__(self, parent=None):
super(PaintWidget, self).__init__(parent)
# geometry parameters
self.geometry = Geometry()
self.scaleOnField = 1.0
self.world_height = 0.0
self.world_width = 0.0
self.trans = QPointF(0.0, 0.0) # 慢性的なトランス
self.mouseTrans = QPointF(0.0, 0.0) # マウス操作で発生する一時的なトランス
self.scale = QPointF(1.0, 1.0)
self.clickPoint = QPointF(0.0, 0.0)
self._current_mouse_pos = QPointF(0.0, 0.0)
# Colors
self.friendDrawColor = Qt.cyan
self.enemyDrawColor = Qt.yellow
self.friend_color = rospy.get_param("friend_color", "blue")
if self.friend_color != "blue":
self.friendDrawColor = Qt.yellow
self.enemyDrawColor = Qt.cyan
self.targetPosDrawColor = QColor(102, 0, 255, 100)
self.avoidingPointDrawColor = QColor(255, 0, 0, 100)
# Replace
self._CLICK_POS_THRESHOLD = 0.1
self._CLICK_VEL_ANGLE_THRESHOLD = self._CLICK_POS_THRESHOLD + 0.1
self._VEL_GAIN = 3.0
self._VEL_MAX = 8.0
self._replace_func = None
self._replace_id = 0
self._replace_is_yellow = False
self._replace_is_friend = False
# Status
self._should_rotate_world = False
self._can_replace = False
self._is_ballpos_replacement = False
self._is_ballvel_replacement = False
self._is_robotpos_replacement = False
self._is_robotangle_replacement = False
# Configs
# This function enables mouse tracking without pressing mouse button
self.setMouseTracking(True)
# Subscribers
self.field_geometry = GeometryFieldSize()
self.sub_geometry = rospy.Subscriber("geometry_field_size",
GeometryFieldSize,
self.callbackGeometry)
self.ballOdom = Odometry()
self.sub_ballPosition = rospy.Subscriber("ball_observer/estimation",
Odometry,
self.callbackBallOdom)
self.friendsIDArray = UIntArray()
self.sub_friendsID = rospy.Subscriber("existing_friends_id", UIntArray,
self.callbackFriendsID)
self._ID_MAX = rospy.get_param('id_max', 12)
self.friendOdoms = [Odometry()] * self._ID_MAX
self.sub_friendOdoms = []
self.enemyIDArray = UIntArray()
self.sub_enemiesID = rospy.Subscriber("existing_enemies_id", UIntArray,
self.callbackEnemiesID)
self.refereeBranch = String()
self.sub_referee_branch = rospy.Subscriber("referee_branch", String,
self.callbackRefereeBranch)
self.passTargetPointFrameCount = 0
self.passTargetPoint = Point()
self.sub_passTargetPoint = rospy.Subscriber(
"pass_target_point", Point, self.callbackPassTargetPoint)
self.debugPoint = [Point()] * 10
rospy.Subscriber("debug_point_0", Point, self.callbackDebugPoint0)
rospy.Subscriber("debug_point_1", Point, self.callbackDebugPoint1)
rospy.Subscriber("debug_point_2", Point, self.callbackDebugPoint2)
rospy.Subscriber("debug_point_3", Point, self.callbackDebugPoint3)
rospy.Subscriber("debug_point_4", Point, self.callbackDebugPoint4)
rospy.Subscriber("debug_point_5", Point, self.callbackDebugPoint5)
rospy.Subscriber("debug_point_6", Point, self.callbackDebugPoint6)
rospy.Subscriber("debug_point_7", Point, self.callbackDebugPoint7)
rospy.Subscriber("debug_point_8", Point, self.callbackDebugPoint8)
rospy.Subscriber("debug_point_9", Point, self.callbackDebugPoint9)
self.enemyOdoms = [Odometry()] * self._ID_MAX
self.sub_enemyOdoms = []
self.targetPositions = [PoseStamped()] * self._ID_MAX
self.sub_targetPositions = []
self.targetVelocities = [TwistStamped()] * self._ID_MAX
self.sub_targetVelocities = []
self.targetIsPosition = [False] * self._ID_MAX
self.avoidingPoints = [Point()] * self._ID_MAX
self.sub_avoidingPoints = []
for i in range(self._ID_MAX):
strID = str(i)
topicFriend = "robot_" + strID + "/odom"
topicEnemy = "enemy_" + strID + "/odom"
topicPosition = "robot_" + strID + "/move_base_simple/goal"
topicVelocity = "robot_" + strID + "/move_base_simple/target_velocity"
topicAvoidingPoint = "robot_" + strID + "/avoiding_point"
self.sub_friendOdoms.append(
rospy.Subscriber(topicFriend,
Odometry,
self.callbackFriendOdom,
callback_args=i))
self.sub_enemyOdoms.append(
rospy.Subscriber(topicEnemy,
Odometry,
self.callbackEnemiesOdom,
callback_args=i))
self.sub_targetPositions.append(
rospy.Subscriber(topicPosition,
PoseStamped,
self.callbackTargetPosition,
callback_args=i))
self.sub_targetVelocities.append(
rospy.Subscriber(topicVelocity,
TwistStamped,
self.callbackTargetVelocity,
callback_args=i))
self.sub_avoidingPoints.append(
rospy.Subscriber(topicAvoidingPoint,
Point,
self.callbackAvoidingPoint,
callback_args=i))
# Publishers
self._pub_replace_ball = rospy.Publisher('replacement_ball',
ReplaceBall,
queue_size=10)
self._pub_replace_robot = rospy.Publisher('replacement_robot',
ReplaceRobot,
queue_size=10)
self.resizeDrawWorld()
def DataReader():
data = ReadFile(FILE_NAME)
# IMU
imu_msg = Imu()
imu_msg.orientation.x = data[0][1:-1,0]
imu_msg.orientation.y = data[0][1:-1,1]
imu_msg.orientation.z = data[0][1:-1,2]
imu_msg.orientation.w = data[0][1:-1,3]
imu_msg.linear_acceleration.x=data[0][1:-1,4]
imu_msg.linear_acceleration.y=data[0][1:-1,5]
imu_msg.linear_acceleration.z=data[0][1:-1,6]
imu_msg.angular_velocity.x=data[0][1:-1,7]
imu_msg.angular_velocity.y=data[0][1:-1,8]
imu_msg.angular_velocity.z=data[0][1:-1,9]
imu_time = data[0][1:-1,10]
imu_length = len(imu_msg.orientation.x)
imu_roll_angle=np.zeros(imu_length,dtype=float)
imu_pitch_angle=np.zeros(imu_length,dtype=float)
imu_yaw_angle=np.zeros(imu_length,dtype=float)
for i in range(imu_length):
x = imu_msg.orientation.x[i]
y = imu_msg.orientation.y[i]
z = imu_msg.orientation.z[i]
w = imu_msg.orientation.w[i]
imu_roll_angle[i] = math.atan2(2 * (y*z + w*x), w*w - x*x - y*y + z*z)*180/PI
imu_pitch_angle[i] = math.asin(-2 * (x*z - w*y))*180/PI
imu_yaw_angle[i] = math.atan2(2 * (x*y + w*z), w*w + x*x - y*y - z*z)*180/PI
# vehicle_state
vehicle_msg =Odometry()
vehicle_msg.pose.pose.position.x =data[1][1:-1,0]
vehicle_msg.pose.pose.position.y =data[1][1:-1,1]
vehicle_msg.pose.pose.position.z =data[1][1:-1,2]
vehicle_msg.pose.pose.orientation.x = data[1][1:-1,3]
vehicle_msg.pose.pose.orientation.y = data[1][1:-1,4]
vehicle_msg.pose.pose.orientation.z = data[1][1:-1,5]
vehicle_msg.pose.pose.orientation.w = data[1][1:-1,6]
vehicle_msg.twist.twist.linear.x= data[1][1:-1,7]
vehicle_msg.twist.twist.linear.y= data[1][1:-1,8]
vehicle_msg.twist.twist.linear.z= data[1][1:-1,9]
vehicle_msg.twist.twist.angular.x= data[1][1:-1,10]
vehicle_msg.twist.twist.angular.y= data[1][1:-1,11]
vehicle_msg.twist.twist.angular.z= data[1][1:-1,12]
vehicle_time = data[1][1:-1,13]
odom_length = len(vehicle_msg.pose.pose.orientation.x)
odom_roll_angle=np.zeros(odom_length,dtype=float)
odom_pitch_angle=np.zeros(odom_length,dtype=float)
odom_yaw_angle=np.zeros(odom_length,dtype=float)
for i in range(odom_length):
x = vehicle_msg.pose.pose.orientation.x[i]
y = vehicle_msg.pose.pose.orientation.y[i]
z = vehicle_msg.pose.pose.orientation.z[i]
w = vehicle_msg.pose.pose.orientation.w[i]
odom_roll_angle[i] = math.atan2(2 * (y*z + w*x), w*w - x*x - y*y + z*z)*180/PI
odom_pitch_angle[i] = math.asin(-2 * (x*z - w*y))*180/PI
odom_yaw_angle[i] = math.atan2(2 * (x*y + w*z), w*w + x*x - y*y - z*z)*180/PI
# GPS
pos_gps = NavSatFix()
pos_gps.latitude = data[2][1:-1,1]
pos_gps.longitude = data[2][1:-1,2]
pos_gps.altitude = data[2][1:-1,3]
gps_time = data[2][1:-1,4]
# chassis
chassis_msg_Steer = data[3][1:-1,0]
chassis_msg_Throttle = data[3][1:-1,1]
chassis_msg_Brake = data[3][1:-1,2]
chassis_msg_Wheel = data[3][1:-1,3]
chassis_msg_Speed = data[3][1:-1,4]
chassis_msg_BucketAngle = data[3][1:-1,5]
chassis_msg_EngineSpeed = data[3][1:-1,6]
chassis_msg_EngineTorque = data[3][1:-1,7]
chassis_time = data[3][1:-1,8]
plt.figure(1)
loc='center'
font_dict={'fontsize': 20,\
'fontweight' : 8.2,\
'verticalalignment': 'baseline',\
'horizontalalignment': loc}
plt.title('trajectory',fontdict=font_dict,loc=loc)
plt.plot(pos_gps.latitude,pos_gps.longitude, color='cyan', label='desired_trajectory')
plt.xlabel('latitude')
plt.ylabel('longitude')
plt.figure(2)
loc='center'
font_dict={'fontsize': 20,\
'fontweight' : 8.2,\
'verticalalignment': 'baseline',\
'horizontalalignment': loc}
plt.title('Pitch Angle Comparison',fontdict=font_dict,loc=loc)
plt.plot(vehicle_time,odom_pitch_angle, color='cyan', label='pitch angle from odom')
plt.plot(imu_time,-imu_pitch_angle, color='r', label='pitch angle from imu')
plt.legend()
plt.ylabel('pitch angle/degree')
plt.xlabel('time/s')
plt.figure(3)
loc='center'
font_dict={'fontsize': 20,\
'fontweight' : 8.2,\
'verticalalignment': 'baseline',\
'horizontalalignment': loc}
plt.title('trajectory',fontdict=font_dict,loc=loc)
plt.plot(vehicle_msg.pose.pose.position.x ,vehicle_msg.pose.pose.position.y, color='cyan', label='desired_trajectory')
plt.xlabel('X')
plt.ylabel('Y')
plt.show()
def __init__(self):
#init stuff
#get stuff
self.num_landmarks = 12
# Estimator stuff
# x = pn, pe, pd, phi, theta, psi
self.xhat = np.zeros((9 + 3*self.num_landmarks, 1))
self.xhat_odom = Odometry()
# Covariance matrix
self.P = np.zeros((9 + 3*self.num_landmarks, 9 + 3*self.num_landmarks))
self.P[9:,9:] = np.eye(3*self.num_landmarks)*9999999.9 # Inf
self.Q = np.diag([10.0, 5.0, 5.0]) # meas noise
# Measurements stuff
# Truth
self.truth_pn = 0.0
self.truth_pe = 0.0
self.truth_pd = 0.0
self.truth_phi = 0.0
self.truth_theta = 0.0
self.truth_psi = 0.0
self.truth_p = 0.0
self.truth_q = 0.0
self.truth_r = 0.0
self.truth_pndot = 0.0
self.truth_pedot = 0.0
self.truth_pddot = 0.0
self.truth_u = 0.0
self.truth_v = 0.0
self.truth_w = 0.0
self.prev_time = 0.0
self.imu_az = 0.0
#aruco Stuff
self.aruco_location = {
76 :[-0.51, 2.302, -1.31],
245 :[1.455, 2.493, -1.486],
55 :[3.92, 1.333, -1.498],
110 :[3.964, -1.753,-1.566],
248 :[2.916, -2.543,-1.537],
64 :[1.181, -2.471,-1.581],
25 :[-1.593,-2.488,-1.572],
121 :[-3.528,-0.658,-1.461],
5 :[-2.023, 2.462,-1.492],
}
self.landmark_number = {
76:[1],
245:[2],
55:[3],
110:[4],
248:[5],
64:[6],
25:[7],
121:[8],
5:[9],
}
# Number of propagate steps
self.N = 5
#Constants
self.g = 9.8
# ROS Stuff
# Init subscribers
self.truth_sub_ = rospy.Subscriber('/mocap/thor/pose', PoseStamped, self.truth_callback)
self.imu_sub_ = rospy.Subscriber('/imu/data', Imu, self.imu_callback)
self.velocity_sub_ = rospy.Subscriber('/velocities', Vector3Stamped, self.velocity_callback)
self.aruco_sub = rospy.Subscriber('/aruco/measurements', MarkerMeasurementArray, self.aruco_meas_callback )
# Init publishers
self.estimate_pub_ = rospy.Publisher('/ekf_estimate', Odometry, queue_size=10)
# # Init Timer
self.pub_rate_ = 100. #
self.update_timer_ = rospy.Timer(rospy.Duration(1.0/self.pub_rate_), self.pub_est)
def main():
global img_ball
global img_arr
global xs
global zs
global pred_xs
global pred_zs
rospy.init_node("colorOfBall")
image_sub = rospy.Subscriber("/camera/color/image_raw",Image,imageCallback)
depth_sub = rospy.Subscriber("/camera/depth/image_rect_raw",Image,depthCallback)
ballPos_pub = rospy.Publisher("/ball/position",Odometry,queue_size=10)
print("before wait")
rospy.wait_for_message("/camera/color/image_raw",Image)
rospy.wait_for_message("/camera/depth/image_rect_raw",Image)
print("after wait")
rate = rospy.Rate(20)
pixToDegree = float(58)/640
while not rospy.is_shutdown():
rospy.wait_for_message("/camera/color/image_raw",Image)
rospy.wait_for_message("/camera/depth/image_rect_raw",Image)
cv_image_hsv = cv2.cvtColor(img_ball,cv2.COLOR_BGR2HSV)
#(rows,cols,channels) = img_ball.shape
#print([rows,cols,channels])
#cv2.circle(cv_image,(320,240),10,(255,0,0))
mask = cv2.inRange(cv_image_hsv,orange_lower,orange_upper)
mask = cv2.erode(mask,None,iterations=1)
mask = cv2.dilate(mask,None,iterations=1)
# find contours in the mask and initialize the current
# (x, y) center of the ball
cnts = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_SIMPLE)
cnts = imutils.grab_contours(cnts)
center = None
# only proceed if at least one contour was found
x = 0
z = 0
if len(cnts) > 0:
# find the largest contour in the mask, then use
# it to compute the minimum enclosing circle and
# centroid
c = max(cnts, key=cv2.contourArea)
((img_ballx, img_bally), radius) = cv2.minEnclosingCircle(c)
M = cv2.moments(c)
z = toDistance(float(img_arr[int(img_bally),int(img_ballx)]))
#print(float(img_arr[img_bally,img_ballx]))
# only proceed if the radius meets a minimum size
if radius > 5 and radius < 200 and img_bally > 140 and (img_bally<380 and img_ballx<540):
# draw the circle and centroid on the frame,
# then update the list of tracked points
cv2.circle(img_ball, (int(img_ballx), int(img_bally)), int(radius),
(0, 0, 255), 2)
cv2.circle(mask,(int(img_ballx), int(img_bally)), int(radius),
(0, 0, 255), 2)
#print([int(img_ballx),int(img_bally),int(radius),z])
deg = pixToDegree*float(img_ballx - 320)
x = z*np.tan(deg*pi/180)
pos = Odometry()
if x!=0 and z!=0:
print([x,z])
xs = np.append(xs,x)
zs = np.append(zs,z)
pos.pose.pose.position.x = x
pos.pose.pose.position.z = z
ballPos_pub.publish(pos)
cv2.imshow("Image window",img_ball)
cv2.imshow("Mask",mask)
#print([0,0,0] + cv_image_hsv[320,240,:])
cv2.waitKey(1) & 0xFF
if xs.size > 20:
xs = xs[-19:]
if zs.size > 20:
zs = zs[-19:]
if xs.size > 10:
pred_zs = findLine(xs[-10:],zs[-10:])
#plt.plot(xs,zs)
#plt.plot(x,z,'x',markersize=12)
if show_ball:
marks.set_xdata(xs)
marks.set_ydata(zs)
current_mark.set_xdata(x)
current_mark.set_ydata(z)
if show_pred_path:
pred_line.set_ydata(pred_zs)
plt.draw()
plt.show()
rate.sleep()
cv2.destroyAllWindows()
def spin(self):
encoders = [0, 0]
self.x = 0 # position in xy plane
self.y = 0
self.th = 0
then = rospy.Time.now()
# things that don't ever change
scan_link = rospy.get_param('~frame_id', 'base_laser_link')
scan = LaserScan(header=rospy.Header(frame_id=scan_link))
scan.angle_min = 0
scan.angle_max = 6.26
scan.angle_increment = 0.017437326
scan.range_min = 0.020
scan.range_max = 5.0
odom = Odometry(header=rospy.Header(frame_id="odom"),
child_frame_id='base_link')
# main loop of driver
r = rospy.Rate(5)
self.robot.requestScan()
while not rospy.is_shutdown():
# prepare laser scan
scan.header.stamp = rospy.Time.now()
#self.robot.requestScan()
scan.ranges = self.robot.getScanRanges()
# get motor encoder values
left, right = self.robot.getMotors()
# send updated movement commands
self.robot.setMotors(
self.cmd_vel[0], self.cmd_vel[1],
max(abs(self.cmd_vel[0]), abs(self.cmd_vel[1])))
# ask for the next scan while we finish processing stuff
self.robot.requestScan()
# now update position information
dt = (scan.header.stamp - then).to_sec()
then = scan.header.stamp
d_left = (left - encoders[0]) / 1000.0
d_right = (right - encoders[1]) / 1000.0
encoders = [left, right]
dx = (d_left + d_right) / 2
dth = (d_right - d_left) / (BASE_WIDTH / 1000.0)
x = cos(dth) * dx
y = -sin(dth) * dx
self.x += cos(self.th) * x - sin(self.th) * y
self.y += sin(self.th) * x + cos(self.th) * y
self.th += dth
# prepare tf from base_link to odom
quaternion = Quaternion()
quaternion.z = sin(self.th / 2.0)
quaternion.w = cos(self.th / 2.0)
# prepare odometry
odom.header.stamp = rospy.Time.now()
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.twist.twist.linear.x = dx / dt
odom.twist.twist.angular.z = dth / dt
# publish everything
self.odomBroadcaster.sendTransform(
(self.x, self.y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w), then,
"base_link", "odom")
self.scanPub.publish(scan)
self.odomPub.publish(odom)
# wait, then do it again
r.sleep()
# shut down
self.robot.setLDS("off")
self.robot.setTestMode("off")
#! /usr/bin/env python
import rospy
from audibot_cfg.vehicle_ns import AudiBotNS
from std_msgs.msg import Header
from nav_msgs.msg import Odometry
from geometry_msgs.msg import TwistStamped
from gazebo_msgs.srv import GetModelState, GetModelStateRequest
audibot_obj = AudiBotNS()
audibot_obj.parse_args()
audibot_obj.exec_odom_cfg()
node_name, model_name, frame_id, topic_name = audibot_obj.get_odom_cfg()
rospy.init_node(node_name, anonymous=True)
odom_pub = rospy.Publisher(topic_name, Odometry, queue_size=10)
rospy.wait_for_service('/gazebo/get_model_state')
get_model_srv = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState)
odom = Odometry()
header = Header()
header.frame_id = frame_id
model = GetModelStateRequest()
model.model_name = model_name
r = rospy.Rate(10)
while not rospy.is_shutdown():
result = get_model_srv(model)
odom.pose.pose = result.pose
odom.twist.twist = result.twist
header.stamp = rospy.Time.now()
odom.header = header
odom_pub.publish(odom)
r.sleep()
from geometry_msgs.msg import Twist
from nav_msgs.msg import Odometry
from sensor_msgs.msg import Range
from std_msgs.msg import Int32
from bebop_msgs.msg import Ardrone3PilotingStateAltitudeChanged
from bebop_msgs.msg import Ardrone3PilotingStateAttitudeChanged
h_p = 0
x_p = 0
y_p = 0
z_o = 0
cnt = 0
mx = 0
my = 0
odom_var = Odometry()
def receive_odom(data):
global x_p, y_p
x_p = data.pose.pose.position.x
y_p = data.pose.pose.position.y
def cnt_callback(data):
global cnt
cnt = data.data
def my_callback(data):
global my
def odometry_calculation(self):
if self.skid_steer:
### Skid-Steering model
velocity_left, velocity_right = espeleo.left_right_velocity([
self.motor_velocity1, self.motor_velocity2,
self.motor_velocity3, self.motor_velocity4,
self.motor_velocity5, self.motor_velocity6
])
# Linear velocity
v_robot = (velocity_right + velocity_left) * (self.alpha_skid / 2)
# Angular velocity
w_robot = ((velocity_right - velocity_left) *
(self.alpha_skid / (2 * self.ycir_skid)))
if self.time_counter_aux == 0:
self.last_time = self.current_time
self.time_counter_aux = 1
# Velocity in the XY plane
vx_robot = v_robot * cos(self.th)
vy_robot = v_robot * sin(self.th)
# Calculating odometry
dt = self.current_time - self.last_time
delta_x = vx_robot * dt
delta_y = vy_robot * dt
delta_th = w_robot * dt
# Integrating pose
self.x += delta_x
self.y += delta_y
self.th += delta_th
else:
v_robot, w_robot = espeleo.get_espeleo_velocity([
self.motor_velocity1, self.motor_velocity2,
self.motor_velocity3, self.motor_velocity4,
self.motor_velocity5, self.motor_velocity6
])
# Velocity in the XY plane
vx_robot = v_robot * cos(self.th)
vy_robot = v_robot * sin(self.th)
if self.time_counter_aux == 0:
self.last_time = self.current_time
self.time_counter_aux = 1
# Calculating odometry
dt = self.current_time - self.last_time
delta_x = vx_robot * dt
delta_y = vy_robot * dt
delta_th = w_robot * dt
# Integrating pose
self.x += delta_x
self.y += delta_y
self.th += delta_th
vel_robot = Twist()
vel_robot.linear.x = v_robot
vel_robot.angular.z = w_robot
# Since all odometry is 6DOF we'll need a quaternion created from yaw
odom_quat = tf.transformations.quaternion_from_euler(0, 0, self.th)
# First, we'll publish the transform over tf
self.odom_broadcaster.sendTransform(
(self.x, self.y, 0.),
odom_quat,
rospy.Time.now(),
"base_link",
"odom" # odom
)
# next, we'll publish the odometry message over ROS
odom = Odometry()
odom.header.stamp = rospy.Time.now()
odom.header.frame_id = "odom"
# set the position
odom.pose.pose = Pose(Point(self.x, self.y, 0.),
Quaternion(*odom_quat))
# set the velocity
odom.child_frame_id = "base_link"
odom.twist.twist = Twist(Vector3(v_robot, 0, 0),
Vector3(0, 0, w_robot))
# Calculating the covariance based on the angular velocity
# if the robot is rotating, the covariance is higher than if its going straight
if abs(w_robot) > 0.2:
covariance_cons = 0.3
else:
covariance_cons = 0.05
odom.pose.covariance[0] = covariance_cons
odom.pose.covariance[7] = covariance_cons
odom.pose.covariance[35] = 100 * covariance_cons
odom.pose.covariance[1] = covariance_cons
odom.pose.covariance[6] = covariance_cons
odom.pose.covariance[31] = covariance_cons
odom.pose.covariance[11] = covariance_cons
odom.pose.covariance[30] = 10 * covariance_cons
odom.pose.covariance[5] = 10 * covariance_cons
odom.pose.covariance[14] = 0.1
odom.pose.covariance[21] = 0.1
odom.pose.covariance[28] = 0.1
odom.twist.covariance[0] = covariance_cons
odom.twist.covariance[7] = covariance_cons
odom.twist.covariance[35] = 100 * covariance_cons
odom.twist.covariance[1] = covariance_cons
odom.twist.covariance[6] = covariance_cons
odom.twist.covariance[31] = covariance_cons
odom.twist.covariance[11] = covariance_cons
odom.twist.covariance[30] = 10 * covariance_cons
odom.twist.covariance[5] = 10 * covariance_cons
odom.twist.covariance[14] = 0.1
odom.twist.covariance[21] = 0.1
odom.twist.covariance[28] = 0.1
# publish the message
self.odom_pub.publish(odom)
self.vel_pub.publish(vel_robot)
self.last_time = self.current_time
def spin(self):
encoders = [0, 0]
self.x = 0 # position in xy plane
self.y = 0
self.th = 0
then = rospy.Time.now()
# things that don't ever change
scan_link = rospy.get_param('~frame_id', 'base_laser_link')
scan = LaserScan(header=rospy.Header(frame_id=scan_link))
scan.angle_min = 0.0
scan.angle_max = 359.0 * pi / 180.0
scan.angle_increment = pi / 180.0
scan.range_min = 0.020
scan.range_max = 5.0
odom = Odometry(header=rospy.Header(frame_id="odom"),
child_frame_id='base_footprint')
button = Button()
sensor = Sensor()
self.robot.setBacklight(1)
self.robot.setLED("Green")
# main loop of driver
r = rospy.Rate(20)
cmd_rate = self.CMD_RATE
while not rospy.is_shutdown():
# notify if low batt
#if self.robot.getCharger() < 10:
# print "battery low " + str(self.robot.getCharger()) + "%"
# get motor encoder values
left, right = self.robot.getMotors()
cmd_rate = cmd_rate - 1
if cmd_rate == 0:
# send updated movement commands
#if self.cmd_vel != self.old_vel or self.cmd_vel == [0,0]:
# max(abs(self.cmd_vel[0]),abs(self.cmd_vel[1])))
#self.robot.setMotors(self.cmd_vel[0], self.cmd_vel[1], (abs(self.cmd_vel[0])+abs(self.cmd_vel[1]))/2)
self.robot.setMotors(
self.cmd_vel[0], self.cmd_vel[1],
max(abs(self.cmd_vel[0]), abs(self.cmd_vel[1])))
cmd_rate = self.CMD_RATE
self.old_vel = self.cmd_vel
# prepare laser scan
scan.header.stamp = rospy.Time.now()
self.robot.requestScan()
scan.ranges, scan.intensities = self.robot.getScanRanges()
# now update position information
dt = (scan.header.stamp - then).to_sec()
then = scan.header.stamp
d_left = (left - encoders[0]) / 1000.0
d_right = (right - encoders[1]) / 1000.0
encoders = [left, right]
#print d_left, d_right, encoders
dx = (d_left + d_right) / 2
dth = (d_right - d_left) / (self.robot.base_width / 1000.0)
x = cos(dth) * dx
y = -sin(dth) * dx
self.x += cos(self.th) * x - sin(self.th) * y
self.y += sin(self.th) * x + cos(self.th) * y
self.th += dth
#print self.x,self.y,self.th
# prepare tf from base_link to odom
quaternion = Quaternion()
quaternion.z = sin(self.th / 2.0)
quaternion.w = cos(self.th / 2.0)
# prepare odometry
odom.header.stamp = rospy.Time.now()
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.twist.twist.linear.x = dx / dt
odom.twist.twist.angular.z = dth / dt
# sensors
lsb, rsb, lfb, rfb = self.robot.getDigitalSensors()
# buttons
btn_soft, btn_scr_up, btn_start, btn_back, btn_scr_down = self.robot.getButtons(
)
# publish everything
self.odomBroadcaster.sendTransform(
(self.x, self.y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w), then,
"base_footprint", "odom")
self.scanPub.publish(scan)
self.odomPub.publish(odom)
button_enum = ("Soft_Button", "Up_Button", "Start_Button",
"Back_Button", "Down_Button")
sensor_enum = ("Left_Side_Bumper", "Right_Side_Bumper",
"Left_Bumper", "Right_Bumper")
for idx, b in enumerate(
(btn_soft, btn_scr_up, btn_start, btn_back, btn_scr_down)):
if b == 1:
button.value = b
button.name = button_enum[idx]
self.buttonPub.publish(button)
for idx, b in enumerate((lsb, rsb, lfb, rfb)):
if b == 1:
sensor.value = b
sensor.name = sensor_enum[idx]
self.sensorPub.publish(sensor)
# wait, then do it again
r.sleep()
# shut down
self.robot.setBacklight(0)
self.robot.setLED("Off")
self.robot.setLDS("off")
self.robot.setTestMode("off")
def talker():
rospy.init_node('infant_odometry', anonymous=True)
#rospy.Subscriber("/cmd_vel", Twist, cmdvel_callback)
rospy.Subscriber("/benz/raspi/encR", Int64, odom_callbackR)
rospy.Subscriber("/benz/raspi/encL", Int64, odom_callbackL)
#rospy.Subscriber("/imu", Imu, imu_callback)
rospy.Subscriber("/imu/serial", String, imuserial_callback)
pub = rospy.Publisher('/odom', Odometry, queue_size=10 )
odo=Odometry()
odo.header.frame_id='odom'
odo.child_frame_id='base_link'
current_time=time.time()
last_time=time.time()
#======= Parameter Setting with Parameter Server=======
# * If the parameter server didn't work, the following sequence would be passed.
if rospy.has_param('infant_learning_odometry/right_wheel_p'):
global D_RIGHT
D_RIGHT=rospy.get_param('infant_learning_odometry/right_wheel_p')
if rospy.has_param('infant_learning_odometry/left_wheel_p'):
global D_LEFT
PRIGHT=rospy.get_param('infant_learning_odometry/left_wheel_p')
if rospy.has_param('infant_learning_odometry/Tread'):
global TREAD
TREAD=rospy.get_param('infant_learning_odometry/Tread')
#print PRIGHT
#print PLEFT
#print TREAD
#===========================================================
odo.pose.pose.position.x=0
odo.pose.pose.orientation.w=0
odo.twist.twist.linear.x=0
odo.twist.twist.angular.z=0
r = rospy.Rate(10)
global dt
global x
global count
global imuYaw
global imuPitch
x=array([0,0,toRadian(0.0)]);
dt=0.1
t=0;
while not rospy.is_shutdown():
count=count+1
current_time=time.time()
dt=current_time-last_time
odo.header.seq=count
odo.header.stamp = rospy.Time.now()
odo.pose.pose.position.x=x[0]
odo.pose.pose.position.y=x[1]
roll=imuRoll
pitch=imuPitch
yaw = x[2]
q = quaternion_from_euler(roll,pitch,yaw)
odo.pose.pose.orientation.x = q[0]
odo.pose.pose.orientation.y = q[1]
odo.pose.pose.orientation.z = q[2]
odo.pose.pose.orientation.w = q[3]
odo.twist.twist.linear.x=u[0]
odo.twist.twist.angular.z=u[1]
pub.publish(odo)
# odom tf
if bc is not None:
bc.sendTransform((x[0],x[1],0.0),q,rospy.Time.now(),'base_link','odom')
if count%2000==0:
print "%8.2f" %x[0],"%8.2f" %x[1],"%8.2f" %toDegree(x[2])
#print (last_time-current_time)
#print t
#result.write('%f,%f,%f,%f,%f\n' %(x[0],x[1],x[2],u[0],u[1]))
last_time=time.time()
t+=dt
r.sleep()
rospy.spin()
from geometry_msgs.msg import Vector3
from geometry_msgs.msg import Point
from sensor_msgs.msg import LaserScan
from interactive_markers.interactive_marker_server import *
from visualization_msgs.msg import *
import numpy as np
import random
from numpy import ones, vstack
from numpy.linalg import lstsq
import math
from nav_msgs.msg import Odometry
from tf.transformations import euler_from_quaternion
#goal is (4.5,9.0)
odata = Odometry()
ldata = LaserScan
wallfollow = False
def pol2cart(rt, tt):
x = rt * np.cos(tt)
y = rt * np.sin(tt)
return (x, y)
def distance(m, c, p, q):
d = abs((m * p) + (-1 * q) + c)
d = d / math.sqrt(m * m + 1)
return d
def callback(self,data):
rospy.loginfo_once("received data lat lon usbl")
if(self.paramsReady()):
rospy.loginfo_once("Valid survey params found: transform_pose starting")
crs_wgs = proj.Proj(init='epsg:4326') # assuming you're using WGS84 geographic
utm_zone = rospy.get_param("/utm_zone")
crs_bng = proj.Proj(init=utm_zone) # use a locally appropriate projected CRS
originLon = rospy.get_param("/origin_lon")
originLat = rospy.get_param("/origin_lat")
# then cast your geographic coordinate pair to the projected system
xoff, yoff = proj.transform(crs_wgs, crs_bng, originLon, originLat)
zoff = rospy.get_param("/origin_z",0)
# input from NavSatFix
lon = data.longitude;
lat = data.latitude;
x, y = proj.transform(crs_wgs, crs_bng, lon, lat)
#x = x-xoff
#y = y-yoff
# small correction
#x = x-(193543.42 +1371)
#y = y-(2208649.25 -3967)
x = x-193543.42 -1371
y = y-2208649.25 +3967
z = 0;
static_transformStamped = geometry_msgs.msg.TransformStamped()
static_transformStamped.header.stamp = rospy.Time.now()
static_transformStamped.header.frame_id = "utm"
static_transformStamped.child_frame_id = "map"
static_transformStamped.transform.translation.x = xoff
static_transformStamped.transform.translation.y = yoff
static_transformStamped.transform.translation.z = 0
quat = tf.transformations.quaternion_from_euler(0,0,0)
static_transformStamped.transform.rotation.x = quat[0]
static_transformStamped.transform.rotation.y = quat[1]
static_transformStamped.transform.rotation.z = quat[2]
static_transformStamped.transform.rotation.w = quat[3]
self.br.sendTransform(static_transformStamped)
# self.br.sendTransform((xoff, yoff, 0),
# tf.transformations.quaternion_from_euler(0, 0, 0),
# data.header.stamp,
# "map",
# "utm")
# output message
transformed_msg = Odometry()
transformed_msg.header.stamp = data.header.stamp;
transformed_msg.child_frame_id = "base_link"
if x== float('Inf') or y==float('Inf'):
pass
else:
transformed_msg.pose.pose.orientation.w = 1.0
transformed_msg.pose.pose.position.x=x
transformed_msg.pose.pose.position.y=y
transformed_msg.pose.pose.position.z=z
transformed_msg.header.frame_id="map"
transformed_msg.pose.covariance = [100., 0., 0., 0., 0., 0.,
0., 100., 0., 0., 0., 0.,
0., 0., 1., 0., 0., 0.,
0., 0., 0., 1., 0., 0.,
0., 0., 0., 0., 1., 0.,
0., 0., 0., 0., 0., 1.]
self.pub.publish(transformed_msg)
if self.pressed:
self.button_pub.publish(transformed_msg)
self.pressed = False
else:
rospy.loginfo_once("Receiving odom message but survey params not ready. waiting...")
def odom_callback(self, odom_msg):
self.slow_vel_count += 1
if (self.initialization):
self.initialization = 0
w_0 = 1. / self.M * np.ones((5, 1))
self.gsf_obj = GSF(self.M, w_0)
for i in range(self.M):
# Establish x_0
if i == 0:
x_0 = np.empty([5, 1])
x_0[0] = odom_msg.pose.pose.position.x + 0.1
x_0[1] = odom_msg.pose.pose.position.y
orientation_quat = odom_msg.pose.pose.orientation
orientation_euler = tf.transformations.euler_from_quaternion(
[
orientation_quat.x, orientation_quat.y,
orientation_quat.z, orientation_quat.w
])
x_0[2] = orientation_euler[2]
x_0[3] = 0
x_0[4] = 0
if i == 1:
x_0 = np.empty([5, 1])
x_0[0] = odom_msg.pose.pose.position.x - 0.1
x_0[1] = odom_msg.pose.pose.position.y
orientation_quat = odom_msg.pose.pose.orientation
orientation_euler = tf.transformations.euler_from_quaternion(
[
orientation_quat.x, orientation_quat.y,
orientation_quat.z, orientation_quat.w
])
x_0[2] = orientation_euler[2]
x_0[3] = 0
x_0[4] = 0
if i == 2:
x_0 = np.empty([5, 1])
x_0[0] = odom_msg.pose.pose.position.x
x_0[1] = odom_msg.pose.pose.position.y + 0.1
orientation_quat = odom_msg.pose.pose.orientation
orientation_euler = tf.transformations.euler_from_quaternion(
[
orientation_quat.x, orientation_quat.y,
orientation_quat.z, orientation_quat.w
])
x_0[2] = orientation_euler[2]
x_0[3] = 0
x_0[4] = 0
if i == 3:
x_0 = np.empty([5, 1])
x_0[0] = odom_msg.pose.pose.position.x
x_0[1] = odom_msg.pose.pose.position.y - 0.1
orientation_quat = odom_msg.pose.pose.orientation
orientation_euler = tf.transformations.euler_from_quaternion(
[
orientation_quat.x, orientation_quat.y,
orientation_quat.z, orientation_quat.w
])
x_0[2] = orientation_euler[2]
x_0[3] = 0
x_0[4] = 0
if i == 4:
x_0 = np.empty([5, 1])
x_0[0] = odom_msg.pose.pose.position.x
x_0[1] = odom_msg.pose.pose.position.y
self.x_prev = x_0[0]
self.y_prev = x_0[1]
orientation_quat = odom_msg.pose.pose.orientation
orientation_euler = tf.transformations.euler_from_quaternion(
[
orientation_quat.x, orientation_quat.y,
orientation_quat.z, orientation_quat.w
])
x_0[2] = orientation_euler[2]
x_0[3] = 0
x_0[4] = 0
# Establish P_0
P_0 = 0.1 * np.identity(5)
# Initialize EKFs
self.gsf_obj.gsf_fill_dict(i, x_0, P_0)
self.time_prev = odom_msg.header.stamp
self.slow_time_prev = self.time_prev
time_curr = odom_msg.header.stamp
d = time_curr - self.time_prev
delta_t = d.to_sec()
# Establish u_k from imu_msg
imu_msg = rospy.wait_for_message("/L01/imu_raw", Imu)
u = np.empty([3, 1])
u[0][0] = imu_msg.angular_velocity.z
u[1][0] = imu_msg.linear_acceleration.x
u[2][0] = imu_msg.linear_acceleration.y
self.gsf_obj.gsf_predict(u, delta_t)
# Establish y_k+1 from odom msg
y = np.empty([3, 1])
y[0][0] = odom_msg.pose.pose.position.x
y[1][0] = odom_msg.pose.pose.position.y
orientation_quat = odom_msg.pose.pose.orientation
orientation_euler = tf.transformations.euler_from_quaternion([
orientation_quat.x, orientation_quat.y, orientation_quat.z,
orientation_quat.w
])
y[2][0] = orientation_euler[2]
# Can possibly create fake velocities here?
if (self.slow_vel_count % 100 == 0):
# Find Current values
slow_time_curr = odom_msg.header.stamp
x_curr = y[0]
y_curr = y[1]
slow_time_delta = slow_time_curr - self.slow_time_prev
delta_t_slow = slow_time_delta.to_sec()
# Find velocitie estimates
x_vel = (x_curr - self.x_prev) / delta_t_slow
y_vel = (y_curr - self.y_prev) / delta_t_slow
y_new = np.empty([5, 1])
y_new[0] = y[0]
y_new[1] = y[1]
y_new[2] = y[2]
y_new[3] = x_vel
y_new[4] = y_vel
# Set previous values to current values
self.x_prev = x_curr
self.y_prev = y_curr
self.slow_time_prev = slow_time_curr
y = y_new
# Given measurements, correct x_hat estimate
self.gsf_obj.gsf_correct(y, delta_t)
# Get and publish MMSE
odom_output = Odometry()
odom_output.header.stamp = rospy.Time.now()
odom_output.header.frame_id = self.frame_id
mu_k_plus_one_plus = self.gsf_obj.get_mu()
self.x_store = np.append(self.x_store, mu_k_plus_one_plus, axis=1)
rotation_quat = tf.transformations.quaternion_from_euler(
0, 0, mu_k_plus_one_plus[2])
odom_output.pose.pose = Pose(
Point(mu_k_plus_one_plus[0], mu_k_plus_one_plus[1], 0),
Quaternion(*rotation_quat))
odom_output.twist.twist = Twist(
Vector3(mu_k_plus_one_plus[3], mu_k_plus_one_plus[4], 0),
Vector3(0, 0, u[0]))
self.filter_output_pub.publish(odom_output)
Sigma_k_plus_one_plus = self.gsf_obj.get_sigma()
self.P_store = np.append(self.P_store,
Sigma_k_plus_one_plus.reshape(25, 1),
axis=1)
P_msg = Float64MultiArray()
P_msg.layout.dim.append(MultiArrayDimension())
P_msg.layout.dim[0].size = 5
P_msg.layout.dim[0].stride = 25
P_msg.layout.dim.append(MultiArrayDimension())
P_msg.layout.dim[1].size = 5
P_msg.layout.dim[1].stride = 5
P_msg.data = Sigma_k_plus_one_plus.reshape(25)
# rospy.loginfo("Publishing y")
self.P_publish.publish(P_msg)
self.time_prev = time_curr
if (self.slow_vel_count == 22000):
#print "Saving to ekf.mat"
#scipy.io.savemat('ekf',{'x': self.x_store, 'P': self.P_store})
self.odom_sub.unregister()
print "Save to csv"
np.savetxt("/home/kyle/catkin_ws/src/ASEN_Project/bag/gsf_x.csv",
self.x_store,
delimiter=",")
np.savetxt("/home/kyle/catkin_ws/src/ASEN_Project/bag/gsf_P.csv",
self.P_store,
delimiter=",")
print "Save complete"
rospy.signal_shtudown("Ended GSF")
