Skip to content

Commit e19c08a

Browse files
nathan-gtnathan-gt
committed
APPARENTLY CANT CALL NAVIGATOR IN MULTITHREAD :)
1 parent 5cb3cf6 commit e19c08a

File tree

6 files changed

+231
-25
lines changed

6 files changed

+231
-25
lines changed

‎src/rove_navigation/launch/mule_behaviour.launch.py

Lines changed: 33 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,33 @@
1+
import os
2+
3+
from ament_index_python.packages import get_package_share_directory
4+
from launch import LaunchDescription
5+
from launch.actions import IncludeLaunchDescription, GroupAction
6+
from launch.launch_description_sources import PythonLaunchDescriptionSource
7+
from launch.substitutions import PathJoinSubstitution
8+
from launch_ros.substitutions import FindPackageShare
9+
10+
from launch_ros.actions import Node
11+
from launch_ros.actions import SetRemap
12+
13+
def generate_launch_description():
14+
15+
mule_behavior = Node(
16+
package="rove_navigation",
17+
executable='mule_behavior',
18+
name='mule_behavior',
19+
output='screen',
20+
)
21+
22+
person_following = Node(
23+
package="rove_navigation",
24+
executable='person_following',
25+
name='navigate_to_person',
26+
output='screen',
27+
)
28+
29+
return LaunchDescription([
30+
#person_following_node,
31+
mule_behavior,
32+
person_following
33+
])

‎src/rove_navigation/rove_navigation/behavior/mule_constants.py

Lines changed: 13 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,13 @@
1+
# states
2+
START = 'START' # At the beginning, exiting this state sets the startpoint
3+
FOLLOWING = 'FOLLOWING' # make person_following behaviour active
4+
REWIND = 'REWIND' # Initiates the rewind phase
5+
PROCESSED_REWIND = 'PROCESSED_REWIND' # Continues the rewind phase until another TPOSE is detected
6+
PAUSE = 'PAUSE' # Freezes the robot in place (TODO: nice to have, make it still oriente itself towards the target, but not move)
7+
END = 'END'
8+
9+
# poses
10+
FIGURE_IDLE = "IDLE" # Continue state
11+
FIGURE_TPOSE = "TPOSE" # Start the follow
12+
FIGURE_BUCKET = "BUCKET" # Pause/UnPause
13+
FIGURE_UP = "UP" # start the rewind phase

‎src/rove_navigation/rove_navigation/lost_connection.py

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -87,7 +87,7 @@ def check_connection(self):
8787
# gets called on a ros timer (main thread)
8888
def fallback_behavior(self):
8989
if self.state==self.OFFLINE and self.poses_remaining > 0:
90-
self.get_logger().info('nb poses remaingin: '+ str(self.poses_remaining))
90+
self.get_logger().info('nb poses remaining: '+ str(self.poses_remaining))
9191
self.navigate_poses()
9292

9393
# self.get_logger().warn(f'Navigating to fallback at ({self.fallback_path[self.fallbackIndex].__str__()})')

‎src/rove_navigation/rove_navigation/mule_behavior.py

