Skip to content

Commit

Permalink
Rethinking following (#234)
Browse files Browse the repository at this point in the history
Co-authored-by: fireblonde <[email protected]>
  • Loading branch information
jws-1 and fireblonde authored Jul 6, 2024
1 parent d59478f commit 87efc93
Show file tree
Hide file tree
Showing 13 changed files with 1,644,711 additions and 184 deletions.
1,644,285 changes: 1,644,285 additions & 0 deletions common/navigation/lasr_person_following/config/trained_leg_detector_res_TIAGo.yaml

Large diffs are not rendered by default.

Original file line number Diff line number Diff line change
@@ -1,23 +1,24 @@
<launch>

<!-- params -->
<param name="forest_file" value="$(find leg_tracker)/config/trained_leg_detector_res=0.33.yaml" />
<param name="forest_file" value="$(find lasr_person_following)/config/trained_leg_detector_res_TIAGo.yaml" />
<param name="scan_topic" value="/scan" />
<param name="fixed_frame" value="odom"/>
<param name="scan_frequency" value="10"/>
<param name="max_detect_distance" value="5.0"/>
<param name="dist_travelled_together_to_initiate_leg_pair" value="0.5"/>
<param name="scan_frequency" value="15"/>
<param name="max_detect_distance" value="10.0"/>
<param name="dist_travelled_together_to_initiate_leg_pair" value="0.1"/>
<param name="display_detected_people" value="true"/>
<param name="max_leg_pairing_dist" value="0.5"/>
<param name="max_std" value="0.9"/>
<param name="min_points_per_cluster" value="5"/>
<!-- run detect_leg_clusters -->
<node pkg="leg_tracker" type="detect_leg_clusters" name="detect_leg_clusters" output="screen"/>

<!-- run joint_leg_tracker -->
<node pkg="leg_tracker" type="joint_leg_tracker.py" name="joint_leg_tracker" output="screen"/>

<!-- run local_occupancy_grid_mapping -->
<node pkg="leg_tracker" type="local_occupancy_grid_mapping" name="local_occupancy_grid_mapping" output="screen"/>
<node pkg="leg_tracker" type="local_occupancy_grid_mapping" name="local_occupancy_grid_mapping" output="screen"/>

</launch>

Original file line number Diff line number Diff line change
Expand Up @@ -5,5 +5,6 @@

<node pkg="lasr_person_following" type="person_following.py" name="person_following" output="screen" />

<node pkg="tiago_2dnav" type="navigation_camera_mgr.py" name="navigation_camera_mgr" output="screen"/>
</launch>

Original file line number Diff line number Diff line change
Expand Up @@ -9,17 +9,32 @@
FollowFeedback,
)

from dynamic_reconfigure.srv import Reconfigure
from dynamic_reconfigure.msg import Config, DoubleParameter, BoolParameter, IntParameter

from lasr_person_following import PersonFollower

import warnings


class PersonFollowingServer:

_server: actionlib.SimpleActionServer
_follower: PersonFollower

def __init__(self) -> None:
self._dynamic_costmap = rospy.ServiceProxy(
"/move_base/local_costmap/set_parameters", Reconfigure
)
self._dynamic_velocity = rospy.ServiceProxy(
"/move_base/PalLocalPlanner/set_parameters", Reconfigure
)
self._dynamic_recovery = rospy.ServiceProxy(
"/move_base/set_parameters", Reconfigure
)

self._update_params()
rospy.sleep(1)
print("Updated params")

