diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000..ba0430d
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1 @@
+__pycache__/
\ No newline at end of file
diff --git a/ROAM-Mapping/params/multi_robot_semantic_mapping_params.yaml b/ROAM-Mapping/params/multi_robot_semantic_mapping_params.yaml
index 72e3add..92653e7 100755
--- a/ROAM-Mapping/params/multi_robot_semantic_mapping_params.yaml
+++ b/ROAM-Mapping/params/multi_robot_semantic_mapping_params.yaml
@@ -2,9 +2,12 @@ octomap:
octomap_topic: "/octomap_full"
pointcloud_topic: "/$(arg name)/semantic_pcl"
frame_id: "world"
+ # resolution: 0.3
+ # max_range: 10.0
+ # raycast_range: 0.5
resolution: 0.3
- max_range: 10.0
- raycast_range: 0.5
+ max_range: 15.0
+ raycast_range: 10.0
clamping_thres_min: 0.006692851
clamping_thres_max: 0.993307149
occupancy_thres: 0.5
@@ -13,25 +16,26 @@ octomap:
psi: 1
phi: -0.3
publish_2d_map: true
- min_ground_z: -1.5
- max_ground_z: 1.5
- consensus_weight: 0.8
- sub_timer: 10
- pub_timer: 10
+ min_ground_z: 0.0
+ max_ground_z: 0.7
+ consensus_weight: 0.2
+ sub_timer: 5
+ pub_timer: 5
# Camera intrinsic matrix parameters
camera:
- fx: 65.24951330239097
- fy: 65.24951330239097
- cx: 80.0
- cy: 60.0
+ fx: 320
+ fy: 320
+ cx: 320
+ cy: 240
- width: 160
- height: 120
+ width: 640
+ height: 480
semantic_pcl:
- color_image_topic: "/$(arg name)/camera/image"
- semantic_image_topic: "/$(arg name)/camera/semantic_image"
- depth_image_topic: "/$(arg name)/depth_camera/depth/image_rect_raw"
+ color_image_topic: "/$(arg name)/image"
+ semantic_image_topic: "/$(arg name)/colored_map"
+ depth_image_topic: "/$(arg name)/depth_image"
point_type: 0
- frame_id: "$(arg name)/camera"
+ frame_id: "$(arg name)/camera_optic"
+ unit_conversion: 1.0
\ No newline at end of file
diff --git a/ROAM-Mapping/src/octomap_generator_semantic_ros.cpp b/ROAM-Mapping/src/octomap_generator_semantic_ros.cpp
index 79dba9d..528481f 100644
--- a/ROAM-Mapping/src/octomap_generator_semantic_ros.cpp
+++ b/ROAM-Mapping/src/octomap_generator_semantic_ros.cpp
@@ -166,7 +166,8 @@ void SemanticOctomapGeneratorNode::publish2DOccupancyMap(const SemanticOctree* o
{
double node_z = it.getZ();
- double node_half_side = pow(it.getSize(), 1/3) / 2;
+ // double node_half_side = pow(it.getSize(), 1/3) / 2;
+ double node_half_side = it.getSize()/2;
double top_side = node_z + node_half_side;
double bottom_side = node_z - node_half_side;
diff --git a/ROAM-Planning/CMakeLists.txt b/ROAM-Planning/CMakeLists.txt
index c49a8da..bada4f3 100644
--- a/ROAM-Planning/CMakeLists.txt
+++ b/ROAM-Planning/CMakeLists.txt
@@ -40,7 +40,7 @@ set(SOURCE_FILES
src/cpp/src/exploration/util.cpp
)
-set(EXPLORATION_LIB_DESTINATION "${CATKIN_DEVEL_PREFIX}/lib/python2.7/dist-packages")
+set(EXPLORATION_LIB_DESTINATION "${CATKIN_DEVEL_PREFIX}/lib/python3/dist-packages")
add_library(exploration_cpp SHARED ${PROJECT_SOURCE_DIR} ${SOURCE_FILES} src/cpp/src/exploration/python.cpp)
target_link_libraries(exploration_cpp ${PYTHON_LIBRARIES})
set_target_properties(exploration_cpp PROPERTIES SUFFIX ".so" PREFIX "_"
diff --git a/ROAM-Planning/launch/planning_multi.launch b/ROAM-Planning/launch/planning_multi.launch
index 4e4d58c..338328b 100755
--- a/ROAM-Planning/launch/planning_multi.launch
+++ b/ROAM-Planning/launch/planning_multi.launch
@@ -12,11 +12,11 @@
-
- -->
+
diff --git a/ROAM-Planning/params/multi_robot_exploration_params.yaml b/ROAM-Planning/params/multi_robot_exploration_params.yaml
index 5c4b5be..238c495 100644
--- a/ROAM-Planning/params/multi_robot_exploration_params.yaml
+++ b/ROAM-Planning/params/multi_robot_exploration_params.yaml
@@ -1,40 +1,40 @@
footprint:
- type: 'tricky_circle'
+ type: 'jackal'
angular_resolution: 0.01745
- inflation_scale: 1.3
+ inflation_scale: 1.4
planning:
- SE2_radius: 16.0
- max_iter: 5
- max_path_length: 100
- step_size: 0.001
- angular_range: 1.570796327
+ SE2_radius: 2
+ max_iter: 10
+ max_path_length: 15.0
+ step_size: 0.001
+ angular_range: 1.570796327
angular_samples: 32
num_orientation_samples: 8
- distance_coeff: 0.001
+ distance_coeff: 0.01
theta_coeff: 0.1
min_frontier_size: 30
- goal_check_radius: 2.0
- draw_radius: 15
+ goal_check_radius: 0.3
+ draw_radius: 0.2
+ clean_kernel_size: 0.6
+ erode_kernel_size: 0.8
+ median_filter_enable: false
map_topic: '/$(arg name)/occupancy_map_2D'
collision_check_period: 1
multi:
- num_agents: 6
- horizon: 5
- delta_q: 10.0
- gamma_q: 0.01
- robot_colision_coeff: 1000000
- epsilon: 1
- optimization_period: 0.1
- should_plan_threshold: 0.3
- need_plan_threshold: 0.2
- need_plan_period: 1.0
+ num_agents: 2
+ horizon: 10
+ delta_q: 10.0
+ gamma_q: 0.01
+ robot_colision_coeff: 5
+ epsilon: 0.1
+ should_plan_threshold: 0.6
+ need_plan_threshold: 0.6
+ need_plan_period: 0.05
navigation:
- tau: 0.005
- alpha: 0.999
- radius: 2.0
- dist: 1.0
- safety_dist: 1.0
+ tau: 0.005
+ dist: 0.2
+ safety_dist: 0.1
publish_odom: true
diff --git a/ROAM-Planning/src/exploration/ga_multi_exploration_node.py b/ROAM-Planning/src/exploration/ga_multi_exploration_node.py
index 2da202c..8d6072c 100755
--- a/ROAM-Planning/src/exploration/ga_multi_exploration_node.py
+++ b/ROAM-Planning/src/exploration/ga_multi_exploration_node.py
@@ -9,17 +9,14 @@
import cv2
import numpy as np
-import matplotlib.pyplot as plt
-
from nav_msgs.msg import OccupancyGrid
from nav_msgs.msg import Path
-from std_msgs.msg import Empty
+from std_msgs.msg import Empty, Header
from geometry_msgs.msg import PoseStamped
from rospy.numpy_msg import numpy_msg
from rospy_tutorials.msg import Floats
-
from utilities.frontier_utils import extract_frontiers, cleanup_map_for_planning
-from footprints.footprint_points import get_tricky_circular_footprint, get_tricky_oval_footprint
+from footprints.footprint_points import get_tricky_circular_footprint, get_tricky_oval_footprint, get_jackal_footprint
from footprints.footprints import CustomFootprint
from planners.astar_cpp import oriented_astar, get_astar_angles
from utilities.util import rc_to_xy, wrap_angles, xy_to_rc
@@ -30,9 +27,8 @@
class GradAscentMultiExplorationAgent:
def __init__(self):
self.robot_name = '/' + rospy.get_param("~agent_name", 'husky')
-
footprint_type = rospy.get_param(self.robot_name + '/footprint/type')
-
+ self.filter_obstacles_flag = rospy.get_param(self.robot_name + '/planning/median_filter_enable')
if footprint_type == 'tricky_circle':
footprint_points = get_tricky_circular_footprint()
elif footprint_type == 'tricky_oval':
@@ -44,10 +40,20 @@ def __init__(self):
footprint_radius * np.array([np.cos(rotation_angles), np.sin(rotation_angles)]).T
elif footprint_type == 'pixel':
footprint_points = np.array([[0., 0.]])
+ elif footprint_type == 'jackal':
+ footprint_points = get_jackal_footprint()
else:
footprint_points = None
assert False and "footprint type specified not supported."
+ clean_kernel_size = int(round(rospy.get_param(self.robot_name + '/planning/clean_kernel_size')\
+ / rospy.get_param(self.robot_name + '/octomap/resolution')))
+ erode_kernel_size = int(round(rospy.get_param(self.robot_name + '/planning/erode_kernel_size')\
+ / rospy.get_param(self.robot_name + '/octomap/resolution')))
+ self.kernel_cleanup = cv2.getStructuringElement(cv2.MORPH_RECT, (clean_kernel_size, clean_kernel_size))
+ self.kernel_erosion = cv2.getStructuringElement(cv2.MORPH_RECT, (erode_kernel_size, erode_kernel_size))
+ self.plan_map_pub = rospy.Publisher(self.robot_name + "/PLANMAP", OccupancyGrid, queue_size = 1, latch=True)
+
self.footprint = CustomFootprint(footprint_points=footprint_points,
angular_resolution=np.pi / 4,
inflation_scale=rospy.get_param(self.robot_name + '/footprint/inflation_scale'))
@@ -105,7 +111,7 @@ def __init__(self):
self.waypoint_inds = None
self.goal = None
self.goal_check_radius = rospy.get_param(self.robot_name + '/planning/goal_check_radius')
- self.draw_radius = rospy.get_param(self.robot_name + '/planning/draw_radius')
+ self.draw_radius = int(np.round(rospy.get_param(self.robot_name + '/planning/draw_radius')/rospy.get_param(self.robot_name + '/octomap/resolution')))
self.map_sub = rospy.Subscriber(rospy.get_param(self.robot_name + '/planning/map_topic'), OccupancyGrid, self.map_callback)
self.collision_check_timer = rospy.get_time()
@@ -151,17 +157,14 @@ def __init__(self):
self.vectorized_rc = None
self.vectorized_xy = None
self.eval_cells = None
- self.kernel_cleanup = cv2.getStructuringElement(cv2.MORPH_RECT, (5, 5))
- self.kernel_erosion = cv2.getStructuringElement(cv2.MORPH_RECT, (5, 5))
-
rospy.sleep(1)
print('Multi agent exploration agent \'{}\' initialized!'.format(self.robot_name))
def team_path_callback(self, team_path_msg):
-
+
if team_path_msg.data[0] == self.agent_id:
return
-
+
if self.received_team_path_num == 0:
self.consensus_delta = np.copy(team_path_msg.data[1:])
else:
@@ -170,12 +173,12 @@ def team_path_callback(self, team_path_msg):
self.received_team_path_num += 1
def need_plan_callback(self, need_plan_msg):
-
+
self.need_plan[1:] = need_plan_msg.data[1:]
if self.check_ready():
self.need_plan[self.agent_id] = 1
-
+
if np.mean(self.need_plan[1:]) >= self.should_plan_thresh:
self.optimize_pose()
else:
@@ -187,8 +190,6 @@ def need_plan_callback(self, need_plan_msg):
def optimize_pose(self):
if self.is_planning:
return
-
- print('Planning started for agent \'{}\'!'.format(self.robot_name))
self.is_planning = True
self.consensus_delta = None
self.received_team_path_num = 0
@@ -200,7 +201,7 @@ def optimize_pose(self):
if np.mean(self.need_plan[1:]) < self.need_plan_thresh:
print('{}: Peer-pressured to stop planning!'.format(self.robot_name))
break
-
+
if self.received_team_path_num > 0:
unweighted_delta = self.consensus_delta - self.received_team_path_num * self.team_path[1:]
self.team_path[1:] += self.weight_mask * unweighted_delta / (self.received_team_path_num + 1)
@@ -217,14 +218,14 @@ def optimize_pose(self):
cell_scores = self.compute_score(reachable_rc)
team_grad[3 * self.horizon * (self.agent_id - 1) + 3 * i:\
3 * self.horizon * (self.agent_id - 1) + 3 * (i + 1)] += self.compute_gradient(pose, cell_scores, reachable_rc)
-
+
for agent in range(self.num_agents):
t_init = 0
robot_collision_penalty = self.robot_colision_coeff
if agent + 1 == self.agent_id:
t_init = i + 1
robot_collision_penalty = 1
-
+
for t in range(t_init, self.horizon):
other_pose = self.team_path[1 + 3 * self.horizon * agent + 3 * t:1 + 3 * self.horizon * agent + 3 * (t + 1)]
dist = np.linalg.norm(pose[:2] - other_pose[:2])
@@ -237,17 +238,18 @@ def optimize_pose(self):
3 * self.horizon * (self.agent_id - 1) + 3 * i + 2] -= grad
team_grad[3 * self.horizon * agent + 3 * t:\
3 * self.horizon * agent + 3 * t + 2] += grad
-
+
self.team_path[1:] += team_grad * self.adjusted_step_size / (iter_num + 1)
self.team_path[3::3] = wrap_angles(self.team_path[3::3])
-
+
self.team_path_pub.publish(self.team_path)
print('{}: {}-th iteration of pose optimization Finished! Gradient norm: {}'.format(self.robot_name, iter_num + 1, np.linalg.norm(team_grad * self.adjusted_step_size / (iter_num + 1))))
-
+
self.publish_path()
self.is_planning = False
def make_init_plan(self):
+ print('{}: LOOKING FOR FRONTIERS!'.format(self.robot_name))
if self.raytrace_precomputed is None:
print('{}: Initializing ray-tracing!'.format(self.robot_name))
max_range = rospy.get_param(self.robot_name + '/octomap/max_range')
@@ -307,7 +309,6 @@ def compute_frontier_path(self, robot_pose):
robot_pose_forward = robot_pose.copy()
robot_pose_forward[0] = robot_pose[0] + 1 * np.cos(robot_pose[2])
robot_pose_forward[1] = robot_pose[1] + 1 * np.sin(robot_pose[2])
-
if frontiers is not None:
for i, f in enumerate(frontiers):
if np.linalg.norm(robot_pose_forward[:2] - f[:2]) < 1:
@@ -323,13 +324,13 @@ def compute_frontier_path(self, robot_pose):
if path_length > self.goal_check_radius:
pixel_path = xy_to_rc(path, self.planning_map)
try:
- collision_inds = np.nonzero(self.planning_map.data[pixel_path[:, 0].astype(int),
- pixel_path[:, 1].astype(int)] == Costmap.OCCUPIED)[0]
- if collision_inds.shape[0] == 0:
- path_list.append(path)
- path_score_list.append(frontier_size[i] / (1 + path_length))
+ collision_inds = np.nonzero(self.planning_map.data[pixel_path[:, 0].astype(int),
+ pixel_path[:, 1].astype(int)] == Costmap.OCCUPIED)[0]
+ if collision_inds.shape[0] == 0:
+ path_list.append(path)
+ path_score_list.append(frontier_size[i] / (1 + path_length))
except IndexError:
- continue
+ continue
best_path = None
if len(path_list) == 0:
@@ -406,12 +407,12 @@ def get_pose_from_tf(self, from_frame_id):
no_transfrom = True
while no_transfrom:
try:
- (translation, rotation) = self.tf_listener.lookupTransform(self.world_frame_id,
- from_frame_id,
- rospy.Time(0))
- no_transfrom = False
+ (translation, rotation) = self.tf_listener.lookupTransform(self.world_frame_id,
+ from_frame_id,
+ rospy.Time(0))
+ no_transfrom = False
except tf.LookupException or tf.ConnectivityException:
- continue
+ continue
euler = tf.transformations.euler_from_quaternion(rotation)
return np.array([translation[0], translation[1], euler[2]])
@@ -435,6 +436,7 @@ def check_ready(self):
def map_callback(self, occ_map_msg):
if not self.is_planning:
+
occupancy_map = 255 - np.array(occ_map_msg.data, dtype=np.uint8)
occupancy_map = occupancy_map.reshape((occ_map_msg.info.height,
occ_map_msg.info.width))
@@ -447,10 +449,20 @@ def map_callback(self, occ_map_msg):
resolution = occ_map_msg.info.resolution
self.planning_map = Costmap(occupancy_map, resolution, origin)
-
- self.planning_map = cleanup_map_for_planning(occupancy_map=self.planning_map, kernel=self.kernel_cleanup, filter_obstacles=True)
+ self.planning_map = cleanup_map_for_planning(occupancy_map=self.planning_map, kernel=self.kernel_cleanup, filter_obstacles=self.filter_obstacles_flag)
self.planning_map.data = cv2.erode(self.planning_map.data, self.kernel_erosion, iterations=1)
-
+ tmp_occ_map = self.planning_map.data.copy()
+ tmp_occ_map_int = np.zeros_like(tmp_occ_map, dtype=np.int8)
+ tmp_occ_map_int[tmp_occ_map == Costmap.UNEXPLORED] = -1
+ tmp_occ_map_int[tmp_occ_map == Costmap.FREE] = 0
+ tmp_occ_map_int[tmp_occ_map == Costmap.OCCUPIED] = 100
+ tmp_occ_map_int = np.flipud(tmp_occ_map_int)
+ # plan_map_msg = OccupancyGrid()
+ # plan_map_msg.header = occ_map_msg.header
+ # plan_map_msg.info = occ_map_msg.info
+ # plan_map_msg.data = tmp_occ_map_int.flatten().tolist()
+ # self.plan_map_msg = plan_map_msg
+ # self.plan_map_pub.publish(plan_map_msg)
check_collision = False
curr_time = rospy.get_time()
if (curr_time - self.collision_check_timer) > self.collision_check_period:
@@ -459,55 +471,55 @@ def map_callback(self, occ_map_msg):
if self.curr_path is not None and check_collision:
pixel_path = xy_to_rc(self.curr_path, self.planning_map)
-
try:
collision_inds = np.nonzero(self.planning_map.data[pixel_path[:, 0].astype(int),pixel_path[:, 1].astype(int)] == Costmap.OCCUPIED)[0]
except IndexError:
return
if collision_inds.shape[0] > 0:
- print('{}: Collision ahead!'.format(self.robot_name))
self.collision_pub.publish(Empty())
-
+
self.is_planning = True
-
+
rem_waypoint_inds = self.waypoint_inds[self.waypoint_inds > collision_inds[0]]
rem_waypoints = self.curr_path[rem_waypoint_inds, :]
-
+
robot_pose = self.get_pose_from_tf(self.robot_frame_id)
self.footprint.draw_circumscribed(robot_pose, self.planning_map, self.draw_radius)
full_path = robot_pose[None, :]
self.waypoint_inds = np.array([0], dtype=int)
for pose in rem_waypoints:
- plan_success, partial_path = oriented_astar(start=full_path[-1], occupancy_map=self.planning_map, footprint=self.footprint, obstacle_values=[Costmap.OCCUPIED], epsilon=1, goal=pose)
+ plan_success, partial_path = oriented_astar(start=full_path[-1], occupancy_map=self.planning_map, footprint=self.footprint,
+ obstacle_values=[Costmap.OCCUPIED], epsilon=1, goal=pose)
if plan_success:
full_path = np.vstack((full_path, partial_path))
self.waypoint_inds = np.concatenate((self.waypoint_inds, [full_path.shape[0] - 1]))
-
+
if len(full_path.shape) > 1:
- path_length = 0
- curr_pose = full_path[0]
- for i, next_pose in enumerate(full_path[1:]):
- path_length += np.linalg.norm(next_pose[:2] - curr_pose[:2])
- if path_length > self.max_path_length:
- full_path = full_path[:i+1]
- self.waypoint_inds = self.waypoint_inds[self.waypoint_inds < full_path.shape[0]]
- break
- else:
- curr_pose = next_pose
+ path_length = 0
+ curr_pose = full_path[0]
+ for i, next_pose in enumerate(full_path[1:]):
+ path_length += np.linalg.norm(next_pose[:2] - curr_pose[:2])
+ if path_length > self.max_path_length:
+ full_path = full_path[:i+1]
+ self.waypoint_inds = self.waypoint_inds[self.waypoint_inds < full_path.shape[0]]
+ break
+ else:
+ curr_pose = next_pose
self.curr_path = full_path
self.goal = full_path[-1]
-
+
path_msg = Path()
path_msg.header.frame_id = self.world_frame_id
-
+
if len(full_path.shape) == 1:
self.add_waypoint(full_path, path_msg)
else:
for pose in full_path:
self.add_waypoint(pose, path_msg)
-
+ print("Revised path generated!")
+
self.path_pub.publish(path_msg)
self.is_planning = False
@@ -657,14 +669,15 @@ def get_failsafe_path(self, state):
if path_length > self.goal_check_radius:
pixel_path = xy_to_rc(random_path, self.planning_map)
try:
- collision_inds = np.nonzero(self.planning_map.data[pixel_path[:, 0].astype(int),
- pixel_path[:, 1].astype(int)] == Costmap.OCCUPIED)[0]
- if collision_inds.shape[0] == 0:
- path = random_path
+ collision_inds = np.nonzero(self.planning_map.data[pixel_path[:, 0].astype(int),
+ pixel_path[:, 1].astype(int)] == Costmap.OCCUPIED)[0]
+ if collision_inds.shape[0] == 0:
+ path = random_path
except IndexError:
- continue
+ continue
rospy.sleep(0.2)
-
+ assert path is not None, "Fail-safe path generation failed!"
+ print(self.robot_name+" Fail-safe path generated!")
return path
def sample_free_pose(self):
@@ -707,6 +720,7 @@ def publish_path(self):
full_path = robot_pose[None, :]
self.waypoint_inds = np.array([0], dtype=int)
for pose in path:
+ # print('waypoint: ', pose)
plan_success, partial_path = oriented_astar(start=full_path[-1],
occupancy_map=self.planning_map,
footprint=self.footprint,
@@ -719,22 +733,23 @@ def publish_path(self):
collision_inds = np.nonzero(self.planning_map.data[pixel_path[:, 0].astype(int),
pixel_path[:, 1].astype(int)] == Costmap.OCCUPIED)[0]
if collision_inds.shape[0] == 0:
- full_path = np.vstack((full_path, partial_path))
- self.waypoint_inds = np.concatenate((self.waypoint_inds, [full_path.shape[0] - 1]))
+ full_path = np.vstack((full_path, partial_path))
+ self.waypoint_inds = np.concatenate((self.waypoint_inds, [full_path.shape[0] - 1]))
+ else:
+ raise ValueError('Collision detected in the path!')
except IndexError:
continue
-
if len(full_path.shape) > 1:
- path_length = 0
- curr_pose = full_path[0]
- for i, next_pose in enumerate(full_path[1:]):
- path_length += np.linalg.norm(next_pose[:2] - curr_pose[:2])
- if path_length > self.max_path_length:
- full_path = full_path[:i+1]
- self.waypoint_inds = self.waypoint_inds[self.waypoint_inds < full_path.shape[0]]
- break
- else:
- curr_pose = next_pose
+ path_length = 0
+ curr_pose = full_path[0]
+ for i, next_pose in enumerate(full_path[1:]):
+ path_length += np.linalg.norm(next_pose[:2] - curr_pose[:2])
+ if path_length > self.max_path_length:
+ full_path = full_path[:i+1]
+ self.waypoint_inds = self.waypoint_inds[self.waypoint_inds < full_path.shape[0]]
+ break
+ else:
+ curr_pose = next_pose
self.curr_path = full_path
self.goal = full_path[-1]
@@ -751,7 +766,7 @@ def publish_path(self):
self.path_pub.publish(path_msg)
- rospy.loginfo("{}: New path published!".format(self.robot_name))
+ rospy.loginfo("{}: Brand New path published!".format(self.robot_name))
def main(args):
diff --git a/ROAM-Planning/src/footprints/footprint_points.py b/ROAM-Planning/src/footprints/footprint_points.py
index 640b5ec..871e7a6 100644
--- a/ROAM-Planning/src/footprints/footprint_points.py
+++ b/ROAM-Planning/src/footprints/footprint_points.py
@@ -54,3 +54,49 @@ def get_tricky_oval_footprint():
[1303.15, -286.54],
[1341.34, -118.37]]
) / 1000.
+
+def generate_clockwise_square(x_size, y_size, points_per_side):
+ """
+ Generates points on a clockwise square with the given parameters
+ :param x_size float: size in the x axis
+ :param y_size float: size in the y axis
+ :param points_per_side int: number of points to generate per side
+ :return array(N, 2)[float]: points defining the square
+ """
+ y_range = np.linspace(-y_size / 2, y_size / 2, points_per_side, endpoint=True)
+ x_range = np.linspace(-x_size / 2, x_size / 2, points_per_side, endpoint=True)
+ y_range = np.vstack(([np.zeros_like(y_range)], [y_range])).T
+ x_range = np.vstack(([[x_range], np.zeros_like(x_range)])).T
+
+ back_points = y_range + x_range[0, :]
+
+ front_points = y_range + x_range[-1, :]
+ zero_ind = np.argmin(np.abs(front_points[:, 1]))
+ front_first_half = front_points[zero_ind:, :]
+ front_last_half = front_points[:zero_ind]
+
+ left_points = x_range + y_range[-1, :]
+
+ right_points = x_range + y_range[0, :]
+ right_points = right_points[::-1, :]
+
+ # points = np.vstack((back_points, front_points, left_points, right_points))
+ points = np.vstack((front_first_half, right_points, back_points, left_points, front_last_half))
+ return points
+
+
+def get_jackal_footprint(points_per_side=11, debug=False):
+ """
+ Gives an estimate of the ClearPath jackal robot footprint
+ :param points_per_side int: number of points per side to discretize
+ :param debug bool: show plots?
+ :return array(N, 2)[float]: points defining the footprint
+ """
+ assert points_per_side % 2 != 0 and "Must be odd, so 0 is generated"
+ length = .508
+ width = .430
+ points = generate_clockwise_square(length, width, points_per_side)
+ if debug:
+ plt.scatter(points[:, 0], points[:, 1])
+ plt.show()
+ return points
\ No newline at end of file
diff --git a/ROAM-Planning/src/navigation/simple_path_navigation_node.py b/ROAM-Planning/src/navigation/simple_path_navigation_node.py
index c648be9..dd6fcc4 100755
--- a/ROAM-Planning/src/navigation/simple_path_navigation_node.py
+++ b/ROAM-Planning/src/navigation/simple_path_navigation_node.py
@@ -12,7 +12,7 @@
from nav_msgs.msg import Path, Odometry
from std_msgs.msg import Empty
-from geometry_msgs.msg import Pose, PoseStamped
+from geometry_msgs.msg import Pose, PoseStamped, Twist
class PathNavigation:
@@ -35,6 +35,7 @@ def __init__(self):
self.collision_sub = message_filters.Subscriber(self.robot_name + '/planner/collision', Empty, queue_size = 15)
self.collision_sub.registerCallback(self.collision_callback)
+ self.cmd_vel_pub = rospy.Publisher(self.robot_name + '/cmd_vel', Twist, queue_size = 15)
self.path = None
self.position_cmd_pub = rospy.Publisher(self.robot_name + "/planner/position_cmd", PoseStamped, queue_size = 15)
@@ -54,18 +55,19 @@ def __init__(self):
peer_collision = False
for i in range(num_agents):
- if self.robot_name[-1] == str(i + 1):
- continue
- peer_frame_id = self.robot_name[:-1] + str(i + 1) + '/base_link' # Assuming the robot frame is named as 'robot_i/base_link'
- peer_pose = self.get_pose_from_tf(peer_frame_id)
- if np.linalg.norm(np.array([next_pose.position.x - peer_pose[0], next_pose.position.y - peer_pose[1]])) <= safety_dist:
- peer_collision = True
- rospy.loginfo("{}: Possible close call! Stopping for deconflicting...".format(self.robot_name))
- break
+ if self.robot_name[-1] == str(i + 1):
+ continue
+ peer_frame_id = self.robot_name[:-1] + str(i + 1) + '/base_link' # Assuming the robot frame is named as 'robot_i/base_link'
+ peer_pose = self.get_pose_from_tf(peer_frame_id)
+ if np.linalg.norm(np.array([next_pose.position.x - peer_pose[0], next_pose.position.y - peer_pose[1]])) <= safety_dist:
+ peer_collision = True
+ rospy.loginfo("{}: Possible close call! Stopping for deconflicting...".format(self.robot_name))
+ break
if peer_collision:
- rospy.sleep(0.5)
- continue
+ self.cmd_vel_pub.publish(Twist())
+ rospy.sleep(0.5)
+ continue
position_cmd_msg = PoseStamped()
position_cmd_msg.pose = next_pose
@@ -81,6 +83,8 @@ def __init__(self):
self.path = self.path[1:]
else:
self.path = None
+ else:
+ self.cmd_vel_pub.publish(Twist())
rospy.sleep(tau)
def get_pose_from_tf(self, from_frame_id):
@@ -109,12 +113,12 @@ def get_tf(self, from_frame_id):
no_transfrom = True
while no_transfrom:
try:
- (translation, rotation) = self.tf_listener.lookupTransform(self.world_frame_id,
- from_frame_id,
- rospy.Time(0))
- no_transfrom = False
+ (translation, rotation) = self.tf_listener.lookupTransform(self.world_frame_id,
+ from_frame_id,
+ rospy.Time(0))
+ no_transfrom = False
except tf.LookupException:
- continue
+ continue
return (translation, rotation)
def path_callback(self, path_msg):