Skip to content

Commit

Permalink
publish /path_all for dashboard
Browse files Browse the repository at this point in the history 
Signed-off-by: Daisuke Sato <daisukes@cmu.edu>
  • Loading branch information
@daisukes
daisukes committed Feb 5, 2024
1 parent 8987b13 commit 07be9c0
Show file tree
Hide file tree
Showing 2 changed files with 79 additions and 66 deletions.
133 changes: 67 additions & 66 deletions cabot_ui/cabot_ui/navgoal.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -290,6 +290,72 @@ def make_goals(delegate, groute, anchor, yaw=None):
return goals


def create_ros_path(navcog_route, anchor, global_map_name):
"""convert a NavCog path to ROS path"""

# convert route to points
points = []
CaBotRclpyUtil.info(F"create_ros_path, {str(navcog_route)}")
last_index = len(navcog_route)-1

def convert(g, a=anchor):
return geoutil.global2local(g, a)

for (index, item) in enumerate(navcog_route):
if index == 0 and isinstance(item.geometry, geojson.LineString):
# if the first item is link, add the source node
points.append(convert(item.source_node.geometry))
elif index == last_index:
# if last item is Point (Node), it would be same as the previous link target node
if isinstance(item.geometry, geojson.Point):
continue
else:
# if the link is a left of the graph and short
if isinstance(item, geojson.RouteLink):
if item.is_leaf and item.length < 3.0:
continue

if isinstance(item.geometry, geojson.Point):
points.append(convert(item.geometry))
elif isinstance(item.geometry, geojson.LineString):
points.append(convert(item.target_node.geometry))
else:
CaBotRclpyUtil.info("geometry is not point or linestring {item.geometry}")
CaBotRclpyUtil.info(F"{index}: {str(points[-1])}")

# make a path from points
path = nav_msgs.msg.Path()
path.header.frame_id = global_map_name
path.poses = []
quat = None
pori = None
for i in range(0, len(points)):
start = points[i]
pose = geometry_msgs.msg.PoseStamped()
pose.header.frame_id = global_map_name
pose.pose = geometry_msgs.msg.Pose()
pose.pose.position.x = start.x
pose.pose.position.y = start.y
pose.pose.position.z = 0.0
if i+1 < len(points):
end = points[i+1]
direction = math.atan2(end.y - start.y, end.x - start.x)
quat = tf_transformations.quaternion_from_euler(0, 0, direction)

pose.pose.orientation.x = quat[0]
pose.pose.orientation.y = quat[1]
pose.pose.orientation.z = quat[2]
pose.pose.orientation.w = quat[3]
path.poses.append(pose)
pori = pose.pose.orientation
else:
pose.pose.orientation = pori
path.poses.append(pose)

CaBotRclpyUtil.info(F"path {path}")
return (path, path.poses[-1])


class Goal(geoutil.TargetPlace):
def __init__(self, delegate, **kwargs):
super(Goal, self).__init__(**kwargs)
Expand Down Expand Up @@ -396,7 +462,7 @@ def __init__(self, delegate, navcog_route, anchor, target_poi=None, set_back=(0,
self.global_map_name = delegate.global_map_name()
self.navcog_route = navcog_route
self.anchor = anchor
(self.ros_path, last_pose) = self._create_ros_path()
(self.ros_path, last_pose) = create_ros_path(self.navcog_route, self.anchor, self.global_map_name)
self.pois = self._extract_pois()
self.handle = None

Expand Down Expand Up @@ -470,71 +536,6 @@ def goal_name_pron(self):
def goal_description(self):
return self._goal_description

def _create_ros_path(self):
"""convert a NavCog path to ROS path"""

# convert route to points
points = []
CaBotRclpyUtil.info(F"create_ros_path, {str(self.navcog_route)}")
last_index = len(self.navcog_route)-1

def convert(g, a=self.anchor):
return geoutil.global2local(g, a)

for (index, item) in enumerate(self.navcog_route):
if index == 0 and isinstance(item.geometry, geojson.LineString):
# if the first item is link, add the source node
points.append(convert(item.source_node.geometry))
elif index == last_index:
# if last item is Point (Node), it would be same as the previous link target node
if isinstance(item.geometry, geojson.Point):
continue
else:
# if the link is a left of the graph and short
if isinstance(item, geojson.RouteLink):
if item.is_leaf and item.length < 3.0:
continue

if isinstance(item.geometry, geojson.Point):
points.append(convert(item.geometry))
elif isinstance(item.geometry, geojson.LineString):
points.append(convert(item.target_node.geometry))
else:
CaBotRclpyUtil.info("geometry is not point or linestring {item.geometry}")
CaBotRclpyUtil.info(F"{index}: {str(points[-1])}")

# make a path from points
path = nav_msgs.msg.Path()
path.header.frame_id = self.global_map_name
path.poses = []
quat = None
pori = None
for i in range(0, len(points)):
start = points[i]
pose = geometry_msgs.msg.PoseStamped()
pose.header.frame_id = self.global_map_name
pose.pose = geometry_msgs.msg.Pose()
pose.pose.position.x = start.x
pose.pose.position.y = start.y
pose.pose.position.z = 0.0
if i+1 < len(points):
end = points[i+1]
direction = math.atan2(end.y - start.y, end.x - start.x)
quat = tf_transformations.quaternion_from_euler(0, 0, direction)

pose.pose.orientation.x = quat[0]
pose.pose.orientation.y = quat[1]
pose.pose.orientation.z = quat[2]
pose.pose.orientation.w = quat[3]
path.poses.append(pose)
pori = pose.pose.orientation
else:
pose.pose.orientation = pori
path.poses.append(pose)

CaBotRclpyUtil.info(F"path {path}")
return (path, path.poses[-1])

def _extract_pois(self):
"""extract pois along the route"""
temp = []
Expand Down
12 changes: 12 additions & 0 deletions cabot_ui/cabot_ui/navigation.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -357,6 +357,8 @@ def __init__(self, node: Node, datautil_instance=None, anchor_file=None, wait_fo
self.plan_sub = node.create_subscription(nav_msgs.msg.Path, plan_input, self._plan_callback, 10, callback_group=MutuallyExclusiveCallbackGroup())
path_output = node.declare_parameter("path_topic", "/path").value
self.path_pub = node.create_publisher(nav_msgs.msg.Path, path_output, transient_local_qos, callback_group=MutuallyExclusiveCallbackGroup())
path_all_output = node.declare_parameter("path_all_topic", "/path_all").value
self.path_all_pub = node.create_publisher(nav_msgs.msg.Path, path_all_output, transient_local_qos, callback_group=MutuallyExclusiveCallbackGroup())

self.updated_goal_sub = node.create_subscription(geometry_msgs.msg.PoseStamped, "/updated_goal", self._goal_updated_callback, 10, callback_group=MutuallyExclusiveCallbackGroup())

Expand Down Expand Up @@ -534,6 +536,16 @@ def _set_destination(self, destination):
groute = self._datautil.get_route(from_id, to_id)
self._sub_goals = navgoal.make_goals(self, groute, self._anchor)

# for dashboad
(gpath, _) = navgoal.create_ros_path(groute, self._anchor, self.global_map_name())
msg = nav_msgs.msg.Path()
msg.header = gpath.header
msg.header.frame_id = "map"
for pose in gpath.poses:
msg.poses.append(self.buffer.transform(pose, "map"))
msg.poses[-1].pose.position.z = 0.0
self.path_all_pub.publish(msg)

# navigate from the first path
self._navigate_next_sub_goal()

Expand Down

0 comments on commit 07be9c0

Please sign in to comment.