def status_cb(self, msg):
self.status = msg.data
if msg.data == "OFF":
traj = MultiDOFJointTrajectory()
traj.points = []
# Take it from odometry
point = MultiDOFJointTrajectoryPoint()
transform = Transform()
transform.translation.x = self.odom_msg.pose.pose.position.x
transform.translation.y = self.odom_msg.pose.pose.position.y
transform.translation.z = self.odom_msg.pose.pose.position.z
transform.rotation.x = self.odom_msg.pose.pose.orientation.x
transform.rotation.y = self.odom_msg.pose.pose.orientation.y
transform.rotation.z = self.odom_msg.pose.pose.orientation.z
transform.rotation.w = self.odom_msg.pose.pose.orientation.w
point.transforms.append(transform)
traj.points.append(point)
self.traj_pub.publish(traj)
def pose_cb(self, msg):
# Fill
temp_traj = MultiDOFJointTrajectory()
temp_traj.header = msg.header
temp_traj.points = []
temp_traj.points.append(MultiDOFJointTrajectoryPoint())
temp_traj.points[0].transforms = []
temp_traj.points[0].transforms.append(Transform())
temp_traj.points[0].transforms[0].translation.x = msg.pose.position.x
temp_traj.points[0].transforms[0].translation.y = msg.pose.position.y
temp_traj.points[0].transforms[0].translation.z = msg.pose.position.z
temp_traj.points[0].transforms[0].rotation.x = msg.pose.orientation.x
temp_traj.points[0].transforms[0].rotation.y = msg.pose.orientation.y
temp_traj.points[0].transforms[0].rotation.z = msg.pose.orientation.z
temp_traj.points[0].transforms[0].rotation.w = msg.pose.orientation.w
# Call trajectory callback with one point
self.trajectory_cb(temp_traj)
def talker():
rospy.init_node('traj_commander', anonymous=True)
pub = rospy.Publisher('/carla/ego_vehicle/desired_waypoints',
MultiDOFJointTrajectory,
queue_size=10)
viz_pub = rospy.Publisher('viz/desired_waypoints', Marker, queue_size=10)
rate = rospy.Rate(20)
while not rospy.is_shutdown():
msg = MultiDOFJointTrajectory()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = '/map'
viz_msg = Marker()
viz_msg.header.stamp = rospy.Time.now()
viz_msg.header.frame_id = '/map'
viz_msg.type = Marker.CUBE_LIST
viz_msg.scale.x = 1
viz_msg.scale.y = 1
viz_msg.scale.z = 1
viz_msg.color.a = 1
viz_msg.points = []
pts = []
num_pt = 30
radius = 19.5
ang_inc = 2.0 * math.pi / num_pt
for i in range(num_pt):
ang = ang_inc * i
pt = MultiDOFJointTrajectoryPoint()
pt.transforms = [Transform(Vector3(radius * math.cos(ang) - 0.5, radius * math.sin(ang) - 0.3, 0), \
Quaternion(0,0,0,1))]
pt.velocities = [Twist(Vector3(6, 0, 0), Vector3(0, 0, 0))]
viz_msg.points.append(
Point(radius * math.cos(ang), radius * math.sin(ang), 0))
pts.append(pt)
msg.points = pts
pub.publish(msg)
viz_pub.publish(viz_msg)
rate.sleep()
def ref_sub(self, msg):
traj = MultiDOFJointTrajectory()
traj.points = []
traj.points.append(msg)
if not self.status == "OFF":
self.traj_pub.publish(traj)
def timer_cb(self, event):
'''
first compute the expected time to enter the interacting area for the ego car
1. then check if the time interval overlaps with the entering time for relevant cars
2. if not, then plan the trajectory like there is no other car
3. if yes, then slow down the vehicle
'''
if self.stateReady:
# print("state is ready!!!!!!!!!!")
pos_x, pos_y = self.state.get_position()
current_speed = self.state.get_speed()
# current_speed = self.reference_speed # to avoid oscillatory behavior
current_ang = self.state.get_polar_angle()
ang_speed = current_speed / self.circle_radius # rad/sec, speed moving in central angle
# -------------------------------
# check for possible interaction
# --------------------------------
def get_ang_area(x_position, y_position):
ang_max = 0
ang_min = 0
if x_position > 0 and y_position > 0:
ang_min = np.arctan2(20 - self.circle_center[1],
-5 - self.circle_center[0])
ang_max = np.arctan2(15 - self.circle_center[1],
-15 - self.circle_center[0])
elif x_position > 0 and y_position <= 0:
ang_min = np.arctan2(5 - self.circle_center[1],
20 - self.circle_center[0])
ang_max = np.arctan2(15 - self.circle_center[1],
15 - self.circle_center[0])
elif x_position <= 0 and y_position <= 0:
ang_min = np.arctan2(-20 - self.circle_center[1],
5 - self.circle_center[0])
ang_max = np.arctan2(-15 - self.circle_center[1],
15 - self.circle_center[0])
return ang_min, ang_max
def enter_area_time(x_position, y_position, ang_speed):
'''
based on current position and speed, compute the t_min and t_max in the interested area
'''
t_min = 0
t_max = 0
relevant_flag = False
ang_min, ang_max = get_ang_area(x_position, y_position)
if np.linalg.norm([ang_min**2 + ang_max**2]) > 0.05:
t_min = (ang_min - current_ang) / ang_speed
t_max = (ang_max - current_ang) / ang_speed
relevant_flag = True
return t_min, t_max, relevant_flag
'''
given current ego location and relevant paths, compute the t_min and t_max
that all the path may occupy the interacting area
if no path occupies the interacting area, then set the relevant_flag to False
otherwise set flag to true
'''
def relevant_enter_area_time(x_position, y_position, path):
# path is nav_msgs/Path message
# this function is only entered if an interacting area exits
ang_min, ang_max = get_ang_area(x_position, y_position)
assert (ang_max > ang_min)
def get_angle(x, y):
return np.arctan2(y - self.circle_center[1],
x - self.circle_center[0])
t_min = 0
t_max = 0
relevant_flag = False
for i in range(self.steps):
curr_t = self.time_step * i
curr_pose_angle = get_angle(path.poses[i].pose.position.x, \
path.poses[i].pose.position.y)
if curr_pose_angle > ang_min and not relevant_flag:
t_min = curr_t
relevant_flag = True
if curr_pose_angle < ang_max and relevant_flag:
t_max = curr_t
return t_min, t_max, relevant_flag
t_min, t_max, flag = enter_area_time(pos_x, pos_y, ang_speed)
if flag:
other_min_list = []
other_max_list = []
for path in self.predicted_traj.paths:
other_tmin, other_tmax, relevant_flag = relevant_enter_area_time(
pos_x, pos_y, path)
if relevant_flag:
other_min_list.append(other_tmin)
other_max_list.append(other_tmax)
def overlap(inter1, inter2):
min1, max1 = inter1
min2, max2 = inter2
min = np.max([min1, min2])
max = np.min([max1, max2])
if min < max - 0.01:
return [min, max]
else:
return [0, 0]
if len(other_max_list) != 0 and len(other_min_list) != 0:
t_max_other = np.min(
[np.max(other_max_list), self.horizon])
t_min_other = np.min(other_min_list)
# print("other cars will enter the area !!!!!!!!!!!!!!!!!!!!!!!!!!!")
# print("occupy time interval", t_min_other, t_max_other)
overlap_min, overlap_max = overlap(
[t_min, t_max], [t_min_other, t_max_other])
else:
overlap_min, overlap_max = 0, 0
if math.sqrt(overlap_min**2 + overlap_max**2) < 0.1:
interaction_flag = False
else:
interaction_flag = True
# if interaction_flag:
# print("they have potential overlap!!!!!!!!!!!!!!!!!!!!!!!!!!!!!")
# print("overlap interval", overlap_min, overlap_max)
else:
interaction_flag = False
if not interaction_flag:
# -------------------------------
# do not consider other cars
# --------------------------------
# print("planner: do not consider other cars")
traj_msg = MultiDOFJointTrajectory()
traj_msg.header.stamp = rospy.Time.now()
traj_msg.header.frame_id = 'map'
traj_msg.points = []
angle_increment = ang_speed * self.time_step
for i in range(self.steps):
ang = current_ang + angle_increment * i
traj_point = MultiDOFJointTrajectoryPoint()
traj_point.transforms = [Transform(Vector3(self.circle_radius * np.cos(ang) + self.circle_center[0], \
self.circle_radius * np.sin(ang) + self.circle_center[1], 0), Quaternion(0,0,0,1))]
traj_point.velocities = [
Twist(Vector3(self.reference_speed, 0, 0),
Vector3(0, 0, 0))
]
traj_msg.points.append(traj_point)
else:
# ------------------------------
# consider other cars
# enter the area 1 sec after t_min_other
# -------------------------------
print("entering 1 seconds after", t_min_other,
"--------------------------")
print("this is the other car entering time", t_min_other)
print("this is the ego entering time", t_min)
if t_min < t_min_other - 0.2:
enter_time = t_min
elif t_min > t_min_other - 0.2 and t_min < t_min_other + 0.3:
enter_time = t_min_other - 0.2
elif t_min > t_min_other + 0.3 and t_min < t_min_other + 2.5:
enter_time = t_min_other + 2.5
# enter_time = np.max([t_min_other - 0.5, 0.5])
# elif t_min > t_min_other + 1.5:
# enter_time = t_min
else:
enter_time = t_min
print("this is final entering time", enter_time)
ang_min, ang_max = get_ang_area(pos_x, pos_y)
print("for postion", pos_x, pos_y, "relevant angle is ",
ang_min, ang_max)
print("current ang", current_ang)
ang_dist = ang_min - current_ang
print("ang to travel before ", ang_dist)
ang_dcc = 2 * (ang_dist - ang_speed * enter_time) / (enter_time
**2)
print("current angle velocity", ang_speed)
print("this is desired deacceleration", ang_dcc)
ang_dcc = np.max([ang_dcc, -self.MAX_ACCELERATION_ANG])
decc_steps = int(enter_time / self.time_step) + 2
print("need to decelerate at ang acceleration", ang_dcc)
min_ang_speed = ang_speed + decc_steps * self.time_step * ang_dcc
print("this is the min speed",
min_ang_speed * self.circle_radius)
acc_steps = self.steps - decc_steps
print("this is the steps left for acceleration", acc_steps)
if acc_steps != 0:
ang_acc = np.min([self.MAX_ACCELERATION_ANG, \
(self.reference_speed/self.circle_radius - min_ang_speed)/(acc_steps * self.time_step)])
print("this is the acc ang:", ang_acc)
traj_msg = MultiDOFJointTrajectory()
traj_msg.header.stamp = rospy.Time.now()
traj_msg.header.frame_id = 'map'
traj_msg.points = []
a = current_ang # polar coordiante angle
ang_s = ang_speed # angular speed
for i in range(self.steps):
if i <= decc_steps:
a = a + ang_s * self.time_step + 1 / 2.0 * ang_dcc * (
self.time_step)**2
ang_s = ang_s + self.time_step * ang_dcc
ang_s = np.max(
[np.min([ang_s, 6.0 / self.circle_radius]), 0])
s = ang_s * self.circle_radius
pt = MultiDOFJointTrajectoryPoint()
pt.transforms = [Transform(Vector3(self.circle_radius * np.cos(a) - 0.5, \
self.circle_radius * np.sin(a) - 0.3, 0), \
Quaternion(0,0,0,1))]
pt.velocities = [
Twist(Vector3(s, 0, 0), Vector3(0, 0, 0))
]
traj_msg.points.append(pt)
if t_min < t_min_other + 1 and t_min > t_min_other - 1:
print("appending ", i, "th point, velocity", s,
"ang vel ", ang_s, "ang", a)
else:
a = a + ang_s * self.time_step + 1 / 2.0 * ang_acc * self.time_step**2
ang_s = ang_s + ang_acc * self.time_step
ang_s = np.max(
[np.min([ang_s, 6.0 / self.circle_radius]), 0])
s = ang_s * self.circle_radius
pt = MultiDOFJointTrajectoryPoint()
pt.transforms = [Transform(Vector3(self.circle_radius * np.cos(a) - 0.5, \
self.circle_radius * np.sin(a) - 0.3, 0), \
Quaternion(0,0,0,1))]
pt.velocities = [
Twist(Vector3(s, 0, 0), Vector3(0, 0, 0))
]
traj_msg.points.append(pt)
if t_min < t_min_other + 1 and t_min > t_min_other - 1:
print("appending ", i, "th point, velocity", s,
"ang vel ", ang_s, "ang", a)
# t_stamp = i * self.time_step
# if t_stamp < overlap_min:
# ang = current_ang
# pt = MultiDOFJointTrajectoryPoint()
# pt.transforms = [Transform(Vector3(self.circle_radius * np.cos(ang) - 0.5, \
# self.circle_radius * np.sin(ang) - 0.3, 0), \
# Quaternion(0,0,0,1))]
# pt.velocities = [Twist(Vector3(3, 0, 0), Vector3(0, 0, 0))]
# traj_msg.points.append(pt)
# else:
# ang_increment = self.reference_speed * (t_stamp - overlap_min) / self.circle_radius
# ang = current_ang + ang_increment
# pt = MultiDOFJointTrajectoryPoint()
# pt.transforms = [Transform(Vector3(self.circle_radius * np.cos(ang) - 0.5, \
# self.circle_radius * np.sin(ang) - 0.3, 0), \
# Quaternion(0,0,0,1))]
# pt.velocities = [Twist(Vector3(self.reference_speed, 0, 0), Vector3(0, 0, 0))]
# traj_msg.points.append(pt)
self.trajectory_pub.publish(traj_msg)
def generateTrajectoryMessage(px, py, pyaw, time_traj):
traj = MultiDOFJointTrajectory()
path = Path()
temp_poses = []
temp_points = []
# path.header.stamp = rospy.Time.now()
path.header.frame_id = "world"
traj.header.stamp = rospy.Time.now()
traj.header.frame_id = "world"
traj.joint_names.append("drone")
for i in range(len(px)):
point = MultiDOFJointTrajectoryPoint()
tf_msg = Transform()
tf_msg.translation.x = px[i]
tf_msg.translation.y = py[i]
tf_msg.translation.z = 1 #altitude of Flight
quaternion = quaternion_from_euler(0, 0, pyaw[i])
tf_msg.rotation.x = quaternion[0]
tf_msg.rotation.y = quaternion[1]
tf_msg.rotation.z = quaternion[2]
tf_msg.rotation.w = quaternion[3]
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = "world"
pose.pose.position.x = px[i]
pose.pose.position.y = py[i]
pose.pose.position.z = 1
pose.pose.orientation.x = quaternion[0]
pose.pose.orientation.y = quaternion[1]
pose.pose.orientation.z = quaternion[2]
pose.pose.orientation.w = quaternion[3]
# vel_msg = Twist()
# vel_msg = self.velocity
# point.velocities.append(vel_msg)
# if pyaw[i] == pyaw[i-1] :
# vel_msg.angular.z = 0
# elif pyaw[i] > pyaw[i-1] :
# vel_msg.angular.z = self.angular_velocity
# elif pyaw[i] < pyaw[i-1] :
# vel_msg.angular.z = -self.angular_velocity
# acc_msg = Twist()
# acc_msg.linear.x = 0
point.transforms.append(tf_msg)
# point.accelerations.append(acc_msg)
point.time_from_start = rospy.Duration.from_sec(
time_traj[i]) #+ rospy.Duration(5)
temp_points.append(point)
temp_poses.append(pose)
traj.points = temp_points[80:]
path.poses = temp_poses[80:]
return traj, path