def make_msg(self, Depth1Image, Depth2Image, detection_data, camera1_param, camera2_param):
bboxes_from_camera1 = BoundingBoxes()
bboxes_from_camera2 = BoundingBoxes()
bboxes_from_camera1.header = Depth1Image.header
bboxes_from_camera2.header = Depth2Image.header
self.now = rospy.get_rostime()
self.timebefore = detection_data.header.stamp
# Add point obstacle
self.obstacle_msg.header.stamp = detection_data.header.stamp
self.obstacle_msg.header.frame_id = "odom" # CHANGE HERE: odom/map
#opencvに変換
bridge = CvBridge()
try:
Depth1image = bridge.imgmsg_to_cv2(Depth1Image, desired_encoding="passthrough")
Depth2image = bridge.imgmsg_to_cv2(Depth2Image, desired_encoding="passthrough")
except CvBridgeError as e:
print(e)
for bbox in detection_data.bounding_boxes:
if (bbox.ymin + bbox.ymax)/2 < Depth1image.shape[0]:
if bbox.ymax > Depth1image.shape[0]:
bbox.ymax = Depth1image.shape[0]
bboxes_from_camera1.bounding_boxes.append(bbox)
else:
bbox.ymin = bbox.ymin - Depth1image.shape[0]
bbox.ymax = bbox.ymax - Depth1image.shape[0]
if bbox.ymin < 0:
bbox.ymin = 0
bboxes_from_camera2.bounding_boxes.append(bbox)
camera1_obstacle_msg, camera2_obstacle_msg = ObstacleArrayMsg(), ObstacleArrayMsg()
camera1_marker_data, camera2_marker_data = MarkerArray(), MarkerArray()
camera1_obstacle_msg, camera1_marker_data = self.bbox_to_position_in_odom(bboxes_from_camera1, Depth1image, camera1_param)
obstacle_msg, marker_data = self.bbox_to_position_in_odom(bboxes_from_camera2, Depth2image, camera2_param, len(camera1_obstacle_msg.obstacles), camera1_obstacle_msg, camera1_marker_data)
self.obstacle_msg.obstacles, self.marker_data.markers = self.update_obstacles(self.obstacle_msg, obstacle_msg, self.marker_data, marker_data)
def pub_timer_callback(self, event):
obstacle_msg = ObstacleArrayMsg()
obstacle_msg.header.stamp = rospy.Time.now()
obstacle_msg.header.frame_id = "map"
obstacle_msg.obstacles = self.heaps2cubes()
obstacle_msg.obstacles.append(self.field)
self.pub_obstacles.publish(obstacle_msg)
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(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 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_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 update_obstacles(self, prev_obstacle_msg, detected_obstacle_msg,
prev_marker_msg, marker_msg):
self.tf_listener.waitForTransform("/odom", "/base_link", rospy.Time(0),
rospy.Duration(0.3))
current_position = self.tf_listener.lookupTransform(
"/odom", "/base_link", rospy.Time(0))
updated_obstacle_msg = ObstacleArrayMsg()
updated_marker_data = MarkerArray()
print(len(detected_obstacle_msg.obstacles))
for detected_obstacle, marker in zip(detected_obstacle_msg.obstacles,
marker_msg.markers):
"""
for prev_obstacle, prev_marker in zip(prev_obstacle_msg.obstacles, prev_marker_msg.markers):
if abs(current_position[0][0] - prev_obstacle.polygon.points[0].x) < 4 or abs(current_position[0][1] - prev_obstacle.polygon.points[0].y) < 4:
if ((detected_obstacle.polygon.points[0].x - prev_obstacle.polygon.points[0].x) * (detected_obstacle.polygon.points[0].x - prev_obstacle.polygon.points[0].x) + (detected_obstacle.polygon.points[0].y - prev_obstacle.polygon.points[0].y) * (detected_obstacle.polygon.points[0].y - prev_obstacle.polygon.points[0].y)) < 1.0:
prev_obstacle.polygon.points[0].x = detected_obstacle.polygon.points[0].x
prev_obstacle.polygon.points[0].y = detected_obstacle.polygon.points[0].y
prev_marker.pose.position.x = marker.pose.position.x
prev_marker.pose.position.y = marker.pose.position.y
updated_obstacle_msg.obstacles.append(detected_obstacle)
updated_marker_data.markers.append(marker)
"""
updated_obstacle_msg.obstacles.append(detected_obstacle)
updated_marker_data.markers.append(marker)
return updated_obstacle_msg.obstacles, updated_marker_data.markers
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 __init__(self):
# Publisherを作成
self.publisher = rospy.Publisher('/move_base/TebLocalPlannerROS/obstacles', ObstacleArrayMsg, queue_size=1)
self.marker_publisher = rospy.Publisher("/visualized_obstacle", MarkerArray, queue_size = 1)
self.obstacle_list = ["person"]
self.tf_br = tf.TransformBroadcaster()
self.tf_listener = tf.TransformListener()
self.obstacle_msg = ObstacleArrayMsg()
self.marker_data = MarkerArray()
self.now = rospy.get_rostime()
self.prev = rospy.get_rostime()
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 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 __init__(self):
# Publisherを作成
self.publisher = rospy.Publisher('/move_base/TebLocalPlannerROS/obstacles', ObstacleArrayMsg, queue_size=1)
self.marker_publisher = rospy.Publisher("/visualized_obstacle", MarkerArray, queue_size = 1)
self.landmark_publisher = rospy.Publisher("/rtabmap/tag_detections", AprilTagDetectionArray, queue_size = 1)
self.landmark_publisher = rospy.Publisher("/rtabmap/gps/fix", NavSatFix, queue_size = 1)
self.obstacle_list = ["person", "tree", "landmark"]
self.landmark_list = ["landmark"]
self.tf_br = tf.TransformBroadcaster()
self.tf_listener = tf.TransformListener()
self.obstacle_msg = ObstacleArrayMsg()
self.marker_data = MarkerArray()
self.marker_data = AprilTagDetectionArray()
self.now = rospy.get_rostime()
self.prev = rospy.get_rostime()
def __init__(self):
rospy.init_node("test_obstacle_msg")
self.obstacle_pub = rospy.Publisher(
'/move_base/TebLocalPlannerROS/obstacles',
ObstacleArrayMsg,
queue_size=1)
self.marker_pub = rospy.Publisher('dynamic_obstacle_markers',
MarkerArray,
queue_size=1)
self.obstacle_msg = ObstacleArrayMsg()
self.obs_array_markers = MarkerArray()
self.range_x = []
self.range_y = []
self.pose_x = []
self.pose_y = []
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 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 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 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(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 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 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()
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_x = distance_x - 0.9
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] - 0.15
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
#!/usr/bin/env python
import rospy, math, random, re
from tf.transformations import quaternion_from_euler
from geometry_msgs.msg import Twist, PolygonStamped, Quaternion, QuaternionStamped, TwistWithCovariance, Point32
from nav_msgs.msg import Odometry
from costmap_converter.msg import ObstacleArrayMsg, ObstacleMsg
# global variables
obstacles_msg = ObstacleArrayMsg()
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
MO_init = np.array([[-3.85, -0.22, ca.pi, -0.6, 0.3], [-4, 1, 0.0, 0.6, 0.3],
[3.91, -11.63, -ca.pi, 0.6, 0.2],
[-4.41, -9.5, ca.pi, -0.6,
0.3]]) # Define all the obstacles in the workspace.
n_MO = len(MO_init[:, 0])
pub = rospy.Publisher('/MO_Obstacles', ObstacleArrayMsg,
queue_size=1) # Setup the publisher and the topic
pub1 = rospy.Publisher('/visualization_marker', MarkerArray, queue_size=1)
rate = rospy.Rate(10) # Define the publishing rate
step = 0.1
while not rospy.is_shutdown():
MO_msg = ObstacleArrayMsg() # Create a msg of type ObstacleArrayMsg
MO_msg.header.stamp = rospy.Time.now()
MO_msg.header.frame_id = '/map'
markerArray = MarkerArray()
for k in range(n_MO):
# Calculate the next step using runge kutta 4. order
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],
import rospy
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)]
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 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 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 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[:]
self.landmark_msg.header.stamp = bboxes.header.stamp
self.landmark_msg.header.frame_id = "base_link"
for bbox in bboxes.bounding_boxes:
if bbox.Class in self.obstacle_list:
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)) < 35:
detected_area = DepthImage[bbox.ymin:bbox.ymax,
bbox.xmin:bbox.xmax]
distance_x = np.median(detected_area) / 1000
distance_x = distance_x + 0.15
if bboxes.header.frame_id == "camera2_color_optical_frame":
distance_x = -distance_x
distance_y = -distance_x * tan_angle_x
if 1.0 < distance_x < 3.0:
self.tf_br.sendTransform(
(-distance_y, 0, distance_x),
tf.transformations.quaternion_from_euler(0, 0, 0),
rospy.Time.now(), bbox.Class,
bboxes.header.frame_id)
self.tf_listener.waitForTransform(
"/base_link", "/" + bbox.Class, rospy.Time(0),
rospy.Duration(0.1))
landmark_position = self.tf_listener.lookupTransform(
"/base_link", bboxes.header.frame_id,
rospy.Time(0))
self.landmark_msg.detections[0].id = [1]
self.landmark_msg.detections[0].size = [0.3]
self.landmark_msg.detections[
0].pose.header = bboxes.header
self.landmark_msg.detections[
0].pose.header.frame_id = bbox.Class
self.landmark_msg.detections[
0].pose.pose.pose.position.x = landmark_position[
0][0]
self.landmark_msg.detections[
0].pose.pose.pose.position.y = landmark_position[
0][1]
self.landmark_msg.detections[
0].pose.pose.pose.position.z = landmark_position[
0][2]
self.landmark_msg.detections[
0].pose.pose.covariance = [
(self.landmark_msg.detections[0].pose.pose.
pose.position.x * 0.01) *
(self.landmark_msg.detections[0].pose.pose.
pose.position.x * 0.01), 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 9999, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
(self.landmark_msg.detections[0].pose.pose.
pose.position.z * 0.01) *
(self.landmark_msg.detections[0].pose.pose.
pose.position.z * 0.01), 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 9999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
9999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 9999
]
self.landmark_publisher.publish(self.landmark_msg)
return obstacle_msg, marker_data