Lines changed: 147 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,147 @@
1+
import time
2+
import socket
3+
from threading import Thread
4+
5+
from sympy import Point
6+
import rclpy
7+
from rclpy.duration import Duration
8+
from rclpy.node import Node
9+
from geometry_msgs.msg import PoseStamped
10+
import numpy as np
11+
from std_msgs.msg import String, Header
12+
from nav_msgs.msg import Odometry, Path
13+
from nav2_msgs.action import NavigateThroughPoses
14+
from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult
15+
import rove_navigation.behavior.mule_constants as consts
16+
17+
# Node to handle when the robot loses connection with internet, stores waypoints to fall back to to try to get back connection
18+
class MuleBehavior(Node):
19+
def __init__(self):
20+
super().__init__('mule_behavior')
21+
self.subscription_person_position = self.create_subscription(
22+
String,
23+
'/tracking/state',
24+
self.state_listener,
25+
10)
26+
self.pub_mule_state = self.create_publisher(
27+
String,
28+
'/mule_state',
29+
10)
30+
self.odom_subscriber = self.create_subscription(
31+
Odometry,
32+
'/odometry/local',
33+
self.odom_listener,
34+
10)
35+
36+
# nav variables
37+
self.startPoint: PoseStamped
38+
self.endpoint: PoseStamped
39+
self.goal_point: PoseStamped
40+
self.currentPos: PoseStamped
41+
self.navigator = BasicNavigator()
42+
43+
44+
self.state = ''
45+
self.change_state(consts.START)
46+
self.navigate_to_goal_point() # returns only when goal reached/cancelled/failed
47+
48+
self.get_logger().info('Initiating MuleBehavior node')
49+
50+
# Thread to execute goal pose posting for rewind
51+
self.connection_thread = Thread(target=self.rewind_behavior)
52+
self.connection_thread.start()
53+
54+
def change_state(self, state):
55+
if(state != self.state):
56+
self.state = state
57+
self.pub_mule_state.publish(String(data=state))
58+
59+
# Subscribed to /odometry/local
60+
def odom_listener(self, msg:Odometry):
61+
self.currentPos = PoseStamped()
62+
self.currentPos.header.frame_id = msg.header.frame_id
63+
self.currentPos.header.stamp = msg.header.stamp
64+
self.currentPos.pose = msg.pose.pose
65+
66+
# Subscribed to /tracking/state, handles state/figure changes
67+
def state_listener(self, msg:String):
68+
if self.state != msg.data:
69+
returnState = msg.data
70+
match msg.data:
71+
case consts.FIGURE_TPOSE:
72+
if self.state == consts.START:
73+
self.startPoint = self.currentPos
74+
self.navigator.setInitialPose(self.startPoint)
75+
self.get_logger().info(f'Mule behavior engaged, point A noted at coordonates {self.startPoint.pose.position}')
76+
returnState = consts.FOLLOWING
77+
78+
case consts.FIGURE_IDLE:
79+
if self.state != consts.REWIND and self.state != consts.PROCESSED_REWIND:
80+
returnState=self.state
81+
82+
case consts.FIGURE_UP:
83+
if self.state != consts.REWIND and self.state != consts.PROCESSED_REWIND:
84+
if self.state == consts.PAUSE: returnState = consts.FOLLOWING
85+
else: returnState = consts.PAUSE
86+
87+
case consts.FIGURE_BUCKET:
88+
if self.state != consts.REWIND and self.state != consts.PROCESSED_REWIND:
89+
self.endpoint = self.currentPos
90+
returnState = consts.REWIND
91+
92+
case _:
93+
self.get_logger().error(f'Unimplemented pose returned: {msg.data}')
94+
95+
self.change_state(returnState)
96+
97+
# runs on a thread continuously
98+
def rewind_behavior(self):
99+
while self.state != consts.END:
100+
if self.state == consts.REWIND:
101+
self.get_logger().info(f'Rewind phase engaged, point B noted at coordonates {self.endpoint.pose.position}')
102+
self.change_state(consts.PROCESSED_REWIND)
103+
self.goal_point = self.startPoint
104+
while self.state == consts.PROCESSED_REWIND:
105+
self.navigate_to_goal_point() # returns only when goal reached/cancelled/failed
106+
result = self.navigator.getResult()
107+
goal = 'A'
108+
nextgoal = 'B'
109+
if self.goal_point == self.startPoint:
110+
goal = 'B'
111+
nextgoal = 'A'
112+
if result == TaskResult.SUCCEEDED:
113+
self.get_logger().info(f'Goal {goal} reached! Initiating nav towards point {nextgoal}')
114+
self.goal_point = self.startPoint if self.goal_point == self.endpoint else self.endpoint
115+
elif result == TaskResult.FAILED:
116+
self.get_logger().error(f'Failed to reach {goal}! Initiating nav towards point {nextgoal}')
117+
self.goal_point = self.startPoint if self.goal_point == self.endpoint else self.endpoint
118+
elif result == TaskResult.CANCELED:
119+
self.get_logger().error(f'Rewinding to point {goal} was cancelled! Initiating nav towards point {nextgoal}')
120+
else :
121+
self.get_logger().warn(f'nav2 navigator task returned with unknown TaskResult')
122+
123+
# Tells navigator to go to current goal Point and processes feedback
124+
def navigate_to_goal_point(self):
125+
i = 0
126+
self.navigator.goToPose(self.goal_point)
127+
while not self.navigator.isTaskComplete():
128+
if self.state != consts.PROCESSED_REWIND:
129+
self.navigator.cancelTask()
130+
131+
feedback:NavigateThroughPoses.Feedback = self.navigator.getFeedback()
132+
self.poses_remaining = feedback.number_of_poses_remaining
133+
if feedback and i % 5 == 0:
134+
# navigation timeout
135+
if Duration.from_msg(feedback.navigation_time) > 1.5 * Duration(seconds=feedback.estimated_time_remaining):
136+
self.navigator.cancelTask()
137+
i+=1
138+
139+
def main(args=None):
140+
rclpy.init(args=args)
141+
node = MuleBehavior()
142+
rclpy.spin(node)
143+
node.destroy_node()
144+
rclpy.shutdown()
145+
146+
if __name__ == '__main__':
147+
main()

‎src/rove_navigation/rove_navigation/person_following.py

