def publish_obstacle_msg(self):
if self.args.no_ros:
return
else:
from tf.transformations import quaternion_from_euler
from geometry_msgs.msg import Point32, Quaternion
from costmap_converter.msg import ObstacleArrayMsg, ObstacleMsg
obstacles_msg = ObstacleArrayMsg()
obstacles_msg.header.stamp = self.get_sim_time()
obstacles_msg.header.frame_id = kFixedFrame
for i in range(1, self.n_sim_agents):
# Add point obstacle
obst = ObstacleMsg()
obst.id = i
obst.polygon.points = [Point32()]
obst.polygon.points[0].x = self.rlenv.virtual_peppers[i].pos[0]
obst.polygon.points[0].y = self.rlenv.virtual_peppers[i].pos[1]
obst.polygon.points[0].z = 0
obst.radius = self.rlenv.vp_radii[i]
yaw = self.rlenv.virtual_peppers[i].pos[2]
q = quaternion_from_euler(0,0,yaw)
obst.orientation = Quaternion(*q)
obst.velocities.twist.linear.x = self.rlenv.virtual_peppers[i].vel[0]
obst.velocities.twist.linear.y = self.rlenv.virtual_peppers[i].vel[1]
obst.velocities.twist.linear.z = 0
obst.velocities.twist.angular.x = 0
obst.velocities.twist.angular.y = 0
obst.velocities.twist.angular.z = self.rlenv.virtual_peppers[i].vel[2]
obstacles_msg.obstacles.append(obst)
self.obstpub.publish(obstacles_msg)
return
def publish_obstacle_msg():
pub = rospy.Publisher('/move_base/TebLocalPlannerROS/obstacles', ObstacleArrayMsg, queue_size=1)
#pub = rospy.Publisher('/p3dx/move_base/TebLocalPlannerROS/obstacles', ObstacleArrayMsg, queue_size=1)
rospy.init_node("test_obstacle_msg")
print('Prepare message now')
obstacle_msg = ObstacleArrayMsg()
obstacle_msg.header.stamp = rospy.Time.now()
obstacle_msg.header.frame_id = "map"
# Add point obstacle
obstacle_msg.obstacles.append(ObstacleMsg())
obstacle_msg.obstacles[0].id = 0
obstacle_msg.obstacles[0].polygon.points = [Point32()]
obstacle_msg.obstacles[0].polygon.points[0].x = -1
obstacle_msg.obstacles[0].polygon.points[0].y = -1
obstacle_msg.obstacles[0].polygon.points[0].z = 0
print('Prepared, publish')
# Add line obstacle
obstacle_msg.obstacles.append(ObstacleMsg())
obstacle_msg.obstacles[1].id = 1
line_start = Point32()
line_start.x = 2
line_start.y = 0
line_end = Point32()
line_end.x = 4
line_end.y = 0
obstacle_msg.obstacles[1].polygon.points = [line_start, line_end]
# Add polygon obstacle
# obstacle_msg.obstacles.append(ObstacleMsg())
# obstacle_msg.obstacles[1].id = 2
# v1 = Point32()
# v1.x = -1
# v1.y = -1
# v2 = Point32()
# v2.x = -0.5
# v2.y = -1.5
# v3 = Point32()
# v3.x = 0
# v3.y = -1
# obstacle_msg.obstacles[2].polygon.points = [v1, v2, v3]
r = rospy.Rate(10) # 10hz
# t = 0.0
while not rospy.is_shutdown():
# Vary y component of the point obstacle
# obstacle_msg.obstacles[0].polygon.points[0].y = 1*math.sin(t)
# t = t + 0.1
# print('Prepared, publish')
pub.publish(obstacle_msg)
r.sleep()
def publish_obstacle_msg():
pub = rospy.Publisher('/test_optim_node/obstacles', ObstacleArrayMsg, queue_size=1)
#pub = rospy.Publisher('/p3dx/move_base/TebLocalPlannerROS/obstacles', ObstacleArrayMsg, queue_size=1)
rospy.init_node("test_obstacle_msg")
obstacle_msg = ObstacleArrayMsg()
obstacle_msg.header.stamp = rospy.Time.now()
obstacle_msg.header.frame_id = "odom" # CHANGE HERE: odom/map
# Add point obstacle
obstacle_msg.obstacles.append(ObstacleMsg())
obstacle_msg.obstacles[0].id = 0
obstacle_msg.obstacles[0].polygon.points = [Point32()]
obstacle_msg.obstacles[0].polygon.points[0].x = 1.5
obstacle_msg.obstacles[0].polygon.points[0].y = 0
obstacle_msg.obstacles[0].polygon.points[0].z = 0
# Add line obstacle
obstacle_msg.obstacles.append(ObstacleMsg())
obstacle_msg.obstacles[1].id = 1
line_start = Point32()
line_start.x = -2.5
line_start.y = 0.5
# line_start.y = -3
line_end = Point32()
line_end.x = -2.5
line_end.y = 2
#line_end.y = -4
obstacle_msg.obstacles[1].polygon.points = [line_start, line_end]
# Add polygon obstacle
obstacle_msg.obstacles.append(ObstacleMsg())
obstacle_msg.obstacles[1].id = 2
v1 = Point32()
v1.x = -1-2
v1.y = -1+1
v2 = Point32()
v2.x = -0.5-2
v2.y = -1.5+1
v3 = Point32()
v3.x = 0-2
v3.y = -1+1
obstacle_msg.obstacles[2].polygon.points = [v1, v2, v3]
r = rospy.Rate(10) # 10hz
t = 0.0
while not rospy.is_shutdown():
# Vary y component of the point obstacle
obstacle_msg.obstacles[0].polygon.points[0].y = 1*math.sin(t)
t = t + 0.1
pub.publish(obstacle_msg)
r.sleep()
def __init__(self):
self.map_pose = Pose()
self.map_width = 600
self.map_height = 600
self.map = OccupancyGrid()
self.map.header.frame_id = 'map'
self.map.info.resolution = 1.0
self.map.info.width = self.map_width
self.map.info.height = self.map_height
self.map.data = range(self.map_width * self.map_height)
grid = np.ndarray((self.map_width, self.map_height),
buffer=np.zeros((self.map_width, self.map_height),
dtype=np.int8),
dtype=np.int8)
grid.fill(np.int8(0))
grid[3:7, 3:7].fill(np.int8(100))
for i in range(self.map_width * self.map_height):
self.map.data[i] = grid.flat[i]
self.global_costmap = OccupancyGrid()
self.global_costmap.info.width = 10
self.global_costmap.info.height = 10
self.global_costmap.info.resolution = 100
self.global_costmap.data = range(100)
grid = np.ndarray((10, 10),
buffer=np.zeros((10, 10), dtype=np.int8),
dtype=np.int8)
grid.fill(np.int8(0))
for i in range(100):
self.map.data[i] = grid.flat[i]
self.global_costmap.info.origin.position.x = -500
self.global_costmap.info.origin.position.y = -500
self.global_costmap.header.frame_id = 'map'
self.obstacle_array = ObstacleArrayMsg()
obstacle = ObstacleMsg()
obstacle.polygon.points = [
Point32(3.0, 3.0, 0.0),
Point32(3.0, 6.0, 0.0),
Point32(6.0, 6.0, 0.0),
Point32(6.0, 3.0, 0.0)
]
obstacle.radius = 2.5
self.obstacle_array.obstacles.append(obstacle)
self.odometry_sub = rospy.Subscriber("base_pose_ground_truth",
Odometry, self.odom_callback)
self.map_pub = rospy.Publisher('move_base/local_costmap/costmap',
OccupancyGrid,
queue_size=5)
self.global_costmap_pub = rospy.Publisher(
'move_base/global_costmap/costmap', OccupancyGrid, queue_size=5)
self.obstacle_pub = rospy.Publisher(
'move_base/TebLocalPlannerROS/obstacles',
ObstacleArrayMsg,
queue_size=5)
def publish_obstacle_msg():
#pub = rospy.Publisher('/test_optim_node/obstacles', ObstacleArrayMsg, queue_size=1)
pub = rospy.Publisher('/move_base/TebLocalPlannerROS/obstacles',
ObstacleArrayMsg,
queue_size=1)
rospy.init_node("test_obstacle_msg")
obstacle_msg = ObstacleArrayMsg()
obstacle_msg.header.stamp = rospy.Time.now()
obstacle_msg.header.frame_id = "map" # CHANGE HERE: odom/map
# Add point obstacle
obstacle_msg.obstacles.append(ObstacleMsg())
obstacle_msg.obstacles[0].id = 0
obstacle_msg.obstacles[0].polygon.points = [Point32()]
obstacle_msg.obstacles[0].polygon.points[0].x = 5
obstacle_msg.obstacles[0].polygon.points[0].y = 2
obstacle_msg.obstacles[0].polygon.points[0].z = 0
obstacle_msg.obstacles.append(ObstacleMsg())
obstacle_msg.obstacles[1].id = 1
obstacle_msg.obstacles[1].polygon.points = [Point32()]
obstacle_msg.obstacles[1].polygon.points[0].x = 5
obstacle_msg.obstacles[1].polygon.points[0].y = 4
obstacle_msg.obstacles[1].polygon.points[0].z = 0
obstacle_msg.obstacles.append(ObstacleMsg())
obstacle_msg.obstacles[2].id = 2
obstacle_msg.obstacles[2].polygon.points = [Point32()]
obstacle_msg.obstacles[2].polygon.points[0].x = 5
obstacle_msg.obstacles[2].polygon.points[0].y = 6
obstacle_msg.obstacles[2].polygon.points[0].z = 0
obstacle_msg.obstacles.append(ObstacleMsg())
obstacle_msg.obstacles[3].id = 3
obstacle_msg.obstacles[3].polygon.points = [Point32()]
obstacle_msg.obstacles[3].polygon.points[0].x = 5
obstacle_msg.obstacles[3].polygon.points[0].y = 8
obstacle_msg.obstacles[3].polygon.points[0].z = 0
r = rospy.Rate(10) # 10hz
t = 0.0
while not rospy.is_shutdown():
# Vary y component of the point obstacle
pub.publish(obstacle_msg)
r.sleep()
def publish_obstacle_msg(self, _pose_x, _pose_y):
obstacle_msg = ObstacleArrayMsg()
obstacle_msg.header.stamp = rospy.Time.now()
obstacle_msg.header.frame_id = "map" # CHANGE HERE: odom/map
block_dis = 0.025
# Add polygon obstacle
obstacle_msg.obstacles.append(ObstacleMsg())
obstacle_msg.obstacles[0].id = 99
# 座標系がx, y逆な気する この順番で四角：ロボットの初期正面方向がx軸, 右側がy軸マイナス方向 0.3mは結構でかいロボット全部埋まるくらい
v1 = Point32()
v1.x = block_dis + _pose_x
v1.y = block_dis + _pose_y
v2 = Point32()
v2.x = block_dis + _pose_x
v2.y = -1 * block_dis + _pose_y
v3 = Point32()
v3.x = -1 * block_dis + _pose_x
v3.y = -1 * block_dis + _pose_y
v4 = Point32()
v4.x = -1 * block_dis + _pose_x
v4.y = block_dis + _pose_y
obstacle_msg.obstacles[0].polygon.points = [v1, v2, v3, v4]
return obstacle_msg
def closest_n_obs(SO_data, pose, n_SO):
if len(SO_data.obstacles[:]) < n_SO:
for i in range(len(SO_data.obstacles[:]), n_SO + 1):
fill_obs = ObstacleMsg()
fill_obs.polygon.points = [Point32()]
fill_obs.polygon.points[0].x = 100
fill_obs.polygon.points[0].y = 100
SO_data.obstacles.append(fill_obs)
dist = np.zeros((1, len(SO_data.obstacles[:])))
for k in range(len(SO_data.obstacles[:])):
[x, y, r] = poligon2centroid(SO_data.obstacles[k].polygon.points[:])
dist[0, k] = np.sqrt((pose[0] - x)**2 + (pose[1] - y)**2)
n_idx = (dist).argsort()[:n_SO]
cl_obs = np.zeros([n_SO * 3])
markerArray = MarkerArray()
for k in range(n_SO):
cl_obs[k * 3:k * 3 + 3] = poligon2centroid(
SO_data.obstacles[n_idx[0, k]].polygon.points[:])
print('These are the closest obs X,Y {}'.format(k + 1),
cl_obs[k * 3:k * 3 + 2])
print('These are the closest obs R {}'.format(k + 1),
cl_obs[k * 3 + 2:k * 3 + 3])
return cl_obs
def makebox(ax, ay, bx, by, cx, cy, dx, dy):
pub = rospy.Publisher('/test_optim_node/obstacles',
ObstacleArrayMsg,
queue_size=1) # /park_planner/lines
#pub = rospy.Publisher('/p3dx/move_base/TebLocalPlannerROS/obstacles', ObstacleArrayMsg, queue_size=1)
obstacle_msg = ObstacleArrayMsg()
obstacle_msg.header.stamp = rospy.Time.now()
obstacle_msg.header.frame_id = "odom" # CHANGE HERE: odom/mapobstacle_msg.obstacles.append(ObstacleMsg())
obstacle_msg.obstacles.append(ObstacleMsg())
obstacle_msg.obstacles.append(ObstacleMsg())
obstacle_msg.obstacles.append(ObstacleMsg())
obstacle_msg.obstacles[0].id = 1
obstacle_msg.obstacles[1].id = 2
obstacle_msg.obstacles[2].id = 3
A = Point32()
A.x = ax
A.y = ay
B = Point32()
B.x = bx
B.y = by
C = Point32()
C.x = cx
C.y = cy
D = Point32()
D.x = dx
D.y = dy
obstacle_msg.obstacles[0].polygon.points = [A, B]
obstacle_msg.obstacles[1].polygon.points = [B, C]
obstacle_msg.obstacles[2].polygon.points = [C, D]
#obstacle_msg.obstacles[0].polygon.points = [v1, v2, v3, v4]
r = rospy.Rate(10) # 10hz
t = 0.0
while not rospy.is_shutdown():
"""# Vary y component of the point obstacle
obstacle_msg.obstacles[0].polygon.points[0].y = 1*math.sin(t)
t = t + 0.1"""
pub.publish(obstacle_msg)
r.sleep()
def heaps2cubes(self):
cubes = []
for i in range(len(self.heaps)):
if self.picked[i]:
continue
for j in range(self.n_cubes_in_heap):
cube = ObstacleMsg()
cube.header.stamp = rospy.Time.now()
cube.header.frame_id = "map"
cube.polygon.points = [Point32(*self.heaps[i][j])]
cubes.append(cube)
return cubes
def publish_obstacle_msg(self):
print("Testing_obstacle_msg")
#obstacle_list = [ObstacleMsg()]
obstacle_arr_msg = ObstacleArrayMsg()
obstacle_arr_msg.header.stamp = rospy.Time.now()
obstacle_arr_msg.header.frame_id = "vicon_world" # CHANGE HERE: odom/map
# Add point obstacle
obstacle_msg = ObstacleMsg()
obstacle_msg.header.stamp = rospy.Time.now()
obstacle_msg.header.frame_id = "vicon_world" # CHANGE HERE: odom/map
obstacle_msg.radius = 0.5
obstacle_msg.id = 2
obstacle_msg.polygon.points = [Point32()]
obstacle_msg.polygon.points[0].x = self.rob_1_pose_trans[0]
obstacle_msg.polygon.points[0].y = self.rob_1_pose_trans[1]
obstacle_msg.polygon.points[0].z = 0
obstacle_msg.orientation.x = self.rob_1_pose_rot[0]
obstacle_msg.orientation.y = self.rob_1_pose_rot[1]
obstacle_msg.orientation.z = self.rob_1_pose_rot[2]
obstacle_msg.orientation.w = self.rob_1_pose_rot[3]
obstacle_msg.velocities.twist.linear.x = self.vel_x
obstacle_msg.velocities.twist.linear.y = self.vel_y
obstacle_msg.velocities.twist.linear.z = 0
obstacle_msg.velocities.twist.angular.x = 0
obstacle_msg.velocities.twist.angular.y = 0
obstacle_msg.velocities.twist.angular.z = 0
obstacle_arr_msg.obstacles.append(obstacle_msg)
#print(obstacle_arr_msg)
r = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
self.rob_2_pub.publish(obstacle_arr_msg)
r.sleep()
def publish_obstacle_msg():
pub = rospy.Publisher('/test_optim_node/obstacles',
ObstacleArrayMsg,
queue_size=1)
#pub = rospy.Publisher('/p3dx/move_base/TebLocalPlannerROS/obstacles', ObstacleArrayMsg, queue_size=1)
rospy.init_node("test_obstacle_msg")
y_0 = -3.0
vel_x = 0.0
vel_y = 0.3
range_y = 6.0
obstacle_msg = ObstacleArrayMsg()
obstacle_msg.header.stamp = rospy.Time.now()
obstacle_msg.header.frame_id = "map" # CHANGE HERE: odom/map
# Add point obstacle
obstacle_msg.obstacles.append(ObstacleMsg())
obstacle_msg.obstacles[0].id = 99
obstacle_msg.obstacles[0].polygon.points = [Point32()]
obstacle_msg.obstacles[0].polygon.points[0].x = -1.5
obstacle_msg.obstacles[0].polygon.points[0].y = 0
obstacle_msg.obstacles[0].polygon.points[0].z = 0
yaw = math.atan2(vel_y, vel_x)
q = tf.transformations.quaternion_from_euler(0, 0, yaw)
obstacle_msg.obstacles[0].orientation = Quaternion(*q)
obstacle_msg.obstacles[0].velocities.twist.linear.x = vel_x
obstacle_msg.obstacles[0].velocities.twist.linear.y = vel_y
obstacle_msg.obstacles[0].velocities.twist.linear.z = 0
obstacle_msg.obstacles[0].velocities.twist.angular.x = 0
obstacle_msg.obstacles[0].velocities.twist.angular.y = 0
obstacle_msg.obstacles[0].velocities.twist.angular.z = 0
r = rospy.Rate(10) # 10hz
t = 0.0
while not rospy.is_shutdown():
# Vary y component of the point obstacle
if (vel_y >= 0):
obstacle_msg.obstacles[0].polygon.points[0].y = y_0 + (vel_y *
t) % range_y
else:
obstacle_msg.obstacles[0].polygon.points[0].y = y_0 + (
vel_y * t) % range_y - range_y
t = t + 0.1
pub.publish(obstacle_msg)
r.sleep()
def callback_base_pose_ground_truth(base_pose_ground_truth, obst_id):
# Search for obstacle id and update fields if found
if obst_id == 10: return # skip the Ball
i = -1 # -1 is the last element
for idx, obst in enumerate(obstacles_msg.obstacles):
if obst.id == obst_id:
i = idx
if i == -1:
obstacle_msg = ObstacleMsg()
obstacle_msg.id = obst_id
obstacles_msg.obstacles.append(obstacle_msg)
# HEADER
obstacles_msg.header.stamp = rospy.Time.now()
obstacles_msg.header.frame_id = 'map'
# OBSTACLES
obstacles_msg.obstacles[i].header.stamp = obstacles_msg.header.stamp
obstacles_msg.obstacles[i].header.frame_id = obstacles_msg.header.frame_id
obstacles_msg.obstacles[i].polygon.points = [
Point32()
] # currently only point obstacles
obstacles_msg.obstacles[i].polygon.points[
0].x = base_pose_ground_truth.pose.pose.position.x
obstacles_msg.obstacles[i].polygon.points[
0].y = base_pose_ground_truth.pose.pose.position.y
obstacles_msg.obstacles[i].polygon.points[
0].z = base_pose_ground_truth.pose.pose.position.z
# ORIENTATIONS
#yaw = math.atan2(base_pose_ground_truth.twist.twist.linear.y, base_pose_ground_truth.twist.twist.linear.x)
#quat = quaternion_from_euler(0.0, 0.0, yaw) # roll, pitch, yaw in radians
#obstacles_msg.obstacles[i].orientation = Quaternion(*quat.tolist())
obstacles_msg.obstacles[
i].orientation = base_pose_ground_truth.pose.pose.orientation
# VELOCITIES
obstacles_msg.obstacles[i].velocities = base_pose_ground_truth.twist
def add_obstacle(self, id, posx, posy, vel_x, vel_y, range_x, range_y):
self.pose_x.append(posx)
self.pose_y.append(posy)
self.range_x.append(range_x)
self.range_y.append(range_y)
samp_obstacle = ObstacleMsg()
samp_obstacle.id = id
samp_obstacle.radius = 0.35
samp_obstacle.polygon.points = [Point32()]
samp_obstacle.polygon.points[0].x = posx
samp_obstacle.polygon.points[0].y = posy
samp_obstacle.polygon.points[0].z = 0
yaw = math.atan2(vel_y, vel_x)
q = tf.transformations.quaternion_from_euler(0, 0, yaw)
samp_obstacle.orientation = Quaternion(*q)
samp_obstacle.velocities.twist.linear.x = vel_x
samp_obstacle.velocities.twist.linear.y = vel_y
samp_obstacle.velocities.twist.linear.z = 0
samp_obstacle.velocities.twist.angular.x = 0
samp_obstacle.velocities.twist.angular.y = 0
samp_obstacle.velocities.twist.angular.z = 0
return samp_obstacle
def publish_obstacle_msg_moving(pose):
points = unit_test2()
position = pose.position
print('postiions: ', position)
rospy.init_node("test_obstacle_msg")
pub = rospy.Publisher('/test_optim_node/obstacles', ObstacleArrayMsg, queue_size=1)
obstacle_msg = ObstacleArrayMsg()
obstacle_msg.header.stamp = rospy.Time.now()
obstacle_msg.header.frame_id = "odom" # CHANGE HERE: odom/map
lines = [[1, 2, 3], [4, 5, 6]]
scale_x = 250
scale_y = 190
shift_x = 4.5
shift_y = 1
xcoords = points[0]/scale_x
ycoords = points[1]/scale_y
j=0
for idx, xcoord in enumerate(xcoords):
if(idx%50 == 0):
obstacle_msg.obstacles.append(ObstacleMsg())
print(type(xcoord))
x = xcoord - position.x
y = ycoords[idx] - position.y
# rotate coordinates
obstacle_msg.obstacles[j].polygon.points = [Point()]
obstacle_msg.obstacles[j].polygon.points[0].x = y
obstacle_msg.obstacles[j].polygon.points[0].y = -x
obstacle_msg.obstacles[j].polygon.points[0].z = 0
j = j+1
r = rospy.Rate(10) # 10hz
t = 0.0
timeout = time.time() + 10 # 2 seconds from now
while True and not rospy.is_shutdown():
test = 0
if test == 5 or time.time() > timeout:
print('timer about to break')
break
pub.publish(obstacle_msg)
r.sleep()
test - test - 1
def people_tracker_callback(self, msg):
obstacle_msg = ObstacleArrayMsg()
#obstacle_msg.header = msg.header
obstacle_msg.header.stamp = rospy.Time.now()
obstacle_msg.header.frame_id = "world"
i = 0
for j in msg.uuids:
obstacle_msg.obstacles.append(ObstacleMsg())
obstacle_msg.obstacles[i].id = np.int64(msg.uuids[i])
obstacle_msg.obstacles[i].polygon.points = [Point32()]
obstacle_msg.obstacles[i].polygon.points[0].x = msg.poses[
i].position.x
obstacle_msg.obstacles[i].polygon.points[0].y = msg.poses[
i].position.y
obstacle_msg.obstacles[i].polygon.points[0].z = msg.poses[
i].position.z
obstacle_msg.obstacles[i].orientation.x = msg.poses[
i].orientation.x
obstacle_msg.obstacles[i].orientation.y = msg.poses[
i].orientation.y
obstacle_msg.obstacles[i].orientation.z = msg.poses[
i].orientation.z
obstacle_msg.obstacles[i].orientation.w = msg.poses[
i].orientation.w
obstacle_msg.obstacles[
i].velocities.twist.linear.x = msg.velocities[i].x
obstacle_msg.obstacles[
i].velocities.twist.linear.y = msg.velocities[i].y
obstacle_msg.obstacles[
i].velocities.twist.linear.z = msg.velocities[i].z
obstacle_msg.obstacles[i].velocities.twist.angular.x = 0
obstacle_msg.obstacles[i].velocities.twist.angular.y = 0
obstacle_msg.obstacles[i].velocities.twist.angular.z = 0
i = i + 1
print("I am publishing Dynamic Obstacles message obstacle_msg.")
self.pub.publish(obstacle_msg)
def __init__(self):
rospy.init_node('obstacles_publisher', anonymous=True)
self.pub_obstacles = rospy.Publisher(
"/move_base/TebLocalPlannerROS/obstacles",
ObstacleArrayMsg,
queue_size=1)
rospy.Subscriber("field/heap_picked",
Int16,
self.heap_picked_callback,
queue_size=1)
# initial coords of cubes
d = 0.058
self.heaps = []
self.heap = np.array([[0, 0, 0], [-d, 0, 0], [0, d, 0], [d, 0, 0],
[0, -d, 0]])
# cubes in initial positions
self.n_heaps = 6
self.n_cubes_in_heap = 5
for n in range(self.n_heaps):
x = rospy.get_param("/field/cube" + str(n + 1) + "c_x") / 1000
y = rospy.get_param("/field/cube" + str(n + 1) + "c_y") / 1000
self.heaps.append(self.heap + [x, y, 0])
# wether a heap is picked
self.picked = np.array([False, False, False, False, False, False])
# field polygon
self.field = ObstacleMsg()
self.field.header.frame_id = "map"
self.field.polygon.points = [
Point32(0, 0, 0),
Point32(3, 0, 0),
Point32(3, 2, 0),
Point32(0, 2, 0)
]
rospy.Timer(rospy.Duration(0.1), self.pub_timer_callback)
def bbox_to_position_in_odom(self, bboxes, DepthImage, camera_param, i=0, obstacle_msg=ObstacleArrayMsg(), marker_data=MarkerArray()):
if i == 0:
del obstacle_msg.obstacles[:]
del marker_data.markers[:]
for bbox in bboxes.bounding_boxes:
if bbox.Class in self.obstacle_list:
try:
tan_angle_x = camera_param[0][0]*(bbox.xmin+bbox.xmax)/2+camera_param[0][1]*(bbox.ymin+bbox.ymax)/2+camera_param[0][2]*1
angle_x = math.atan(tan_angle_x)
if abs(math.degrees(angle_x)) < 40:
detected_area = DepthImage[bbox.ymin:bbox.ymax, bbox.xmin:bbox.xmax]
distance_x = np.median(detected_area)/1000
distance_x = distance_x + 0.15
distance_y = - distance_x * tan_angle_x
if 1.0 < distance_x < 4.0:
obstacle_msg.obstacles.append(ObstacleMsg())
marker_data.markers.append(Marker())
self.tf_br.sendTransform((-distance_y, 0, distance_x), tf.transformations.quaternion_from_euler(0, 0, 0), rospy.Time.now(), bbox.Class + str(i), bboxes.header.frame_id)
self.tf_listener.waitForTransform("/odom", "/" + bbox.Class + str(i), rospy.Time(0), rospy.Duration(0.5))
obstable_position = self.tf_listener.lookupTransform("/odom", bbox.Class + str(i), rospy.Time(0))
obstacle_msg.obstacles[i].header.stamp, obstacle_msg.obstacles[i].header.frame_id = bboxes.header.stamp, "odom"
obstacle_msg.obstacles[i].id = i
obstacle_msg.obstacles[i].polygon.points = [Point32()]
obstacle_msg.obstacles[i].polygon.points[0].x = obstable_position[0][0]
obstacle_msg.obstacles[i].polygon.points[0].y = obstable_position[0][1]
obstacle_msg.obstacles[i].polygon.points[0].z = obstable_position[0][2]
marker_data.markers[i].header.stamp, marker_data.markers[i].header.frame_id = bboxes.header.stamp, "odom"
marker_data.markers[i].ns, marker_data.markers[i].id = bbox.Class, i
marker_data.markers[i].action = Marker.ADD
marker_data.markers[i].pose.position.x, marker_data.markers[i].pose.position.y, marker_data.markers[i].pose.position.z = obstable_position[0][0], obstable_position[0][1], obstable_position[0][2]
marker_data.markers[i].pose.orientation.x, marker_data.markers[i].pose.orientation.y, marker_data.markers[i].pose.orientation.z, marker_data.markers[i].pose.orientation.w= tf.transformations.quaternion_from_euler(0, 0, 0)
marker_data.markers[i].color.r, marker_data.markers[i].color.g, marker_data.markers[i].color.b, marker_data.markers[i].color.a = 1, 0, 0, 1
marker_data.markers[i].scale.x, marker_data.markers[i].scale.y, marker_data.markers[i].scale.z = 0.2, 0.2, 1
marker_data.markers[i].type = 3
i = i + 1
except Exception as e:
print(e)
return obstacle_msg, marker_data
def to_obsarray_bulked():
array = ObstacleArrayMsg()
array.header.frame_id = 'field'
oid = [0]
def next_id():
oid[0] += 1
return oid[0] - 1
lines = [
# small cone corner
((0, 0), (0, 2.125)),
((0, 0), (2.125, 0)),
((1, 0), (1, 2.125)),
((0, 1), (2.125, 1)),
# long cone corner
((6, 6), (6, 6 - 3.125)),
((6, 6), (6 - 3.125, 6)),
((5, 6), (5, 6 - 3.125)),
((6, 5), (6 - 3.125, 5)),
( # center cones
(2, 2), (2, 2.5), (3.5, 4), (4, 4), (4, 3.5), (2.5, 2)),
# goal line
((0, 4.1), (1.9, 6)),
((4.1, 0), (6, 1.9))
]
for l in lines:
obs = ObstacleMsg(polygon=Polygon([to_point(p) for p in l]))
obs.id = next_id()
array.obstacles.append(obs)
for stationary in ((2, 4), (4, 2)):
obs = ObstacleMsg(polygon=to_square(stationary, 0.127))
obs.id = next_id()
array.obstacles.append(obs)
return array
def publish_obstacle_msg(points):
rospy.init_node("test_obstacle_msg")
pub = rospy.Publisher('/test_optim_node/obstacles', ObstacleArrayMsg, queue_size=1)
obstacle_msg = ObstacleArrayMsg()
obstacle_msg.header.stamp = rospy.Time.now()
obstacle_msg.header.frame_id = "odom" # CHANGE HERE: odom/map
lines = [[1, 2, 3], [4, 5, 6]]
scale_x = 190
scale_y = 260
shift_x = 4.5
shift_y = 1
xcoords = points[0]/scale_x - shift_x
ycoords = points[1]/scale_y - shift_y
# xcoords = lines[0]
# ycoords = lines[1]
# obstacle_msg.obstacles[0].id = 0
j=0
for idx, xcoord in enumerate(xcoords):
if(idx%50 == 0):
obstacle_msg.obstacles.append(ObstacleMsg())
# print('point: x= ', xcoord, ' y= ', ycoords[idx], ' idx: ', idx)
print(type(xcoord))
# Add point obstacle
# v1 = Point32()
# v1.x = xcoord
# v1.y = ycoords[idx]
# obstacle_msg.obstacles[idx].id = idx
x = xcoord
y = ycoords[idx]
# rotate coordinates
obstacle_msg.obstacles[j].polygon.points = [Point()]
obstacle_msg.obstacles[j].polygon.points[0].x = y
obstacle_msg.obstacles[j].polygon.points[0].y = -x
obstacle_msg.obstacles[j].polygon.points[0].z = 0
j = j+1
# obstacle_msg.obstacles.append(ObstacleMsg())
# obstacle_msg.obstacles[1].id = 2
# v1 = Point32()
# v1.x = -1
# v1.y = -1
# v2 = Point32()
# v2.x = -0.5
# v2.y = -1.5
# v3 = Point32()
# v3.x = 0
# v3.y = -1
# obstacle_msg.obstacles[2].polygon.points = [v1, v2, v3]
r = rospy.Rate(10) # 10hz
t = 0.0
timeout = time.time() + 5 # 10 seconds from now
while True and not rospy.is_shutdown():
test = 0
if test == 5 or time.time() > timeout:
print('timer about to break')
break
pub.publish(obstacle_msg)
r.sleep()
test - test - 1
def publish_obstacles(self):
with self.lock:
if self.transport_data is None:
return
# non-leg obstacles
timestamp, xx, yy, clusters, is_legs, cogs, radii, tf_rob_in_fix = self.transport_data
# tracks
track_ids = []
tracks_latest_pos, tracks_color = [], []
tracks_in_frame, tracks_velocities = [], []
tracks_radii = []
for trackid in self.tracker.active_tracks:
track = self.tracker.active_tracks[trackid]
xy = np.array(track.pos_history[-1])
is_track_in_frame = True
if trackid in self.tracker.latest_matches:
color = (0.,1.,0.,1.) # green
elif trackid in self.tracker.new_tracks:
color = (0.,0.,1.,1.) # blue
else:
color = (0.7, 0.7, 0.7, 1.)
is_track_in_frame = False
track_ids.append(trackid)
tracks_latest_pos.append(xy)
tracks_color.append(color)
tracks_in_frame.append(is_track_in_frame)
tracks_velocities.append(track.estimate_velocity())
tracks_radii.append(track.avg_radius())
# publish trackedpersons
from spencer_tracking_msgs.msg import TrackedPersons, TrackedPerson
pub = rospy.Publisher("/tracked_persons", TrackedPersons, queue_size=1)
tp_msg = TrackedPersons()
tp_msg.header.frame_id = self.kFixedFrame
tp_msg.header.stamp = timestamp
for trackid, xy, in_frame, vel, radius in zip(track_ids, tracks_latest_pos, tracks_in_frame, tracks_velocities, tracks_radii):
# if not in_frame:
# continue
tp = TrackedPerson()
tp.track_id = trackid
tp.is_occluded = False
tp.is_matched = in_frame
tp.detection_id = trackid
tp.pose.pose.position.x = xy[0]
tp.pose.pose.position.y = xy[1]
heading_angle = np.arctan2(vel[1], vel[0]) # guess heading from velocity
from geometry_msgs.msg import Quaternion
tp.pose.pose.orientation = Quaternion(
*tf.transformations.quaternion_from_euler(0, 0, heading_angle))
tp.twist.twist.linear.x = vel[0]
tp.twist.twist.linear.y = vel[1]
tp.twist.twist.angular.z = 0 # unknown
tp_msg.tracks.append(tp)
pub.publish(tp_msg)
pub = rospy.Publisher('/obstacles', ObstacleArrayMsg, queue_size=1)
obstacles_msg = ObstacleArrayMsg()
obstacles_msg.header.stamp = timestamp
obstacles_msg.header.frame_id = self.kFixedFrame
for trackid, xy, in_frame, vel, radius in zip(track_ids, tracks_latest_pos, tracks_in_frame, tracks_velocities, tracks_radii):
if not in_frame:
continue
# Add point obstacle
obst = ObstacleMsg()
obst.id = trackid
obst.polygon.points = [Point32()]
obst.polygon.points[0].x = xy[0]
obst.polygon.points[0].y = xy[1]
obst.polygon.points[0].z = 0
obst.radius = radius
yaw = np.arctan2(vel[1], vel[0])
q = tf.transformations.quaternion_from_euler(0,0,yaw)
obst.orientation = Quaternion(*q)
obst.velocities.twist.linear.x = vel[0]
obst.velocities.twist.linear.y = vel[1]
obst.velocities.twist.linear.z = 0
obst.velocities.twist.angular.x = 0
obst.velocities.twist.angular.y = 0
obst.velocities.twist.angular.z = 0
obstacles_msg.obstacles.append(obst)
pub.publish(obstacles_msg)
pub = rospy.Publisher('/close_nonleg_obstacles', ObstacleArrayMsg, queue_size=1)
MAX_DIST_STATIC_CLUSTERS_M = 3.
cogs_in_fix = []
for i in range(len(cogs)):
cogs_in_fix.append(apply_tf(cogs[i], Pose2D(tf_rob_in_fix)))
obstacles_msg = ObstacleArrayMsg()
obstacles_msg.header.stamp = timestamp
obstacles_msg.header.frame_id = self.kFixedFrame
for c, cog_in_fix, cog_in_rob, r, is_leg in zip(clusters, cogs_in_fix, cogs, radii, is_legs):
if np.linalg.norm(cog_in_rob) < self.kRobotRadius:
continue
if len(c) < self.kMinClusterSize:
continue
# leg obstacles are already published in the tracked obstacles topic
if is_leg:
continue
# close obstacles only
if np.linalg.norm(cog_in_rob) > MAX_DIST_STATIC_CLUSTERS_M:
continue
# Add point obstacle
obst = ObstacleMsg()
obst.id = 0
obst.polygon.points = [Point32()]
obst.polygon.points[0].x = cog_in_fix[0]
obst.polygon.points[0].y = cog_in_fix[1]
obst.polygon.points[0].z = 0
obst.radius = r
yaw = 0
q = tf.transformations.quaternion_from_euler(0,0,yaw)
from geometry_msgs.msg import Quaternion
obst.orientation = Quaternion(*q)
obst.velocities.twist.linear.x = 0
obst.velocities.twist.linear.y = 0
obst.velocities.twist.linear.z = 0
obst.velocities.twist.angular.x = 0
obst.velocities.twist.angular.y = 0
obst.velocities.twist.angular.z = 0
obstacles_msg.obstacles.append(obst)
pub.publish(obstacles_msg)
pub = rospy.Publisher('/obstacle_markers', MarkerArray, queue_size=1)
# delete all markers
ma = MarkerArray()
mk = Marker()
mk.header.frame_id = self.kFixedFrame
mk.ns = "obstacles"
mk.id = 0
mk.type = 0 # ARROW
mk.action = 3 # deleteall
ma.markers.append(mk)
pub.publish(ma)
# publish tracks
ma = MarkerArray()
id_ = 0
# track endpoint
for trackid, xy, in_frame, vel in zip(track_ids, tracks_latest_pos, tracks_in_frame, tracks_velocities):
if not in_frame:
continue
normvel = np.linalg.norm(vel)
if normvel == 0:
continue
mk = Marker()
mk.header.frame_id = self.kFixedFrame
mk.ns = "tracks"
mk.id = trackid
mk.type = 0 # ARROW
mk.action = 0
mk.scale.x = np.linalg.norm(vel)
mk.scale.y = 0.1
mk.scale.z = 0.1
mk.color.r = color[0]
mk.color.g = color[1]
mk.color.b = color[2]
mk.color.a = color[3]
mk.frame_locked = True
mk.pose.position.x = xy[0]
mk.pose.position.y = xy[1]
mk.pose.position.z = 0.03
yaw = np.arctan2(vel[1], vel[0])
q = tf.transformations.quaternion_from_euler(0,0,yaw)
mk.pose.orientation = Quaternion(*q)
ma.markers.append(mk)
pub.publish(ma)
def find_closest_n_so(SO_data, pose, n_SO):
# This function finds the closest n_SO number of obstacles.
#If there are less measured obstacle than n_SO append extra far away obstacles to the SO data
if len(SO_data.obstacles[:]) < n_SO:
for i in range(len(SO_data.obstacles[:]), n_SO + 1):
fill_obs = ObstacleMsg()
fill_obs.polygon.points = [Point32()]
fill_obs.polygon.points[0].x = 100
fill_obs.polygon.points[0].y = 100
SO_data.obstacles.append(fill_obs)
dist = np.zeros((1, len(SO_data.obstacles[:])))
for k in range(len(SO_data.obstacles[:])):
#print('This is for point {}'.format(k), SO_data.obstacles[k].polygon.points[:])
#print('This is for point {}'.format(k), len(SO_data.obstacles[k].polygon.points[:]))
[x, y] = poligon2centroid(SO_data.obstacles[k].polygon.points[:])
#print('This is centroid x and y: ', [x, y])
dist[0, k] = np.sqrt((pose[0] - x)**2 + (pose[1] - y)**2)
#print('This is the dist: ', dist[0, k])
# Indexes of the closest Static obstacles
n_idx = (dist).argsort()[:n_SO]
# cl_obs is a n_SO*6 long vector that will be filled up with the closest triangles, lines and points
cl_obs = np.zeros([n_SO * 6])
cl_sizes = np.zeros([n_SO])
for k in range(n_SO):
SO_now = SO_data.obstacles[n_idx[0, k]].polygon.points[:]
if len(SO_now) == 1:
cl_obs[k * 6:k * 6 + 6] = np.array(
[SO_now[0].x, SO_now[0].y, 0, 0, 0, 0])
cl_sizes[k] = 1
elif len(SO_now) == 2:
if SO_now[0].x == SO_now[1].x and SO_now[0].y == SO_now[1].y:
cl_obs[k * 6:k * 6 + 6] = np.array(
[SO_now[0].x, SO_now[0].y, 0, 0, 0, 0])
cl_sizes[k] = 1
else:
cl_obs[k * 6:k * 6 + 6] = np.array(
[SO_now[0].x, SO_now[0].y, SO_now[1].x, SO_now[1].y, 0, 0])
cl_sizes[k] = 2
elif len(SO_now) == 3:
if SO_now[0].x == SO_now[2].x and SO_now[0].y == SO_now[2].y:
cl_obs[k * 6:k * 6 + 6] = np.array(
[SO_now[0].x, SO_now[0].y, SO_now[1].x, SO_now[1].y, 0, 0])
cl_sizes[k] = 2
else:
cl_obs[k * 6:k * 6 + 6] = np.array([
SO_now[0].x, SO_now[0].y, SO_now[1].x, SO_now[1].y,
SO_now[2].x, SO_now[2].y
])
cl_sizes[k] = 3
else:
cl_sizes[k] = 3
distances = np.zeros(len(SO_now) - 1)
for i in range(len(SO_now) - 1):
distances[i] = np.sqrt((pose[0] - SO_now[i].x)**2 +
(pose[1] - SO_now[i].y)**2)
cl_node_index = distances.argsort()[:n_SO]
cl_obs[k * 6:k * 6 + 6] = np.array([
SO_now[cl_node_index[0]].x, SO_now[cl_node_index[0]].y,
SO_now[cl_node_index[1]].x, SO_now[cl_node_index[1]].y,
SO_now[cl_node_index[2]].x, SO_now[cl_node_index[2]].y
])
#print('These are the closest points for obs {}'.format(k+1), cl_obs[k*6:k*6+6])
#print('cl_obs', cl_obs)
#print('This is the shape of cl_obs', cl_obs.shape)
#print('cl_sizes', cl_sizes)
combined_obs_and_size = np.append(cl_obs, cl_sizes)
return combined_obs_and_size
def run():
global h2_odom, h3_odom, tf_buffer, tf_listener_1
rospy.init_node('local_planning_test_script')
tf_buffer = tf2_ros.Buffer(rospy.Duration(1200.0)) # tf buffer length
tf_listener = tf2_ros.TransformListener(tf_buffer)
tf_listener_1 = tf.TransformListener()
pub_h2 = rospy.Publisher("/h2/cmd_vel", Twist, queue_size=1)
pub_h3 = rospy.Publisher("/h3/cmd_vel", Twist, queue_size=1)
pub_h1_goal = rospy.Publisher("/move_base_simple/goal",
PoseStamped,
queue_size=1)
sub_h1_pose = rospy.Subscriber("/odometry/filtered", Odometry, h1_odom_cb)
sub_h2_pose = rospy.Subscriber("/h2/odometry/filtered", Odometry,
h2_odom_cb)
sub_h3_pose = rospy.Subscriber("/h3/odometry/filtered", Odometry,
h3_odom_cb)
pub_h1_obstacless = rospy.Publisher(
"/move_base/TebLocalPlannerROS/obstacles",
ObstacleArrayMsg,
queue_size=1)
rate = rospy.Rate(20)
count = 0
while not rospy.is_shutdown():
# try:
# (trans, rot) = tf_listener.lookupTransform('/map', '/h2/odom', rospy.Time(0))
# except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
# continue
# print(type(trans))
# print(type(rot))
# print(h2_odom)
# print(h3_odom)
if h2_odom is not None and h3_odom is not None:
h2_obstacles = ObstacleMsg()
h2_obstacles.header = h2_odom.header
point = Point32()
point.x = h2_odom.pose.pose.position.x
point.y = h2_odom.pose.pose.position.y
point.z = h2_odom.pose.pose.position.z
h2_obstacles.polygon.points.append(point)
h2_obstacles.radius = 0.5
h2_obstacles.velocities = h2_odom.twist
#print("h2 velocity:{} {}".format(h2_odom.twist.twist.linear.x,h2_odom.twist.twist.linear.y))
h2_obstacles.orientation = h2_odom.pose.pose.orientation
h3_obstacles = ObstacleMsg()
point1 = Point32()
point1.x = h3_odom.pose.pose.position.x
point1.y = h3_odom.pose.pose.position.y
point1.z = h3_odom.pose.pose.position.z
h3_obstacles.header = h3_odom.header
h3_obstacles.polygon.points.append(point1)
h3_obstacles.radius = 0.5
h3_obstacles.velocities = h3_odom.twist
h3_obstacles.orientation = h3_odom.pose.pose.orientation
msg = ObstacleArrayMsg()
msg.header = h2_obstacles.header
msg.obstacles.append(h2_obstacles)
msg.obstacles.append(h3_obstacles)
pub_h1_obstacless.publish(msg)
if count < 20:
#print("begin pub")
msg = PoseStamped()
msg.header.seq = count
msg.header.frame_id = "map"
msg.pose.position.x = -6
msg.pose.position.y = 0
msg.pose.orientation.x = 0.0
msg.pose.orientation.y = 0.0
msg.pose.orientation.z = 0.0
msg.pose.orientation.w = 1.0
pub_h1_goal.publish(msg)
# if 40 < count < 190: this seting will hit the robot
if 20 < count < 380:
msg_h2 = Twist()
msg_h2.linear.x = 1.0
msg_h2.angular.z = 0.0
pub_h2.publish(msg_h2)
if 20 < count < 1800:
msg_h3 = Twist()
msg_h3.linear.x = 0.30 #use teb set it 0.30,use steb set it 0.45
msg_h3.angular.z = 0.0
#pub_h3.publish(msg_h3)
if count >= 2800:
break
count += 1
storage()
rate.sleep()
return
def publish_obstacle_msg():
pub = rospy.Publisher('/move_base/TebLocalPlannerROS/obstacles',
ObstacleArrayMsg,
queue_size=1)
rospy.init_node("obstacle_msg")
obstacle_msg = ObstacleArrayMsg()
obstacle_msg.header.stamp = rospy.Time.now()
obstacle_msg.header.frame_id = "odom" # CHANGE HERE: odom/map
# Add line obstacle
obstacle_msg.obstacles.append(ObstacleMsg())
obstacle_msg.obstacles[0].id = 1
line_start = Point32()
line_start.x = -2.0
line_start.y = 0.0
line_end = Point32()
line_end.x = -0.5
line_end.y = 0.0
obstacle_msg.obstacles[0].polygon.points = [line_start, line_end]
# Add line obstacle
obstacle_msg.obstacles.append(ObstacleMsg())
obstacle_msg.obstacles[1].id = 2
line_start = Point32()
line_start.x = -0.5
line_start.y = 0.0
line_end = Point32()
line_end.x = -0.5
line_end.y = -0.5
obstacle_msg.obstacles[1].polygon.points = [line_start, line_end]
# Add line obstacle
obstacle_msg.obstacles.append(ObstacleMsg())
obstacle_msg.obstacles[2].id = 2
line_start = Point32()
line_start.x = -0.5
line_start.y = -0.5
line_end = Point32()
line_end.x = 0.5
line_end.y = -0.5
obstacle_msg.obstacles[2].polygon.points = [line_start, line_end]
# Add line obstacle
obstacle_msg.obstacles.append(ObstacleMsg())
obstacle_msg.obstacles[3].id = 3
line_start = Point32()
line_start.x = 0.5
line_start.y = -0.5
line_end = Point32()
line_end.x = 0.5
line_end.y = 0.0
obstacle_msg.obstacles[3].polygon.points = [line_start, line_end]
# Add line obstacle
obstacle_msg.obstacles.append(ObstacleMsg())
obstacle_msg.obstacles[4].id = 3
line_start = Point32()
line_start.x = 0.5
line_start.y = 0.0
line_end = Point32()
line_end.x = 2.0
line_end.y = 0.0
obstacle_msg.obstacles[4].polygon.points = [line_start, line_end]
r = rospy.Rate(10) # 10hz
t = 0.0
while not rospy.is_shutdown():
pub.publish(obstacle_msg)
r.sleep()
def publish_obstacle_msg():
pub = rospy.Publisher('/test_optim_node/obstacles',
ObstacleArrayMsg,
queue_size=1)
rospy.init_node("obstacle_msg")
obstacle_msg = ObstacleArrayMsg()
obstacle_msg.header.stamp = rospy.Time.now()
obstacle_msg.header.frame_id = "odom/map" # CHANGE HERE: odom/map
# Add bottom obstacle
obstacle_msg.obstacles.append(ObstacleMsg())
obstacle_msg.obstacles[0].id = 0
v0 = Point32()
v0.x = -12.0
v0.y = 0.0
v1 = Point32()
v1.x = 0.0
v1.y = 0.0
v2 = Point32()
v2.x = 0.0
v2.y = -2.5
v3 = Point32()
v3.x = 7.5
v3.y = -2.5
v4 = Point32()
v4.x = 7.5
v4.y = 0
v5 = Point32()
v5.x = 17
v5.y = 0
v6 = Point32()
v6.x = 17
v6.y = -10
v7 = Point32()
v7.x = -12
v7.y = -10
obstacle_msg.obstacles[0].polygon.points = [v0, v1, v2, v3, v4, v5, v6, v7]
# Add top obstacle
obstacle_msg.obstacles.append(ObstacleMsg())
obstacle_msg.obstacles[1].id = 1
v0 = Point32()
v0.x = 17.0
v0.y = 5.0
v1 = Point32()
v1.x = -12.0
v1.y = 5.0
v2 = Point32()
v2.x = -12.0
v2.y = 10.0
v3 = Point32()
v3.x = 17.0
v3.y = 10.0
obstacle_msg.obstacles[1].polygon.points = [v0, v1, v2, v3]
r = rospy.Rate(100) # 10hz
while not rospy.is_shutdown():
pub.publish(obstacle_msg)
r.sleep()
def to_obsarray():
array = ObstacleArrayMsg()
array.header.frame_id = 'field'
oid = [0]
def next_id():
oid[0] += 1
return oid[0] - 1
red_line = [to_point((0, 4.1)), to_point((1.9, 6))]
blue_line = [to_point((4.1, 0)), to_point((6, 1.9))]
for l in (blue_line, red_line):
obs = ObstacleMsg(polygon=Polygon(l))
obs.id = next_id()
array.obstacles.append(obs)
for stationary in ((2, 4), (4, 2)):
obs = ObstacleMsg(polygon=to_square(stationary, 0.127))
obs.id = next_id()
array.obstacles.append(obs)
for color, pos in world.goals:
obs = ObstacleMsg(polygon=to_square(pos, 0.127))
obs.id = next_id()
array.obstacles.append(obs)
for pos in world.cones:
obs = ObstacleMsg(polygon=to_square(pos, 0.076))
obs.id = next_id()
array.obstacles.append(obs)
return array
def publish_obstacles_scan(self, flow, dt):
'''
Generate and publish ObstacleArrayMsg from flow vectors.
'''
scan_resolution = 20.0 / self.num_r # scan_cartesian_one side(m)/index; resolution = 0.105 for index:190
obstacle_vels = flow / dt * scan_resolution # dt = 0.0249~~
# Generate obstacle_msg for TebLocalPlanner here.
obstacle_msg = ObstacleArrayMsg()
obstacle_msg.header.stamp = rospy.Time.now()
obstacle_msg.header.frame_id = self.global_frame
# get the scan sensor pose (= robot_pose)
sensor_position, sensor_quaternion = self.listener.lookupTransform(
'/base_scan', '/map', rospy.Time())
sensor_euler = tf.transformations.euler_from_quaternion(
sensor_quaternion) # return (roll, pitch, yaw)
sensor_angle = -sensor_euler[2] # angle = yaw
_cos = math.cos(sensor_angle)
_sin = math.sin(sensor_angle)
robot_pose = (sensor_position[0], sensor_position[1], sensor_angle)
obstacle_speed = obstacle_vels[:, 2:]
obstacle_speed = np.linalg.norm(obstacle_speed, axis=1)
#mask_img = gaussian_filter(self.scan_msg.ranges.T, 1.0)
#obstacle_speed[mask_img < 40] = 0
for i in range(obstacle_vels.shape[0]):
# Add point obstacle to the message depend on obstacle speed
if obstacle_speed[i] > 0 and obstacle_speed[i] < 1:
m = sensor_position[0] + obstacle_vels[i][0] / 3
n = sensor_position[1] + obstacle_vels[i][1] / 3
# k = _cos*m - _sin*n
# l = _sin*m + _cos*n
# flow_vector_position = (
# _cos*k + _sin*l,
# - _sin*k + _cos*l
# )
flow_vector_position = (m, n)
# flow_vector_position = (
# _cos*sensor_position[0] + _sin*sensor_position[1] + obstacle_vels[i][0]/3,
# _sin*sensor_position[0] + _cos*sensor_position[1] + obstacle_vels[i][1]/3
# )
# 첫 발행까지 시간소요 높음, 회전하면서 postion 동시에 변화
# flow_vector_position = (
# sensor_position[0] + (_cos*obstacle_vels[i][0] - _sin*obstacle_vels[i][1])/3,
# sensor_position[1] + (_sin*obstacle_vels[i][0] + _cos*obstacle_vels[i][1])/3
# )
# 첫 발행까지 시간소요 높음, 회전하면서 postion 동시에 변화
line_scale = (1. - np.exp(-np.linalg.norm([
flow_vector_position[0] - robot_pose[0],
flow_vector_position[1] - robot_pose[1]
]) / 2.)) * self.prediction_horizon
# print(flow_vector_position)
obstacle_msg.obstacles.append(ObstacleMsg())
obstacle_msg.obstacles[-1].id = len(obstacle_msg.obstacles) - 1
obstacle_msg.obstacles[-1].polygon.points = [
Point32(), Point32()
]
obstacle_msg.obstacles[-1].polygon.points[
0].x = flow_vector_position[0]
obstacle_msg.obstacles[-1].polygon.points[
0].y = flow_vector_position[1]
obstacle_msg.obstacles[-1].polygon.points[0].z = 0
obstacle_msg.obstacles[-1].polygon.points[
1].x = flow_vector_position[
0] + obstacle_vels[i][2] * line_scale / 10
obstacle_msg.obstacles[-1].polygon.points[
1].y = flow_vector_position[
1] + obstacle_vels[i][3] * line_scale / 10
obstacle_msg.obstacles[-1].polygon.points[1].z = 0
self.obstacle_pub.publish(obstacle_msg) # Publish predicted obstacles
print("one msg published")
def dyn_obs_main():
RosNodeName = 'dyn_obs_polygon'
rospy.init_node(RosNodeName, anonymous=True)
lidarPointsLabelled_listener = LidarPointsLabelledListener()
#lidarPoints_listener = LidarPointsListener()
humanState_listener = HumanStateListener()
#encoderIMU_listener = EncoderIMUListener()
icp_listener = ICPListener()
lvls = LvlSetGenerator("low")
rate_init = rospy.Rate(10.0)
rate_init.sleep()
pub = rospy.Publisher('/move_base/TebLocalPlannerROS/obstacles',
ObstacleArrayMsg,
queue_size=1)
#WP = Waypoint()
#MPC=doMPC()
while not rospy.is_shutdown():
start = time.time()
#waypoint = WP.waypoint
#if doGoalOnly == False:
# rangeData = lidarPoints_listener.RangeData
#else:
# rangeData = []
staticData = lidarPointsLabelled_listener.StaticData
personData = lidarPointsLabelled_listener.PersonData
#robot_state = encoderIMU_listener.State # wrt to global origin
robot_state = icp_listener.State # wrt to global origin
#print "robot state", robot_state
#robot_twist = encoderIMU_listener.Twist # wrt to global frame
human_state = humanState_listener.State # wrt to GV body frame
#print "robot state", robot_state
obstacle_msg = ObstacleArrayMsg()
obstacle_msg.header.stamp = rospy.Time.now()
obstacle_msg.header.frame_id = "odom"
# Add polygon obstacle
for i in xrange(len(human_state)):
obstacle_msg.obstacles.append(ObstacleMsg())
#print human_state
vx = human_state[i][2]
vy = human_state[i][3]
print "vx", vx
print "vy", vy
if fabs(vx) <= 0.1 and fabs(vy) <= 0.1:
vx = 0.0
vy = 0.0
if vx == 0.0:
vx = 1e-5
t = atan(vy / vx)
if vx * vy >= 0.0:
if vx <= 0.0:
t = t + pi
else:
if vx < 0.0:
t = t + pi
elif vy < 0.0:
t = t + 2 * pi
print "obs", i, human_state[i][0], human_state[i][1]
#print "angle", t
if human_state[i][0] == 0.0 and human_state[i][1] == 0.0:
continue
obstacle_msg.obstacles[i].id = 2
obstacle_msg.obstacles[i].polygon.points = []
X, Y = lvls.CalculateLevelSet(t)
l = len(X)
X = np.zeros((1, l), dtype=np.float) + X
Y = np.zeros((1, l), dtype=np.float) + Y
#X=np.array([[-1.0,0.0,1.0,0.0]])
#Y=np.array([[0.0,-1.0,0.0,1.0]])
X = X / 3
Y = Y / 3
t1 = -robot_state[2]
R1 = [[cos(t1), -sin(t1)], [sin(t1), cos(t1)]]
hs = np.matmul(R1, [[human_state[i][0]], [human_state[i][1]]])
#print hs
x = np.ones((1, l), dtype=np.float) * (robot_state[0] + hs[0])
y = np.ones((1, l), dtype=np.float) * (robot_state[1] + hs[1])
xy = np.concatenate((x, y), axis=0)
XY = np.concatenate((X, Y), axis=0)
R = [[cos(t), -sin(t)], [sin(t), cos(t)]]
XY = np.matmul(R, XY)
XY = XY + xy
XY = np.transpose(XY)
#print "xy", xy
#print "XY", XY
Obshull = ConvexHull(XY)
ObsCvxHull = XY[Obshull.vertices, :]
len_poly = len(ObsCvxHull)
#print ObsCvxHull
#v=Point32()
for j in xrange(len_poly):
obstacle_msg.obstacles[i].polygon.points.append(Point32())
obstacle_msg.obstacles[i].polygon.points[j].x = ObsCvxHull[j][
0]
obstacle_msg.obstacles[i].polygon.points[j].y = ObsCvxHull[j][
1]
#print obstacle_msg.obstacles[0].polygon.points
pub.publish(obstacle_msg)
#else:
# print "angle"
"""
v1 = Point32()
v1.x = -1
v1.y = -1
v2 = Point32()
v2.x = -0.5
v2.y = -1.5
v3 = Point32()
v3.x = 0
v3.y = -1
#print v1
obstacle_msg.obstacles[0].polygon.points = [v1, v2, v3]
obstacle_msg.obstacles[0].polygon.points.append(v1)
#print obstacle_msg.obstacles[0].polygon.points
"""
rate_init.sleep()
#if len(human_state):
#print "human state", human_state[0]
#if doPlot:
#ax.plot(human_state[0][1], human_state[0][0],color='blue',marker='D')
#Cluster = clusters(rangeData)
#staticCluster = clusters(staticData)
#personCluster = clusters(personData)
if doPlot:
ax.scatter(Cluster.clusterXY[:, 1],
Cluster.clusterXY[:, 0],
color='black')
#ax.scatter(staticCluster.clusterXY[:,1], staticCluster.clusterXY[:,0], color='black')
#ax.scatter(personCluster.clusterXY[:,1], personCluster.clusterXY[:,0], color='green')
#elliHull = EnclosingEllipse(Cluster)
#elliHull = EnclosingEllipse(staticCluster)
#count = len(Cluster.clusterIdx)
#count = len(staticCluster.clusterIdx)
"""
for i in xrange(count):
#print elliHull.a[i], elliHull.b[i]
centroid = [elliHull.centroid[i][1],elliHull.centroid[i][0]]
if fabs(elliHull.m[i]) < 1e-6:
if elliHull.m[i] >0:
elliHull.m[i] = 0.001
else:
elliHull.m[i] = -0.001
if doPlot:
elli=patches.Ellipse(xy=centroid,width=2*elliHull.a[i], height=2*elliHull.b[i], angle=degrees(atan(1/(elliHull.m[i]))), fc='None')
ax.add_artist(elli)
if doPlot:
ax.grid()
ax.axis([-5,5,0,5])
ax.set_title('Obstacle position w.r.t to robot')
plt.pause(0.05)
#plt.draw()
ax.cla()
"""
end = time.time()
k1 = np.array((MO_init[k, 3] * ca.cos(MO_init[k, 2]),
MO_init[k, 3] * ca.sin(MO_init[k, 2])))
k2 = np.array(
(MO_init[k, 3] * ca.cos(MO_init[k, 2]) + step / 2 * k1[0],
MO_init[k, 3] * ca.sin(MO_init[k, 2]) + step / 2 * k1[1]))
k3 = np.array(
(MO_init[k, 3] * ca.cos(MO_init[k, 2]) + step / 2 * k2[0],
MO_init[k, 3] * ca.sin(MO_init[k, 2]) + step / 2 * k2[1]))
k4 = np.array((MO_init[k, 3] * ca.cos(MO_init[k, 2]) + step * k3[0],
MO_init[k, 3] * ca.sin(MO_init[k, 2]) + step * k3[0]))
xf = np.array((MO_init[k, 0], MO_init[k, 1]) + step / 6 *
(k1 + 2 * k2 + 2 * k3 + k4))
MO_init[k, 0] = xf[0]
MO_init[k, 1] = xf[1]
print('This is for n_MO {}'.format(k), xf)
# Add obstacle parameters and states at time Ts to the obstacle message
MO_msg.obstacles.append(ObstacleMsg())
MO_msg.obstacles[k].id = k
MO_msg.obstacles[k].polygon.points = [Point32()]
MO_msg.obstacles[k].polygon.points[0].x = xf[0]
MO_msg.obstacles[k].polygon.points[0].y = xf[1]
MO_msg.obstacles[k].radius = MO_init[k, 4]
MO_msg.obstacles[k].orientation.z = MO_init[k, 2]
MO_msg.obstacles[k].velocities.twist.linear.x = MO_init[k, 3]
#print(MO_msg.obstacles[0])
rate.sleep()
pub.publish(MO_msg)
print('It should be published now')
def pt(x, y):
p = Point32()
p.x = x
p.y = y
p.z = 0
return p
pub = rospy.Publisher("/obstacles", ObstacleArrayMsg, queue_size=1)
rospy.init_node("talker")
rate = rospy.Rate(10)
msg = ObstacleArrayMsg()
ob1 = ObstacleMsg()
ob1.header.frame_id = "map"
# ob1.polygon.points = [pt(-5.1, 4.9), pt(4.9,4.9), pt(4.9, -5.1), pt(5.1,-5.1), pt(5.1,5.1), pt(-5.1,5.1)]
ob1.polygon.points = [pt(-5, 5), pt(-4, 6), pt(-3, 6), pt(-3, 2)]
ob2 = ObstacleMsg()
ob2.header.frame_id = "map"
# ob2.polygon.points = [pt(-5.1,-5.1), pt(-4.9, -5.1), pt(-4.9, 4.9), pt(-5.1, 4.9)]
ob2.polygon.points = [pt(5, 1), pt(2, 6), pt(4, 7), pt(6, 5), pt(6, 1)]
ob3 = ObstacleMsg()
ob3.header.frame_id = "map"
# ob3.polygon.points = [pt(0,5), pt(0,10), pt(2,10), pt(2, 5)]
ob3.polygon.points = [pt(-4, -3), pt(0, -5), pt(3, -4), pt(5, -1)]
msg.header.frame_id = "map"