self._server = actionlib.SimpleActionServer(
"follow_person", FollowAction, execute_cb=self._execute_cb, auto_start=False
Expand All @@ -31,6 +46,7 @@ def __init__(self) -> None:
self._server.start()

def _execute_cb(self, _: FollowGoal) -> None:
print("Executing follow_person action")
while not self._follower.begin_tracking(ask=False):
rospy.logwarn("No people found, retrying...")
rospy.sleep(rospy.Duration(1.0))
Expand All @@ -45,6 +61,23 @@ def _execute_cb(self, _: FollowGoal) -> None:
def _preempt_cb(self) -> None:
raise NotImplementedError("Preempt not implemented yet")

def _update_params(self):
config = Config()
config.ints.append(IntParameter(name="width", value=4))
config.ints.append(IntParameter(name="height", value=4))
self._dynamic_costmap(config)

config = Config()
config.doubles.append(DoubleParameter(name="max_vel_x", value=0.6))

self._dynamic_velocity(config)

config = Config()
config.bools.append(BoolParameter(name="recovery_behavior_enabled", value=1))
config.bools.append(BoolParameter(name="clearing_rotation_allowed", value=0))

self._dynamic_recovery(config)


if __name__ == "__main__":
rospy.init_node("person_following_server")
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -13,17 +13,20 @@

from typing import Union, List

import actionlib
from move_base_msgs.msg import MoveBaseAction, MoveBaseActionGoal, MoveBaseGoal
from actionlib_msgs.msg import GoalStatus
from sensor_msgs.msg import PointCloud2

import rosservice
import tf2_ros as tf
import tf2_geometry_msgs # type: ignore
from tf2_geometry_msgs.tf2_geometry_msgs import do_transform_pose
from nav_msgs.srv import GetPlan
from nav_msgs.msg import Path
from lasr_vision_msgs.srv import DetectWave
from play_motion_msgs.msg import PlayMotionAction
from play_motion_msgs.msg import PlayMotionGoal

from lasr_vision_msgs.srv import DetectWaveRequest

from math import atan2
import numpy as np
Expand All @@ -38,7 +41,6 @@


class PersonFollower:

# Parameters
_start_following_radius: float
_start_following_angle: float
Expand Down Expand Up @@ -71,17 +73,19 @@ def __init__(
self,
start_following_radius: float = 2.0,
start_following_angle: float = 45.0,
n_secs_static_finished: float = 10.0,
n_secs_static_plan_close: float = 5.0,
new_goal_threshold: float = 0.5,
n_secs_static_finished: float = 8.0,
n_secs_static_plan_close: float = 10.0,
new_goal_threshold: float = 2.0,
stopping_distance: float = 1.0,
static_speed: float = 0.0015,
):
self._start_following_radius = start_following_radius
self._start_following_angle = start_following_angle
self._n_secs_static_finished = n_secs_static_finished
self._n_secs_static_plan_close = n_secs_static_plan_close
self._new_goal_threshold = new_goal_threshold
self._stopping_distance = stopping_distance
self._static_speed = static_speed

self._track_id = None

Expand Down Expand Up @@ -120,6 +124,16 @@ def __init__(
if not self._transcribe_speech_client_available:
rospy.logwarn("Transcribe speech client not available")

self._detect_wave = rospy.ServiceProxy("/detect_wave", DetectWave)
if not self._detect_wave.wait_for_service(rospy.Duration.from_sec(10.0)):
rospy.logwarn("Detect wave service not available")

self._play_motion = actionlib.SimpleActionClient(
"play_motion", PlayMotionAction
)
if not self._play_motion.wait_for_server(rospy.Duration.from_sec(10.0)):
rospy.logwarn("Play motion client not available")

def _tf_pose(self, pose: PoseStamped, target_frame: str):
trans = self._buffer.lookup_transform(
target_frame, pose.header.frame_id, rospy.Time(0), rospy.Duration(1.0)
Expand All @@ -129,7 +143,7 @@ def _tf_pose(self, pose: PoseStamped, target_frame: str):
def _robot_pose_in_odom(self) -> Union[PoseStamped, None]:
try:
current_pose: PoseWithCovarianceStamped = rospy.wait_for_message(
"/amcl_pose", PoseWithCovarianceStamped
"/robot_pose", PoseWithCovarianceStamped
)
except AttributeError:
return None
Expand Down Expand Up @@ -203,8 +217,11 @@ def begin_tracking(self, ask: bool = False) -> bool:
rospy.loginfo(f"Tracking person with ID {self._track_id}")
return True

def _recover_track(self) -> bool:
if self._tts_client_available:
def _recover_track(self, say) -> bool:
if not say:
self._tts("I SAW A person waving", wait=True)

if self._tts_client_available and say:
self._tts("I lost track of you, please come back", wait=True)

while not self.begin_tracking(ask=True) and not rospy.is_shutdown():
Expand All @@ -213,6 +230,58 @@ def _recover_track(self) -> bool:

return True

# recover with vision, look up and check if person is waving
def _recover_vision(self, prev_pose: PoseStamped) -> bool:
# look up with playmotion and detect wave service
# if detected, begin tracking

# use play motion to look up
self._cancel_goal()

goal = PlayMotionGoal()
goal.motion_name = "look_centre"
self._play_motion.send_goal_and_wait(goal)

self._tts("Can you wave at me so that i can try to find you easily", wait=True)

# detect wave
try:
pcl = rospy.wait_for_message("/xtion/depth_registered/points", PointCloud2)
req = DetectWaveRequest()
req.pcl_msg = pcl
req.dataset = "resnet50"
req.confidence = 0.1
response = self._detect_wave(req)
if response.wave_detected:
rospy.loginfo("Wave detected, beginning tracking")
if np.isnan(response.wave_position.point.x) or np.isnan(
response.wave_position.point.y
):
return False
goal_pose = self._tf_pose(
PoseStamped(
pose=Pose(
position=Point(
x=response.wave_position.point.x,
y=response.wave_position.point.y,
z=response.wave_position.point.z,
),
orientation=Quaternion(0, 0, 0, 1),
),
header=pcl.header,
),
"map",
)
rospy.loginfo(goal_pose.pose.position)
goal_pose.pose.orientation = self._compute_face_quat(
prev_pose.pose, goal_pose.pose
)
self._move_base(goal_pose)
return True
except rospy.ServiceException as e:
rospy.loginfo(f"Error detecting wave: {e}")
return False

def _euclidian_distance(self, p1: Pose, p2: Pose) -> float:
return np.linalg.norm(
np.array([p1.position.x, p1.position.y])
Expand Down Expand Up @@ -314,8 +383,10 @@ def follow(self) -> None:
person_trajectory: PoseArray = PoseArray()
person_trajectory.header.frame_id = "odom"
prev_track: Union[None, Person] = None
prev_goal: Union[None, PoseStamped] = None
last_goal_time: Union[None, rospy.Time] = None
going_to_person: bool = False
track_vels: [float] = []

while not rospy.is_shutdown():

Expand All @@ -326,15 +397,38 @@ def follow(self) -> None:
filter(lambda track: track.id == self._track_id, tracks.people),
None,
)
# keep a sliding window of the tracks velocity
if track is not None:
track_vels.append((track.vel_x, track.vel_y))
if len(track_vels) > 10:
track_vels.pop(0)

if track is None:
rospy.loginfo("Lost track of person, recovering...")
self._cancel_goal()
person_trajectory = PoseArray()
self._recover_track()
recovery = self._recover_vision(prev_goal)
self._recover_track(say=not recovery)
prev_track = None
continue

if prev_track is None:
robot_pose: PoseStamped = self._robot_pose_in_odom()

goal_pose = self._tf_pose(
PoseStamped(
pose=Pose(
position=track.pose.position,
orientation=robot_pose.pose.orientation,
),
header=tracks.header,
),
"map",
)
self._move_base(goal_pose)
prev_goal = goal_pose
last_goal_time = rospy.Time.now()
prev_track = track

# Distance to the previous pose
dist_to_prev = (
self._euclidian_distance(track.pose, prev_track.pose)
Expand All @@ -350,42 +444,28 @@ def follow(self) -> None:
"map",
)
self._move_base(goal_pose)
prev_goal = goal_pose
prev_track = track
last_goal_time = rospy.Time.now()
person_trajectory.poses.append(track.pose)
person_trajectory.header.stamp = rospy.Time.now()
self._person_trajectory_pub.publish(person_trajectory)
going_to_person = False
elif last_goal_time is not None:
delta_t: float = (rospy.Time.now() - last_goal_time).to_sec()
if (
self._n_secs_static_plan_close
<= delta_t
< self._n_secs_static_finished
) and not going_to_person:
self._cancel_goal()
goal_pose = self._get_pose_on_path(
self._tf_pose(self._robot_pose_in_odom(), "map"),
self._tf_pose(
PoseStamped(pose=track.pose, header=tracks.header),
"map",
),
self._stopping_distance,
)
if goal_pose is not None:
self._move_base(goal_pose)
going_to_person = True
else:
rospy.logwarn("Could not find a path to the person")
elif delta_t >= self._n_secs_static_finished:
rospy.loginfo(
"Person has been static for too long, checking if we are finished"
)
self._move_base_client.wait_for_result()
if self._check_finished():
rospy.loginfo("Finished following person")
break
else:
rospy.loginfo("Person is not finished, continuing")
last_goal_time = None
continue
elif (
self._move_base_client.get_state() in [GoalStatus.ABORTED]
and prev_goal is not None
):
rospy.logwarn("Goal was aborted, retrying")
rospy.logwarn((rospy.Time.now() - last_goal_time).to_sec())
rospy.logwarn(track.pose == prev_track.pose)
rospy.logwarn("")
self._move_base(prev_goal)
elif (
np.mean([np.linalg.norm(vel) for vel in track_vels])
< self._static_speed
):

rospy.logwarn("Person has been static for too long, stopping")
self._cancel_goal()
if self._check_finished():
rospy.loginfo("Finished following person")
break
rospy.loginfo("")
rospy.loginfo(np.mean([np.linalg.norm(vel) for vel in track_vels]))
rospy.loginfo("")
13 changes: 13 additions & 0 deletions common/vision/lasr_vision_bodypix/launch/gesture_service.launch
Original file line number Diff line number Diff line change
@@ -0,0 +1,13 @@
<launch>
<description>Start the BodyPix service</description>
<usage doc="BodyPix service"></usage>
<usage doc="Preload models and enable debug topic">debug:=true preload:=['resnet50', 'mobilenet50']</usage>

<arg name="debug" default="true" doc="Whether to publish plotted images to /bodypix/debug/model_name" />
<arg name="preload" default="[]" doc="Array of models to preload when starting the service" />

<node name="bodypix_gesture_service" pkg="lasr_vision_bodypix" type="gesture_service.py" output="screen">
<param name="debug" type="bool" value="$(arg debug)" />
<param name="preload" type="yaml" value="$(arg preload)" />
</node>
</launch>
Loading

0 comments on commit 87efc93

Please sign in to comment.