Lines changed: 35 additions & 24 deletions
Original file line numberDiff line numberDiff line change
@@ -3,7 +3,9 @@
33
from geometry_msgs.msg import PointStamped, PoseStamped
44
from tf2_ros import Buffer, TransformListener
55
from tf2_geometry_msgs import PointStamped
6+
from std_msgs.msg import String
67
import math
8+
import rove_navigation.behavior.mule_constants as consts
79

810
class NavigateToPersonNode(Node):
911
def __init__(self, truncate_distance):
@@ -18,37 +20,46 @@ def __init__(self, truncate_distance):
1820
PoseStamped,
1921
'/goal_pose',
2022
10)
21-
23+
self.pub_mule_state = self.create_subscription(
24+
String,
25+
'/mule_state',
26+
self.mule_state_listener,
27+
10)
2228
self.tf_buffer = Buffer()
2329
self.tf_listener = TransformListener(self.tf_buffer, self)
30+
self.curr_phase = consts.START
2431

2532
# Distance to truncate from the target position
2633
self.truncate_distance = truncate_distance
34+
35+
def mule_state_listener(self, msg: String):
36+
self.curr_phase = msg
2737

2838
def navigate_to_person(self, msg):
29-
# Ensure that the transformation is available
30-
now = rclpy.time.Time()
31-
if self.tf_buffer.can_transform('map', msg.header.frame_id, now):
32-
point_transformed = self.tf_buffer.transform(msg, 'map', timeout=rclpy.duration.Duration(seconds=1))
33-
goal_pose = PoseStamped()
34-
goal_pose.header.frame_id = "map"
35-
goal_pose.header.stamp = self.get_clock().now().to_msg()
36-
37-
# Calculate the angle for the truncated goal using atan2
38-
angle = math.atan2(point_transformed.point.y, point_transformed.point.x)
39-
40-
# Calculate the position truncating the specified distance from the transformed position
41-
goal_pose.pose.position.x = point_transformed.point.x - self.truncate_distance * math.cos(angle)
42-
goal_pose.pose.position.y = point_transformed.point.y - self.truncate_distance * math.sin(angle)
43-
goal_pose.pose.position.z = 0.0 # Normally zero for ground robots
44-
45-
goal_pose.pose.orientation.w = 1.0 # No rotation about the z-axis
46-
47-
# Log the navigation target for debugging
48-
self.get_logger().info(f'Navigating to truncated goal: {goal_pose.pose.position.x}, {goal_pose.pose.position.y}')
49-
50-
# Publish the goal
51-
self.goal_update_pub.publish(goal_pose)
39+
if self.curr_phase == consts.FOLLOWING:
40+
# Ensure that the transformation is available
41+
now = rclpy.time.Time()
42+
if self.tf_buffer.can_transform('map', msg.header.frame_id, now):
43+
point_transformed = self.tf_buffer.transform(msg, 'map', timeout=rclpy.duration.Duration(seconds=1))
44+
goal_pose = PoseStamped()
45+
goal_pose.header.frame_id = "map"
46+
goal_pose.header.stamp = self.get_clock().now().to_msg()
47+
48+
# Calculate the angle for the truncated goal using atan2
49+
angle = math.atan2(point_transformed.point.y, point_transformed.point.x)
50+
51+
# Calculate the position truncating the specified distance from the transformed position
52+
goal_pose.pose.position.x = point_transformed.point.x - self.truncate_distance * math.cos(angle)
53+
goal_pose.pose.position.y = point_transformed.point.y - self.truncate_distance * math.sin(angle)
54+
goal_pose.pose.position.z = 0.0 # Normally zero for ground robots
55+
56+
goal_pose.pose.orientation.w = 1.0 # No rotation about the z-axis
57+
58+
# Log the navigation target for debugging
59+
self.get_logger().info(f'Navigating to truncated goal: {goal_pose.pose.position.x}, {goal_pose.pose.position.y}')
60+
61+
# Publish the goal
62+
self.goal_update_pub.publish(goal_pose)
5263

5364
def main(args=None):
5465
rclpy.init(args=args)

‎src/rove_navigation/setup.py

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -22,6 +22,7 @@
2222
description='TODO: Package description',
2323
license='TODO: License declaration',
2424
tests_require=['pytest'],
25+
package_dir={'': 'src'},
2526
entry_points={
2627
'console_scripts': [
2728
'navigate_to_person = rove_navigation.navigate_to_person:main',
@@ -31,6 +32,7 @@
3132
'frontier_publisher = rove_navigation.frontier_publisher:main',
3233
'exploration = rove_navigation.exploration:main',
3334
'lost_connection = rove_navigation.lost_connection:main',
35+
'mule_behavior = rove_navigation.mule_behavior:main',
3436
],
3537
},
3638
)

0 commit comments

Comments
 (0)