def move_camera(self, t):
"""
This function moves the camera in a circle around the table.
After 1000 steps the camera moves lower (to get a different angle and maybe more
nice data)
TODO: Maybe other camera movement?
:param t: step
:param camera: camera frame
:param angle: angle in which camera need to move
:param radius: around which camera is moving
:return: updated angle
"""
cam_px, cam_py, cam_pz = self.camera.getPosition()
cam_q1, cam_q2, cam_q3, cam_q4 = self.camera.getQuaternion()
q = Quaternion(cam_q1, cam_q2, cam_q3, cam_q4)
e1, e2, e3 = q.to_euler(degrees=True)
cam_px_new = cam_px + np.cos(self.angle) * self.radius
cam_py_new = cam_py + np.sin(self.angle) * self.radius
if t % 1000 == 0:
cam_pz = cam_pz - 0.05
e1 = e1 + 2.5
quat_new = Quaternion.from_euler(e1, e2, e3 + 5, degrees=True)
self.camera.setPosition([cam_px_new, cam_py_new, cam_pz])
self.camera.setQuaternion([quat_new[0], quat_new[1], quat_new[2], quat_new[3]])
self.angle += 0.0872665
if t == 12000:
self.camera.setPosition([0.6, -.75, 1.85])
self.camera.setQuaternion(self.cam_quat)
self.angle = 0
self.radius = 0.075
def callback_odom(self, msg):
if self.rosb:
self.last_odom = msg
self.T = np.array((msg.pose.pose.position.x,
msg.pose.pose.position.y)).reshape(1, 2)
q = Quaternion(msg.pose.pose.orientation.x,
msg.pose.pose.orientation.y,
msg.pose.pose.orientation.z,
msg.pose.pose.orientation.w)
(r, p, y) = q.to_euler()
self.theta = r
self.R_plus = np.array([[np.cos(self.theta), -np.sin(self.theta)],
[np.sin(self.theta),
np.cos(self.theta)]]).reshape(2, 2)
self.R_minus = np.array(
[[np.cos(-self.theta), -np.sin(-self.theta)],
[np.sin(-self.theta),
np.cos(-self.theta)]]).reshape(2, 2)
self.pose = np.array(
[msg.pose.pose.position.x, msg.pose.pose.position.y, y],
dtype=np.float32)
self.pose_history_x.append(self.pose[0])
self.pose_history_y.append(self.pose[1])
self.thetta_history.append(self.theta)
self.rosb = False
def show_image_pose(frame, pose):
cv2.imshow('frame', frame)
if (cv2.waitKey(25) & 0xFF == ord('q')):
return False
posx = pose.pose.position.x
posy = pose.pose.position.y
posz = pose.pose.position.z
w = pose.pose.orientation.w
x = pose.pose.orientation.x
y = pose.pose.orientation.y
z = pose.pose.orientation.z
q = Quaternion(w, x, y, z)
e = q.to_euler(degrees=True)
roll = float(e[0])
pitch = float(e[1])
yaw = float(e[2])
print("orientation:", roll, pitch, yaw)
print("position:", posx, posy, posz)
print("------------------------------")
return True
def save_imu_pose(client, filename, id):
filename = filename + '.jpg'
vehicle_name = "Drone" + str(id)
imu_data = client.getImuData(vehicle_name=vehicle_name)
state = client.getMultirotorState(vehicle_name=vehicle_name)
lon = state.kinematics_estimated.position.y_val + real_init_pos[id][
0] # x val
lat = state.kinematics_estimated.position.x_val + real_init_pos[id][
1] # y val
alt = state.kinematics_estimated.position.z_val
w = imu_data.orientation.w_val
x = imu_data.orientation.x_val
y = imu_data.orientation.y_val
z = imu_data.orientation.z_val
q = Quaternion(w, x, y, z)
e = q.to_euler(degrees=True)
roll = float(e[0])
pitch = float(e[1])
yaw = float(e[2])
st = (os.path.basename(filename)) + "," + '%.3f'%lon + "," + \
'%.3f'%lat + "," + '%.3f'%alt + "," + \
'%.3f'%yaw + "," + '%.3f'%pitch + "," + \
'%.3f'%roll + "\n"
print(st)
global file_handle
file_handle.write(st)
def cb(data):
global f, count, bridge
try:
curr_img = bridge.imgmsg_to_cv2(data.image, "bgr8")
except CvBridgeError as e:
print(e)
w = data.pose.pose.orientation.w
x = data.pose.pose.orientation.x
y = data.pose.pose.orientation.y
z = data.pose.pose.orientation.z
lon = data.pose.pose.position.x
lat = data.pose.pose.position.y
alt = data.pose.pose.position.z
q = Quaternion(w, x, y, z)
e = q.to_euler(degrees=True)
roll = float(e[0])
pitch = float(e[1])
yaw = float(e[2])
st = str (count) + ".jpg" + "," + '%.3f'%lon + "," + \
'%.3f'%lat + "," + '%.3f'%alt + "," + \
'%.3f'%yaw + "," + '%.3f'%pitch + "," + \
'%.3f'%roll + "\n"
print(st)
f.write(st)
cv2.imwrite(dirname + '/' + str(count) + '.jpg', curr_img)
count += 1
def cb(self, data):
try:
curr_img = self.bridge.imgmsg_to_cv2(data.image, "bgr8")
except CvBridgeError as e:
print(e)
w = data.pose.pose.orientation.w
x = data.pose.pose.orientation.x
y = data.pose.pose.orientation.y
z = data.pose.pose.orientation.z
pos_x = data.pose.pose.position.x
pos_y = data.pose.pose.position.y
pos_z = data.pose.pose.position.z
q = Quaternion(w, x, y, z)
e = q.to_euler(degrees=True)
roll = float(e[0])
pitch = float(e[1])
yaw = float(e[2])
self.pose = [pos_x, pos_y, pos_z, yaw, roll, pitch]
print("pose:", self.pose)
self.combine(curr_img, self.pose)
def robot_pose_cb(self, data):
self.robot_x = data.pose.pose.position.x
self.robot_y = data.pose.pose.position.y
q = Quaternion(data.pose.pose.orientation.w,
data.pose.pose.orientation.x,
data.pose.pose.orientation.y,
data.pose.pose.orientation.z)
self.robot_th = q.to_euler(degrees=False)[2]
def update_pose(self, data):
try:
self.pose.x = data.pose[self.robot_id].position.x
self.pose.y = data.pose[self.robot_id].position.y
quaternion = Quaternion(data.pose[self.robot_id].orientation.x,
data.pose[self.robot_id].orientation.y,
data.pose[self.robot_id].orientation.z,
data.pose[self.robot_id].orientation.w)
self.pose.theta = (quaternion.to_euler()[0] / pi) + 0.25
except IndexError:
None
def callback_odom(self, msg):
self.T = np.array((msg.pose.pose.position.x,
msg.pose.pose.position.y)).reshape(1, 2)
q = Quaternion(msg.pose.pose.orientation.x,
msg.pose.pose.orientation.y,
msg.pose.pose.orientation.z,
msg.pose.pose.orientation.w)
(r, p, y) = q.to_euler()
theta = r
self.R_plus = np.array([[np.cos(theta), -np.sin(theta)],
[np.sin(theta), np.cos(theta)]]).reshape(2, 2)
self.R_minus = np.array([[np.cos(-theta), -np.sin(-theta)],
[np.sin(-theta),
np.cos(-theta)]]).reshape(2, 2)
def pose_cb(self, data):
w = data.pose.orientation.w
x = data.pose.orientation.x
y = data.pose.orientation.y
z = data.pose.orientation.z
pos_x = data.pose.position.x
pos_y = data.pose.position.y
pos_z = data.pose.position.z
q = Quaternion(w, x, y, z)
e = q.to_euler(degrees=True)
roll = float(e[0])
pitch = float(e[1])
yaw = float(e[2])
self.pose = [pos_x, pos_y, pos_z, yaw, roll, pitch]
def pose_callback(data):
global yaw, pitch, roll
global lon, lat, alt
w = data.pose.orientation.w
x = data.pose.orientation.x
y = data.pose.orientation.y
z = data.pose.orientation.z
lon = float(data.pose.position.x) + float(offset[0])
lat = float(data.pose.position.y) + float(offset[1])
alt = float(data.pose.position.z) + float(offset[2])
q = Quaternion(w, x, y, z)
e = q.to_euler(degrees=True)
roll = float(e[0])
pitch = float(e[1])
yaw = float(e[2])
def callback_modelstates(self, msg):
for i in range(len(msg.name)):
if (msg.name[i][0:5] == "actor"):
actor_id = int(msg.name[i][5])
posx = msg.pose[i].position.x
posy = msg.pose[i].position.y
posz = msg.pose[i].position.z
x = msg.pose[i].orientation.x
y = msg.pose[i].orientation.y
z = msg.pose[i].orientation.z
w = msg.pose[i].orientation.w
q = Quaternion(w, x, y, z)
e = q.to_euler(degrees=True)
self.pedestrian_pose[actor_id] = [posx, posy, e[2]]
def dwa_wrapper(final_list):
odom_dec = {}
q = Quaternion(final_list[0].pose.pose.orientation.w,
final_list[0].pose.pose.orientation.x,
final_list[0].pose.pose.orientation.y,
final_list[0].pose.pose.orientation.z)
e = q.to_euler(degrees=False)
odom_dec["x"] = final_list[0].pose.pose.position.x
odom_dec["y"] = final_list[0].pose.pose.position.y
odom_dec["theta"] = e[2]
odom_dec["u"] = final_list[0].twist.twist.linear.x
odom_dec["omega"] = final_list[0].twist.twist.angular.z
cnfg = Config(odom_dec, final_list[1])
obs = Obstacles(final_list[2].ranges, cnfg)
v_list, w_list, cost_list = DWA(cnfg, obs)
return v_list, w_list, cost_list, final_list[3]
def Angles():
streamMotion = resolve_stream('type', 'Motion') # Resolve stream
inlet2 = StreamInlet(streamMotion[0]) # Create a new inlet to read Motion data from the stream
sample, timestamp = inlet2.pull_sample() # Get a new sample with timestamp
# Motion data is received as Quaternion
# This converts quaternion to angles with degrees as units
# The Q1,Q2,Q3,Q4 are at index 3,4,5,6 ofthe array input
q = Quaternion(sample[3], sample[4], sample[5], sample[6])
e = q.to_euler(degrees=True)
# Processes/stores the angles correctly
angles = []
angles.append(e)
angles = np.array(angles)
angles.flatten()
np.rint([angles])
print(angles)
# Angles are stored in an array, we return them separated
return angles[0][0],angles[0][1],angles[0][2]
def _get_obs(self):
"""
Here we define what sensor data defines our robots observations
To know which Variables we have acces to, we need to read the
TurtleBot2Env API DOCS
:return:
"""
start_tt = time.time()
rospy.logdebug("Start Get Observation ==>")
# We get the laser scan data
laser_scan = self.get_laser_scan()
rospy.logdebug("BEFORE DISCRET _episode_done==>" +
str(self._episode_done))
discretized_observations = self.discretize_observation(
laser_scan, self.new_ranges)
rospy.logdebug("Observations==>" + str(discretized_observations))
rospy.logdebug("AFTER DISCRET_episode_done==>" +
str(self._episode_done))
rospy.logdebug("END Get Observation ==>")
discretized_observations = numpy.asarray(discretized_observations)
# New code for getting observations based on DWA
odom_data = self.get_odom()
self.odom_dict = {}
self._reached_goal = False
q = Quaternion(odom_data.pose.pose.orientation.w,
odom_data.pose.pose.orientation.x,
odom_data.pose.pose.orientation.y,
odom_data.pose.pose.orientation.z)
e = q.to_euler(degrees=False)
self.odom_dict["x"] = odom_data.pose.pose.position.x
self.odom_dict["y"] = odom_data.pose.pose.position.y
self.odom_dict["theta"] = e[2]
self.odom_dict["u"] = odom_data.twist.twist.linear.x
self.odom_dict["omega"] = odom_data.twist.twist.angular.z
cnfg = Config(self.odom_dict, self.goal_pose)
self.obs = Obstacles(laser_scan.ranges, cnfg)
if (self.n_skipped_count == 0):
self.obs_list_stacked = numpy.delete(self.obs_list_stacked, (0, 1),
1)
self.obs_list_stacked = numpy.append(self.obs_list_stacked,
self.obs.obst, 1)
self.n_skipped_count += 1
elif (self.n_skipped_count < self.n_skipped_frames):
self.obs_list_stacked[:, ((2 * self.n_stacked_frames) - 2):((
2 * self.n_stacked_frames))] = self.obs.obst
self.n_skipped_count += 1
elif (self.n_skipped_count == self.n_skipped_frames):
self.obs_list_stacked[:, ((2 * self.n_stacked_frames) - 2):((
2 * self.n_stacked_frames))] = self.obs.obst
self.n_skipped_count = 0
# print("The stacked obs list {}".format(self.obs_list_stacked))
# print("The stacked obs list part {}".format(self.obs_list_stacked[:5,:]))
self.v_matrix, self.w_matrix, self.cost_matrix, self.obst_cost_matrix, self.to_goal_cost_matrix = DWA(
cnfg, self.obs_list_stacked, self.n_stacked_frames)
# print("The w_matrix after {}".format(self.w_matrix[:5,:]))
# print("The w_matrix {}".format(self.w_matrix[:,self.n_stacked_frames - 1]))
# print("The cost_matrix {}".format(self.cost_matrix[:,self.n_stacked_frames - 1]))
if (self.visualize_obs == True) and (self.robot_number == 0):
self.viz_obs()
# self.stacked_obs = numpy.stack((self.v_matrix, self.w_matrix, self.cost_matrix), axis=2)
self.stacked_obs = numpy.stack(
(self.v_matrix, self.w_matrix, self.obst_cost_matrix,
self.to_goal_cost_matrix),
axis=2)
# self.obst_cost_matrix = self.obst_cost_matrix.flatten('F')
# self.to_goal_cost_matrix = self.to_goal_cost_matrix.flatten('F')
# self.stacked_obs = numpy.concatenate((self.obst_cost_matrix, self.to_goal_cost_matrix), axis=0)
# self.stacked_obs = numpy.expand_dims(self.stacked_obs, axis=1)
self.current_distance2goal = self._get_distance2goal()
if (self.current_distance2goal < 0.5):
self._episode_done = True
self._reached_goal = True
return self.stacked_obs
def _init_env_variables(self):
"""
Inits variables needed to be initialised each time we reset at the start
of an episode.
:return:
"""
# For Info Purposes
self.cumulated_reward = 0.0
# Set to false Done, because its calculated asyncronously
self._episode_done = False
# We wait a small ammount of time to start everything because in very fast resets, laser scan values are sluggish
# and sometimes still have values from the prior position that triguered the done.
time.sleep(1.0)
# TODO: Add reset of published filtered laser readings
laser_scan = self.get_laser_scan()
discretized_ranges = laser_scan.ranges
odom_data_init = self.get_odom()
odom_dict_init = {}
q = Quaternion(odom_data_init.pose.pose.orientation.w,
odom_data_init.pose.pose.orientation.x,
odom_data_init.pose.pose.orientation.y,
odom_data_init.pose.pose.orientation.z)
e = q.to_euler(degrees=False)
odom_dict_init["x"] = odom_data_init.pose.pose.position.x
odom_dict_init["y"] = odom_data_init.pose.pose.position.y
odom_dict_init["theta"] = e[2]
odom_dict_init["u"] = odom_data_init.twist.twist.linear.x
odom_dict_init["omega"] = odom_data_init.twist.twist.angular.z
cnfg = Config(odom_dict_init, self.goal_pose)
obs_init = Obstacles(laser_scan.ranges, cnfg)
self.obs_list_stacked = numpy.column_stack(
(obs_init.obst for _ in range(0, self.n_stacked_frames)))
self.counter = 1
self.episode_num = self.episode_num + 1
init_obs = self._get_obs()
self.previous_distance2goal = self._get_distance2goal()
self.publish_filtered_laser_scan(
laser_original_data=laser_scan,
new_filtered_laser_range=discretized_ranges)
self.total_collisions += self.episode_collisions
print("Total number of collsions ------------ {}".format(
self.total_collisions))
self.episode_collisions = 0
self.n_skipped_count = 0
file = open('VelList.csv', 'w')
file_ang = open('angularVelList.csv', 'w')
with file:
write = csv.writer(file)
write.writerows(self.list_vel)
write_ang = csv.writer(file_ang)
write_ang.writerows(self.list_angular_vel)
self.list_vel.append([
"lower_limit", "upper_limit", "current_velocity",
"Violation_Status"
])
self.list_angular_vel.append([
"lower_limit", "upper_limit", "current_velocity",
"Violation_Status"
])
def update_point(self, current_state, request_velocity):
self.state = np.array([
current_state.pose.position.x, current_state.pose.position.y,
current_state.pose.position.z, current_state.pose.orientation.z
])
self.x_controller.maxOut = request_velocity
self.y_controller.maxOut = request_velocity
self.altitude_controller.maxOut = request_velocity
# Drawing a vector from our current position to a target position
output_vector = np.empty(3)
output_vector[0] = self.target[0] - self.state[0]
output_vector[1] = self.target[1] - self.state[1]
output_vector[2] = self.target[2] - self.state[2]
# Normalizing vector
output_vector = output_vector / (output_vector[0]**2 + output_vector[1]
**2 + output_vector[2]**2)**.5
time = rospy.get_time()
x_factor = self.x_controller.update(self.target[0], self.state[0],
time)
y_factor = self.y_controller.update(self.target[1], self.state[1],
time)
altitude_factor = self.altitude_controller.update(
self.target[2], self.state[2], time)
# Multiplying each controller factor on the vector component it corresponds to...
output_vector[0] *= abs(x_factor)
output_vector[1] *= abs(y_factor)
output_vector[2] *= abs(altitude_factor)
#string = "x: " + str(output_vector[0]) + " y: " + str(output_vector[1]) + " z: " + str(output_vector[2])
#rospy.loginfo_throttle(0.5, string)
# Creating the output Twist message
output = Twist()
output.linear.x = output_vector[0]
output.linear.y = output_vector[1]
output.linear.z = output_vector[2]
# Testing just raw PID instead
#output.linear.x = x_factor
#output.linear.y = y_factor
#output.linear.z = altitude_factor
# TODO: Yaw controller
# The current_state.orientation is a Pose message, and therefore actually a quaternion, which means...
# ... that we have to translate the quaternion into euler form so that we can compare the two correctly for the PID controller :)
current_quat = Quaternion(current_state.pose.orientation.w,
current_state.pose.orientation.x,
current_state.pose.orientation.y,
current_state.pose.orientation.z)
current_euler = current_quat.to_euler()
# Since the PID controller calculate the error itself and I dont wanna custimize it, I ghetto fix and create a fake error myself with the rules that apply for the yaw
error = (current_euler[2] + math.pi) - self.target[3]
#error = (error + 180) % 360 - 180
if error > math.pi:
error -= math.pi * 2
elif error < -math.pi:
error += math.pi * 2
yaw_input = self.Yaw.update(0, error, time)
#rospy.loginfo(self.target[3])
#rospy.loginfo(current_euler[2])
output.angular.z = yaw_input
return output
def __odom_callback(self, msg: Odometry):
orientation_q = msg.pose.pose.orientation
q = Quaternion(orientation_q.w, orientation_q.x, orientation_q.y,
orientation_q.z)
(self.__roll, self.__pitch, self.__yaw) = q.to_euler()
