From 4c8429496ca192c969eb64efb4540c176e193852 Mon Sep 17 00:00:00 2001 From: JeiKeiLim Date: Wed, 28 Sep 2022 10:54:10 +0900 Subject: [PATCH 01/48] Add waypoint flight(WIP). --- run_docker.sh | 35 ++++---------- src/rc_demo/launch/rc_flight.launch | 9 ++++ src/rc_demo/rc_flight.py | 27 +++++++++-- src/rc_demo/res/waypoints.yaml | 16 ++++++ src/rc_demo/utils.py | 75 +++++++++++++++++++++++++++++ 5 files changed, 133 insertions(+), 29 deletions(-) create mode 100644 src/rc_demo/launch/rc_flight.launch create mode 100644 src/rc_demo/res/waypoints.yaml diff --git a/run_docker.sh b/run_docker.sh index 80b55885..ce401800 100755 --- a/run_docker.sh +++ b/run_docker.sh @@ -57,31 +57,16 @@ elif [ "$1" = "run" ]; then CMD_ARGS="$CMD_ARGS --gpus all" fi - # TODO(ulken94): The LIO-Livox should not be mounted for aarch64. - if [ "$ARCH" = "aarch64" ]; then - docker run $DOCKER_OPT --privileged \ - -e DISPLAY=${DISPLAY} \ - -e TERM=xterm-256color \ - -v /tmp/.X11-unix:/tmp/.X11-unix:ro \ - -v /dev:/dev \ - -v $PWD/src:/home/user/catkin_ws/src \ - --network host \ - $CMD_ARGS \ - $DOCKER_TAG \ - $RUN_SHELL - else - docker run $DOCKER_OPT --privileged \ - -e DISPLAY=${DISPLAY} \ - -e TERM=xterm-256color \ - -v /tmp/.X11-unix:/tmp/.X11-unix:ro \ - -v /dev:/dev \ - -v $PWD/catkin_ws/src:/home/user/catkin_ws/src \ - -v $PWD/res:/home/user/res \ - --network host \ - $CMD_ARGS \ - $DOCKER_TAG \ - $RUN_SHELL - fi + docker run $DOCKER_OPT --privileged \ + -e DISPLAY=${DISPLAY} \ + -e TERM=xterm-256color \ + -v /tmp/.X11-unix:/tmp/.X11-unix:ro \ + -v /dev:/dev \ + -v $PWD/src:/home/user/catkin_ws/src \ + --network host \ + $CMD_ARGS \ + $DOCKER_TAG \ + $RUN_SHELL last_cont_id=$(docker ps -qn 1) echo $(docker ps -qn 1) > $PWD/.last_exec_cont_id.txt diff --git a/src/rc_demo/launch/rc_flight.launch b/src/rc_demo/launch/rc_flight.launch new file mode 100644 index 00000000..54166be6 --- /dev/null +++ b/src/rc_demo/launch/rc_flight.launch @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/src/rc_demo/rc_flight.py b/src/rc_demo/rc_flight.py index 2d7f135a..e6d13681 100755 --- a/src/rc_demo/rc_flight.py +++ b/src/rc_demo/rc_flight.py @@ -2,17 +2,21 @@ import sys import rospy +import yaml +import time from mavros_msgs.msg import RCIn -from utils import ControlMessage +from utils import ControlMessage, WaypointMessage class RCHandler: """Receieve RC stick data.""" - def __init__(self) -> None: + def __init__(self, use_rc_control: bool = True, waypoint_path: str = "") -> None: + self.use_rc_control = use_rc_control self.rc_sub = rospy.Subscriber("/mavros/rc/in", RCIn, self.callback_rc_in) self.control_msg = ControlMessage() + self.waypoint_msg = WaypointMessage() self.is_calibrated = False self.calib_count = 0 @@ -26,6 +30,19 @@ def __init__(self) -> None: self.cmd_magnitude = 1.0 + if waypoint_path != "": + with open(waypoint_path, 'r') as f: + self.waypoints = yaml.load(f, yaml.SafeLoader) + else: + self.waypoints = { + 'waypoint_0': [[0.0, 0.0, 1.2]], + 'waypoint_1': [[0.0, 0.0, 1.2]], + } + + time.sleep(3) + for waypoint in self.waypoints['waypoint_0']: + self.waypoint_msg.add_waypoint(*waypoint) + def callback_rc_in(self, msg: RCIn) -> None: """Receieves /mavros/rc/in @@ -64,7 +81,7 @@ def callback_rc_in(self, msg: RCIn) -> None: else: self.calib_count = 0 - if not self.is_calibrated: + if not self.is_calibrated or not self.use_rc_control: return self.control_msg.send_control(-self.roll * self.cmd_magnitude, @@ -76,8 +93,10 @@ def callback_rc_in(self, msg: RCIn) -> None: if __name__ == "__main__": args = rospy.myargv(argv=sys.argv) rospy.init_node("flight_control_node", anonymous=True) + use_rc_control = rospy.get_param('~rc_control', False) + waypoint_path = rospy.get_param('~waypoint', "") - rc_handler = RCHandler() + rc_handler = RCHandler(use_rc_control, waypoint_path) rospy.spin() diff --git a/src/rc_demo/res/waypoints.yaml b/src/rc_demo/res/waypoints.yaml new file mode 100644 index 00000000..f9c2415a --- /dev/null +++ b/src/rc_demo/res/waypoints.yaml @@ -0,0 +1,16 @@ +waypoint_0: + [ + [10.0, 1.5, 2.0], + [14, 0.8, 3.2], + [18, 0.8, 3.5], + [19, -1.1, 2.0], + [23, -1.1, 2.0], + [25, -0.5, 2.5], + [28, 0.0, 3.2], + [33, 0.0, 1.2], + ] + +waypoint_1: + [ + [0.0, 0.0, 0.0], + ] diff --git a/src/rc_demo/utils.py b/src/rc_demo/utils.py index 1b12745b..1902f3f6 100644 --- a/src/rc_demo/utils.py +++ b/src/rc_demo/utils.py @@ -1,8 +1,11 @@ import rospy +import math from std_msgs.msg import String from mavros_msgs.srv import SetMode from mavros_msgs.srv import CommandBool +from nav_msgs.msg import Path, Odometry +from geometry_msgs.msg import PoseStamped class ControlMessage: @@ -25,6 +28,78 @@ def send_control(self, roll: float, pitch: float, yaw: float, throttle: float) - self.control_pub.publish(msg) +class WaypointMessage: + """Send waypoint message via MavROS""" + + def __init__(self) -> None: + self.waypoint_pub = rospy.Publisher('/waypoint_generator/waypoints', + Path, + queue_size=1) + self.odom_sub = rospy.Subscriber('/Odometry', + Odometry, + self.callback_odometry) + self._timer_send_waypoint = rospy.Timer(rospy.Duration(0.1), self._loop_send_waypoint) + self.distance_tolerance = 0.5 + + self.current_position = Odometry() + self.target_position = PoseStamped() + self.is_moving = False + self.waypoints = [] + self.send_seq = 0 + + def add_waypoint(self, x: float, y: float, z: float) -> None: + self.waypoints.append((x, y, z)) + + def _check_current_location(self) -> bool: + if len(self.waypoints) < 1: + return False + + target_point = self.target_position.pose.position + current_point = self.current_position.pose.pose.position + + distance = math.sqrt(math.pow(target_point.x - current_point.x, 2) + math.pow(target_point.y - current_point.y, 2) + math.pow(target_point.z - current_point.z, 2)) + + if distance < self.distance_tolerance: + return True + + return False + + def _loop_send_waypoint(self, event = None) -> None: + if self.is_moving and not self._check_current_location(): + return + else: + self.is_moving = False + + if len(self.waypoints) < 1: + return + + self.send_seq += 1 + + target_point = self.waypoints.pop(0) + + msg = Path() + msg.header.seq = self.send_seq + msg.header.stamp = rospy.Time.now() + msg.header.frame_id = "map" + + pose = PoseStamped() + pose.header.seq = self.send_seq + pose.header.stamp = rospy.Time.now() + pose.header.frame_id = "map" + pose.pose.position.x = target_point[0] + pose.pose.position.y = target_point[1] + pose.pose.position.z = target_point[2] + pose.pose.orientation.w = 1.0 + msg.poses.append(pose) + self.target_position = pose + + self.waypoint_pub.publish(msg) + self.is_moving = True + + def callback_odometry(self, msg: Odometry) -> None: + self.current_position = msg + + class MAVROSCommander: def __init__(self): self.set_mode_client = rospy.ServiceProxy('mavros/set_mode', SetMode) From 0457fd0fb00646d7a7e4982e6ba1d1659ef3f65f Mon Sep 17 00:00:00 2001 From: JeiKeiLim Date: Wed, 28 Sep 2022 10:55:15 +0900 Subject: [PATCH 02/48] Add comments for running environment. --- src/rc_demo/res/waypoints.yaml | 1 + 1 file changed, 1 insertion(+) diff --git a/src/rc_demo/res/waypoints.yaml b/src/rc_demo/res/waypoints.yaml index f9c2415a..d2dd0442 100644 --- a/src/rc_demo/res/waypoints.yaml +++ b/src/rc_demo/res/waypoints.yaml @@ -1,4 +1,5 @@ waypoint_0: + # This way point has been set from roslaunch simulation_environment mavros_posix_sitl_lidar.launch world:=$HOME/custom_worlds/add_competition_outfield.world x:=4 y:=0 z:=1 vehicle:=px4vision lidar:=ouster [ [10.0, 1.5, 2.0], [14, 0.8, 3.2], From bccb798e9827f7482cc6f0e4c9b99a7104e70708 Mon Sep 17 00:00:00 2001 From: Jongkuk Lim Date: Wed, 28 Sep 2022 06:09:51 +0000 Subject: [PATCH 03/48] Add RC switch for waypoint flight. --- src/rc_demo/launch/rc_flight.launch | 2 ++ src/rc_demo/rc_flight.py | 44 +++++++++++++++++++++++++---- src/rc_demo/utils.py | 11 ++++++++ 3 files changed, 52 insertions(+), 5 deletions(-) diff --git a/src/rc_demo/launch/rc_flight.launch b/src/rc_demo/launch/rc_flight.launch index 54166be6..eea713dd 100644 --- a/src/rc_demo/launch/rc_flight.launch +++ b/src/rc_demo/launch/rc_flight.launch @@ -1,9 +1,11 @@ + + diff --git a/src/rc_demo/rc_flight.py b/src/rc_demo/rc_flight.py index e6d13681..ebf3e764 100755 --- a/src/rc_demo/rc_flight.py +++ b/src/rc_demo/rc_flight.py @@ -4,20 +4,30 @@ import rospy import yaml import time +import threading from mavros_msgs.msg import RCIn from utils import ControlMessage, WaypointMessage +waypoint_thread_lock = threading.Lock() + class RCHandler: """Receieve RC stick data.""" - def __init__(self, use_rc_control: bool = True, waypoint_path: str = "") -> None: + def __init__(self, + use_rc_control: bool = True, + waypoint_path: str = "", + target_waypoint: str = "waypoint_0") -> None: self.use_rc_control = use_rc_control + self.target_waypoint = target_waypoint + self.rc_sub = rospy.Subscriber("/mavros/rc/in", RCIn, self.callback_rc_in) self.control_msg = ControlMessage() self.waypoint_msg = WaypointMessage() + self.is_waypoint_working = False + self.is_calibrated = False self.calib_count = 0 @@ -39,16 +49,38 @@ def __init__(self, use_rc_control: bool = True, waypoint_path: str = "") -> None 'waypoint_1': [[0.0, 0.0, 1.2]], } - time.sleep(3) - for waypoint in self.waypoints['waypoint_0']: - self.waypoint_msg.add_waypoint(*waypoint) + def _start_waypoint(self) -> None: + if self.is_waypoint_working: + return + + waypoint_thread_lock.acquire() + + self.is_waypoint_working = True + self.waypoint_msg.clear_waypoint() + self.waypoint_msg.add_waypoints(self.waypoints[self.target_waypoint]) + waypoint_thread_lock.release() + + def _terminate_waypoint(self) -> None: + waypoint_thread_lock.acquire() + + self.is_waypoint_working = False + self.waypoint_msg.clear_waypoint() + + waypoint_thread_lock.release() def callback_rc_in(self, msg: RCIn) -> None: """Receieves /mavros/rc/in Receieved stick message will be sent """ + + # Check waypoint switch + if 1300 < msg.channels[6] < 1700: + self._start_waypoint() + elif 1000 < msg.channels[6] < 1300: + self._terminate_waypoint() + for i in range(4): self.val_range[0] = min(self.val_range[0], msg.channels[i]) self.val_range[1] = max(self.val_range[1], msg.channels[i]) @@ -93,10 +125,12 @@ def callback_rc_in(self, msg: RCIn) -> None: if __name__ == "__main__": args = rospy.myargv(argv=sys.argv) rospy.init_node("flight_control_node", anonymous=True) + use_rc_control = rospy.get_param('~rc_control', False) waypoint_path = rospy.get_param('~waypoint', "") + target_waypoint = rospy.get_param('~target_waypoint', "waypoint_0") - rc_handler = RCHandler(use_rc_control, waypoint_path) + rc_handler = RCHandler(use_rc_control, waypoint_path, target_waypoint) rospy.spin() diff --git a/src/rc_demo/utils.py b/src/rc_demo/utils.py index 1902f3f6..446a9a88 100644 --- a/src/rc_demo/utils.py +++ b/src/rc_demo/utils.py @@ -1,6 +1,8 @@ import rospy import math +from typing import Tuple, List + from std_msgs.msg import String from mavros_msgs.srv import SetMode from mavros_msgs.srv import CommandBool @@ -47,9 +49,18 @@ def __init__(self) -> None: self.waypoints = [] self.send_seq = 0 + def clear_waypoint(self) -> None: + self.waypoints.clear() + self.target_position.pose = self.current_position.pose.pose + self.is_moving = False + def add_waypoint(self, x: float, y: float, z: float) -> None: self.waypoints.append((x, y, z)) + def add_waypoints(self, points: List[Tuple[float, float, float]]) -> None: + for point in points: + self.add_waypoint(*point) + def _check_current_location(self) -> bool: if len(self.waypoints) < 1: return False From 456407d890c999bcf37cbb5387e2471102eda6e9 Mon Sep 17 00:00:00 2001 From: JeiKeiLim Date: Wed, 28 Sep 2022 15:56:11 +0900 Subject: [PATCH 04/48] Add waypoiint trigger test topic sub --- src/rc_demo/rc_flight.py | 14 ++++++++++++++ src/rc_demo/utils.py | 37 +++++++++++++++++++++++++------------ 2 files changed, 39 insertions(+), 12 deletions(-) diff --git a/src/rc_demo/rc_flight.py b/src/rc_demo/rc_flight.py index ebf3e764..9bc40577 100755 --- a/src/rc_demo/rc_flight.py +++ b/src/rc_demo/rc_flight.py @@ -6,6 +6,7 @@ import time import threading +from std_msgs.msg import String from mavros_msgs.msg import RCIn from utils import ControlMessage, WaypointMessage @@ -23,6 +24,7 @@ def __init__(self, self.target_waypoint = target_waypoint self.rc_sub = rospy.Subscriber("/mavros/rc/in", RCIn, self.callback_rc_in) + self.waypoint_trigger_sub = rospy.Subscriber("/custom/waypoint_trigger", String, self.callback_waypoint_trigger) self.control_msg = ControlMessage() self.waypoint_msg = WaypointMessage() @@ -69,6 +71,18 @@ def _terminate_waypoint(self) -> None: waypoint_thread_lock.release() + def callback_waypoint_trigger(self, msg: String) -> None: + """Test trigger callback for waypoint flight. + + You will need to trigger this by following command. + rostopic pub /custom/waypoint_trigger std_msgs/String "data: 'on'" + rostopic pub /custom/waypoint_trigger std_msgs/String "data: 'off'" + """ + if msg.data == "on": + self._start_waypoint() + elif msg.data == "off": + self._terminate_waypoint() + def callback_rc_in(self, msg: RCIn) -> None: """Receieves /mavros/rc/in diff --git a/src/rc_demo/utils.py b/src/rc_demo/utils.py index 446a9a88..f6a5d607 100644 --- a/src/rc_demo/utils.py +++ b/src/rc_demo/utils.py @@ -40,7 +40,8 @@ def __init__(self) -> None: self.odom_sub = rospy.Subscriber('/Odometry', Odometry, self.callback_odometry) - self._timer_send_waypoint = rospy.Timer(rospy.Duration(0.1), self._loop_send_waypoint) + self._timer_send_waypoint = rospy.Timer(rospy.Duration(3.0), + self._loop_send_waypoint) self.distance_tolerance = 0.5 self.current_position = Odometry() @@ -49,18 +50,22 @@ def __init__(self) -> None: self.waypoints = [] self.send_seq = 0 + def clear_waypoint(self) -> None: self.waypoints.clear() self.target_position.pose = self.current_position.pose.pose self.is_moving = False + def add_waypoint(self, x: float, y: float, z: float) -> None: self.waypoints.append((x, y, z)) + def add_waypoints(self, points: List[Tuple[float, float, float]]) -> None: for point in points: self.add_waypoint(*point) + def _check_current_location(self) -> bool: if len(self.waypoints) < 1: return False @@ -76,31 +81,36 @@ def _check_current_location(self) -> bool: return False def _loop_send_waypoint(self, event = None) -> None: + target_point = None if self.is_moving and not self._check_current_location(): - return + target_point = self.target_position else: self.is_moving = False if len(self.waypoints) < 1: - return + target_point = self.target_position self.send_seq += 1 - target_point = self.waypoints.pop(0) + if target_point == None: + target_point = self.waypoints.pop(0) + + pose = PoseStamped() + pose.header.seq = self.send_seq + pose.header.stamp = rospy.Time.now() + pose.header.frame_id = "map" + pose.pose.position.x = target_point[0] + pose.pose.position.y = target_point[1] + pose.pose.position.z = target_point[2] + pose.pose.orientation.w = 1.0 + else: + pose = target_point msg = Path() msg.header.seq = self.send_seq msg.header.stamp = rospy.Time.now() msg.header.frame_id = "map" - pose = PoseStamped() - pose.header.seq = self.send_seq - pose.header.stamp = rospy.Time.now() - pose.header.frame_id = "map" - pose.pose.position.x = target_point[0] - pose.pose.position.y = target_point[1] - pose.pose.position.z = target_point[2] - pose.pose.orientation.w = 1.0 msg.poses.append(pose) self.target_position = pose @@ -110,6 +120,9 @@ def _loop_send_waypoint(self, event = None) -> None: def callback_odometry(self, msg: Odometry) -> None: self.current_position = msg + if self.target_position.pose.position.x == 0 and self.target_position.pose.position.y == 0 and self.target_position.pose.position.z == 0: + self.target_position.pose = msg.pose.pose + class MAVROSCommander: def __init__(self): From f0f7a8515f52e58feeff18c137560c583dccbf16 Mon Sep 17 00:00:00 2001 From: JeiKeiLim Date: Wed, 28 Sep 2022 16:46:01 +0900 Subject: [PATCH 05/48] Fix run_in_flight.launch for waypoint flight. --- src/planner/plan_manage/launch/run_in_flight.launch | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/src/planner/plan_manage/launch/run_in_flight.launch b/src/planner/plan_manage/launch/run_in_flight.launch index eb343eeb..22bc21ca 100644 --- a/src/planner/plan_manage/launch/run_in_flight.launch +++ b/src/planner/plan_manage/launch/run_in_flight.launch @@ -3,6 +3,8 @@ + + @@ -77,7 +79,7 @@ - + @@ -88,8 +90,11 @@ - - + + + + + @@ -77,7 +79,7 @@ - + @@ -88,8 +90,11 @@ - - + + + + + @@ -94,6 +96,7 @@ + diff --git a/src/rc_demo/launch/rc_flight.launch b/src/rc_demo/launch/rc_flight.launch index eea713dd..2842ac5b 100644 --- a/src/rc_demo/launch/rc_flight.launch +++ b/src/rc_demo/launch/rc_flight.launch @@ -2,10 +2,12 @@ + + diff --git a/src/rc_demo/rc_flight.py b/src/rc_demo/rc_flight.py index 9bc40577..e464eaff 100755 --- a/src/rc_demo/rc_flight.py +++ b/src/rc_demo/rc_flight.py @@ -19,14 +19,15 @@ class RCHandler: def __init__(self, use_rc_control: bool = True, waypoint_path: str = "", - target_waypoint: str = "waypoint_0") -> None: + target_waypoint: str = "waypoint_0", + arrive_tolerance: float = 0.5) -> None: self.use_rc_control = use_rc_control self.target_waypoint = target_waypoint self.rc_sub = rospy.Subscriber("/mavros/rc/in", RCIn, self.callback_rc_in) self.waypoint_trigger_sub = rospy.Subscriber("/custom/waypoint_trigger", String, self.callback_waypoint_trigger) self.control_msg = ControlMessage() - self.waypoint_msg = WaypointMessage() + self.waypoint_msg = WaypointMessage(arrive_tolerance) self.is_waypoint_working = False @@ -143,8 +144,9 @@ def callback_rc_in(self, msg: RCIn) -> None: use_rc_control = rospy.get_param('~rc_control', False) waypoint_path = rospy.get_param('~waypoint', "") target_waypoint = rospy.get_param('~target_waypoint', "waypoint_0") + arrive_tolerance = rospy.get_param('~arrive_tolerance', 0.5) - rc_handler = RCHandler(use_rc_control, waypoint_path, target_waypoint) + rc_handler = RCHandler(use_rc_control, waypoint_path, target_waypoint, arrive_tolerance) rospy.spin() diff --git a/src/rc_demo/utils.py b/src/rc_demo/utils.py index e0e89347..3a8ba228 100644 --- a/src/rc_demo/utils.py +++ b/src/rc_demo/utils.py @@ -33,7 +33,7 @@ def send_control(self, roll: float, pitch: float, yaw: float, throttle: float) - class WaypointMessage: """Send waypoint message via MavROS""" - def __init__(self) -> None: + def __init__(self, distance_tolerance: float = 0.5) -> None: self.waypoint_pub = rospy.Publisher('/waypoint_generator/waypoints', Path, queue_size=1) @@ -42,7 +42,7 @@ def __init__(self) -> None: self.callback_odometry) self._timer_send_waypoint = rospy.Timer(rospy.Duration(3.0), self._loop_send_waypoint) - self.distance_tolerance = 0.5 + self.distance_tolerance = distance_tolerance self.current_position = Odometry() self.target_position = PoseStamped() From 246c84109380171814c45d43c856ef4f6422f749 Mon Sep 17 00:00:00 2001 From: Haneol Kim Date: Thu, 29 Sep 2022 14:21:57 +0900 Subject: [PATCH 17/48] Add outdoor waypoints --- src/rc_demo/res/waypoints.yaml | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/src/rc_demo/res/waypoints.yaml b/src/rc_demo/res/waypoints.yaml index 069e185b..cece6538 100644 --- a/src/rc_demo/res/waypoints.yaml +++ b/src/rc_demo/res/waypoints.yaml @@ -20,9 +20,12 @@ waypoint_outdoor: # This way point has been set from roslaunch simulation_environment mavros_posix_sitl_lidar.launch world:=$HOME/custom_worlds/add_competition_outfield.world x:=10.5 y:=-1 z:=1 vehicle:=px4vision lidar:=ouster [ [3.72308462946, 2.16384181223, 1.59992909121], # first plants - [7.20119367623, 2.13588568156, 2.61656869656], # second plants - [12.7070259193, 2.08961550795, 2.43913609922], # wall - [14.3681804363, -0.525301411499, 1.50505519304], # before net + # [7.20119367623, 2.13588568156, 2.61656869656], # second plants -> for simulation + [7.20119367623, 2.13588568156, 1.71656869656], # second plants -> for rosbag + # [9.50119367623, 2.13588568156, 3.21656869656], # second plants + [10.70119367623, 2.13588568156, 3.21656869656], # second plants + [11.0070259193, 2.08961550795, 2.43913609922], # wall + [15.2681804363, -0.525301411499, 1.50505519304], # before net [17.8681804363, -0.525301411499, 1.50505519304], # after net [25, 0.00, 1.8005] # car ] From 90add00279b73cc5fae914c2810e17e67f84927f Mon Sep 17 00:00:00 2001 From: JeiKeiLim Date: Thu, 29 Sep 2022 14:55:08 +0900 Subject: [PATCH 18/48] Fix waypoint for rosbag. --- src/rc_demo/res/waypoints.yaml | 22 ++++++++++++++++------ 1 file changed, 16 insertions(+), 6 deletions(-) diff --git a/src/rc_demo/res/waypoints.yaml b/src/rc_demo/res/waypoints.yaml index cece6538..c46fb91c 100644 --- a/src/rc_demo/res/waypoints.yaml +++ b/src/rc_demo/res/waypoints.yaml @@ -3,7 +3,7 @@ waypoint_0: [ [10.0, 1.5, 2.0], [14, 0.8, 3.2], - [18, 0.8, 3.5], + [18, 0.8, 3.2], [19, -1.1, 2.0], [23, -1.1, 2.0], [25, -0.5, 2.5], @@ -22,10 +22,20 @@ waypoint_outdoor: [3.72308462946, 2.16384181223, 1.59992909121], # first plants # [7.20119367623, 2.13588568156, 2.61656869656], # second plants -> for simulation [7.20119367623, 2.13588568156, 1.71656869656], # second plants -> for rosbag - # [9.50119367623, 2.13588568156, 3.21656869656], # second plants - [10.70119367623, 2.13588568156, 3.21656869656], # second plants + [8.10119367623, 2.13588568156, 3.21656869656], # before wall + [10.70119367623, 2.13588568156, 3.21656869656], # before wall [11.0070259193, 2.08961550795, 2.43913609922], # wall - [15.2681804363, -0.525301411499, 1.50505519304], # before net - [17.8681804363, -0.525301411499, 1.50505519304], # after net - [25, 0.00, 1.8005] # car + [13.4681804363, -0.525301411499, 1.50505519304], # before net + [18.8681804363, -0.525301411499, 1.50505519304], # after net + [18.8681804363, 0.0, 1.50505519304], # after net + # [25, 0.00, 1.8005] # car + ] + +waypoint_indoor: + [ + [3.5, -0.9, 0.7], # Right of stair + [4.4, 1.3, 0.8], # In front of stair + [9.7, 1.6, 3.4], # Middle of stair left + [9.7, -0.3, 3.6], # Middle of stair right + [3.6, -0.3, 5.7], # 2nd floor ] From 3a44e31ae564e4884e0e3726c1287f51428696cf Mon Sep 17 00:00:00 2001 From: svin3 Date: Thu, 29 Sep 2022 15:00:05 +0900 Subject: [PATCH 19/48] fix change height algorithm 2 --- src/planner/plan_manage/src/ego_replan_fsm.cpp | 17 ++++++++++++++--- src/planner/plan_manage/src/traj_server.cpp | 10 ++-------- 2 files changed, 16 insertions(+), 11 deletions(-) diff --git a/src/planner/plan_manage/src/ego_replan_fsm.cpp b/src/planner/plan_manage/src/ego_replan_fsm.cpp index 881360b9..2ecffa95 100644 --- a/src/planner/plan_manage/src/ego_replan_fsm.cpp +++ b/src/planner/plan_manage/src/ego_replan_fsm.cpp @@ -245,13 +245,24 @@ namespace ego_planner trigger_ = true; init_pt_ = odom_pos_; - bool success = false; end_pt_ << msg.poses[0].pose.position.x, msg.poses[0].pose.position.y, msg.poses[0].pose.position.z; - end_pt_ << checkEnableWaypoint(odom_pos_, end_pt_); - success = planner_manager_->planGlobalTraj(odom_pos_, odom_vel_, Eigen::Vector3d::Zero(), end_pt_, Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero()); + Eigen::Vector3d sub_xy(end_pt_.x() - init_pt_.x(), end_pt_.y() - init_pt_.y(), 0); + if (sub_xy.norm() < no_replan_thresh_ && abs(end_pt_.z() - init_pt_.z()) > 0.1) { + traj_pts_.clear(); + traj_pts_.push_back(init_pt_); + traj_pts_.push_back(end_pt_); + traj_pts_.push_back(end_pt_); + visualization_->displayTrajList(traj_pts_, 0); + visualization_->displayGoalPoint(end_pt_, Eigen::Vector4d(0, 0.5, 0.5, 1), 0.3, 0); + return; + } + + end_pt_ << checkEnableWaypoint(odom_pos_, end_pt_); visualization_->displayGoalPoint(end_pt_, Eigen::Vector4d(0, 0.5, 0.5, 1), 0.3, 0); + bool success = planner_manager_->planGlobalTraj(odom_pos_, odom_vel_, Eigen::Vector3d::Zero(), end_pt_, Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero()); + if (success) { diff --git a/src/planner/plan_manage/src/traj_server.cpp b/src/planner/plan_manage/src/traj_server.cpp index 91c6f4bf..047961e2 100644 --- a/src/planner/plan_manage/src/traj_server.cpp +++ b/src/planner/plan_manage/src/traj_server.cpp @@ -184,7 +184,7 @@ void cmdCallback(const ros::TimerEvent &e) /* check receive traj_ before calculate target */ if (receive_traj_) { - Eigen::Vector3d z_offset = Eigen::Vector3d(0, 0, 0.1); + Eigen::Vector3d z_offset = Eigen::Vector3d(0, 0, 0); Eigen::Vector3d closestPoint = findClosestPoint(odom_pos_, traj_[0]); Eigen::Vector3d refTarget = findClosestPoint(closestPoint, traj_[0], forward_length_) + z_offset; Eigen::Vector3d refTarget_forward = findClosestPoint(closestPoint, traj_[0], forward_length_ * 2.5) + z_offset; @@ -237,18 +237,12 @@ void cmdCallback(const ros::TimerEvent &e) msg.velocity.x = refVel.x(); msg.velocity.y = refVel.y(); msg.velocity.z = refVel.z(); - - if ((ros::Time::now() - start_time_).toSec() < 1.0) { - msg.velocity.x = 0.0; - msg.velocity.y = 0.0; - msg.velocity.z = 0.0; - } } if (sub_vector_xy.norm() > 0.5) { msg.type_mask |= msg.IGNORE_YAW_RATE; msg.yaw = ref_yaw; - } else if (sub_vector_xy.norm() < 0.1) { + } else if (sub_vector.norm() < 0.1) { msg.type_mask |= msg.IGNORE_YAW; receive_traj_ = false; last_odom_pos_ = odom_pos_; From 324e2c8487915e55b25b937ca5f9e2062840f349 Mon Sep 17 00:00:00 2001 From: svin3 Date: Thu, 29 Sep 2022 15:56:14 +0900 Subject: [PATCH 20/48] fix near waypoint not working --- src/planner/plan_manage/src/ego_replan_fsm.cpp | 1 + src/planner/plan_manage/src/traj_server.cpp | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/src/planner/plan_manage/src/ego_replan_fsm.cpp b/src/planner/plan_manage/src/ego_replan_fsm.cpp index 2ecffa95..701b9d52 100644 --- a/src/planner/plan_manage/src/ego_replan_fsm.cpp +++ b/src/planner/plan_manage/src/ego_replan_fsm.cpp @@ -460,6 +460,7 @@ namespace ego_planner { // cout << "near end" << endl; traj_pts_.push_back(end_pt_); + if (traj_pts_.size() == 2) traj_pts_.push_back(end_pt_); visualization_->displayTrajList(traj_pts_, 0); current_traj_ = generateTraj(traj_pts_); return; diff --git a/src/planner/plan_manage/src/traj_server.cpp b/src/planner/plan_manage/src/traj_server.cpp index 047961e2..b07216fd 100644 --- a/src/planner/plan_manage/src/traj_server.cpp +++ b/src/planner/plan_manage/src/traj_server.cpp @@ -108,7 +108,7 @@ void customTrajCallback(visualization_msgs::MarkerConstPtr msg) UniformBspline pos_traj(pos_pts, 3, 1.0); - if (pos_traj.getTimeSum() > 0 && pos_traj.getLength(0.02) > forward_length_) { + if (pos_traj.getTimeSum() > 0) { start_time_ = ros::Time::now(); traj_.clear(); traj_.push_back(pos_traj); From 76873bd2780c1f86fde19d7ad22c893f77c65ccb Mon Sep 17 00:00:00 2001 From: Jongkuk Lim Date: Wed, 5 Oct 2022 01:07:19 +0000 Subject: [PATCH 21/48] Modify ego planner parameters in flight test time --- src/planner/plan_manage/launch/advanced_param.xml | 2 +- .../plan_manage/launch/run_in_flight.launch | 14 +++++++------- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/planner/plan_manage/launch/advanced_param.xml b/src/planner/plan_manage/launch/advanced_param.xml index 876a3e93..4ffe364f 100644 --- a/src/planner/plan_manage/launch/advanced_param.xml +++ b/src/planner/plan_manage/launch/advanced_param.xml @@ -106,7 +106,7 @@ - + diff --git a/src/planner/plan_manage/launch/run_in_flight.launch b/src/planner/plan_manage/launch/run_in_flight.launch index 825f0b4e..c3228e8b 100644 --- a/src/planner/plan_manage/launch/run_in_flight.launch +++ b/src/planner/plan_manage/launch/run_in_flight.launch @@ -2,10 +2,10 @@ - + - + @@ -36,11 +36,11 @@ - - + + - + @@ -70,7 +70,7 @@ - + @@ -80,7 +80,7 @@ - + From ea0624e7ccdde57bd2466bcbe7cc5e8f82ba8cac Mon Sep 17 00:00:00 2001 From: svin3 Date: Wed, 5 Oct 2022 13:47:44 +0900 Subject: [PATCH 22/48] fix merge err --- src/planner/plan_manage/src/traj_server.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/planner/plan_manage/src/traj_server.cpp b/src/planner/plan_manage/src/traj_server.cpp index 8dd1aaf1..b07216fd 100644 --- a/src/planner/plan_manage/src/traj_server.cpp +++ b/src/planner/plan_manage/src/traj_server.cpp @@ -242,7 +242,7 @@ void cmdCallback(const ros::TimerEvent &e) if (sub_vector_xy.norm() > 0.5) { msg.type_mask |= msg.IGNORE_YAW_RATE; msg.yaw = ref_yaw; - } else if (sub_vector_xy.norm() < 0.1) { + } else if (sub_vector.norm() < 0.1) { msg.type_mask |= msg.IGNORE_YAW; receive_traj_ = false; last_odom_pos_ = odom_pos_; From c434062769f674cde2a5ae0cdd096459e6cfb6da Mon Sep 17 00:00:00 2001 From: svin3 Date: Wed, 5 Oct 2022 13:54:23 +0900 Subject: [PATCH 23/48] fix map_size_z > virtual_ceil_height --- src/planner/plan_manage/launch/run_in_px4sim.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/planner/plan_manage/launch/run_in_px4sim.launch b/src/planner/plan_manage/launch/run_in_px4sim.launch index ebf22b86..241f1fde 100644 --- a/src/planner/plan_manage/launch/run_in_px4sim.launch +++ b/src/planner/plan_manage/launch/run_in_px4sim.launch @@ -2,7 +2,7 @@ - + From 40b7909c57e011b2ef50a668ae0ce2c2f5672af7 Mon Sep 17 00:00:00 2001 From: svin3 Date: Wed, 5 Oct 2022 14:11:47 +0900 Subject: [PATCH 24/48] update max_vel param & add takeoff waypoint at waypoint_outdoor --- src/planner/plan_manage/launch/run_in_flight.launch | 1 + src/planner/plan_manage/launch/run_in_px4sim.launch | 1 + src/planner/plan_manage/src/traj_server.cpp | 6 ++++-- src/rc_demo/res/waypoints.yaml | 1 + 4 files changed, 7 insertions(+), 2 deletions(-) diff --git a/src/planner/plan_manage/launch/run_in_flight.launch b/src/planner/plan_manage/launch/run_in_flight.launch index c3228e8b..72e4713f 100644 --- a/src/planner/plan_manage/launch/run_in_flight.launch +++ b/src/planner/plan_manage/launch/run_in_flight.launch @@ -82,6 +82,7 @@ + diff --git a/src/planner/plan_manage/launch/run_in_px4sim.launch b/src/planner/plan_manage/launch/run_in_px4sim.launch index 241f1fde..ace07c2a 100644 --- a/src/planner/plan_manage/launch/run_in_px4sim.launch +++ b/src/planner/plan_manage/launch/run_in_px4sim.launch @@ -78,6 +78,7 @@ + diff --git a/src/planner/plan_manage/src/traj_server.cpp b/src/planner/plan_manage/src/traj_server.cpp index b07216fd..c467b853 100644 --- a/src/planner/plan_manage/src/traj_server.cpp +++ b/src/planner/plan_manage/src/traj_server.cpp @@ -39,6 +39,8 @@ double time_forward_; bool use_velocity_control_, enable_rotate_head_; double forward_length_; +double max_vel_; + void bsplineCallback(ego_planner::BsplineConstPtr msg) { // parse pos traj @@ -229,10 +231,9 @@ void cmdCallback(const ros::TimerEvent &e) msg.type_mask |= msg.IGNORE_PX | msg.IGNORE_PY | msg.IGNORE_PZ; double alpha = 0.5; - double max_velocity = 1.0; Eigen::Vector3d refVel = alpha * (refTarget - odom_pos_) + (1.0 - alpha) * (closestPoint - odom_pos_); - refVel = refVel * (min(sub_vector.norm(), max_velocity) / refVel.norm()); + refVel = refVel * (min(sub_vector.norm(), max_vel_) / refVel.norm()); msg.velocity.x = refVel.x(); msg.velocity.y = refVel.y(); @@ -336,6 +337,7 @@ int main(int argc, char **argv) nh.param("traj_server/use_velocity_control", use_velocity_control_, false); nh.param("traj_server/forward_length", forward_length_, 1.0); nh.param("traj_server/enable_rotate_head", enable_rotate_head_, true); + nh.param("traj_server/max_vel", max_vel_, 1.0); ros::Duration(1.0).sleep(); diff --git a/src/rc_demo/res/waypoints.yaml b/src/rc_demo/res/waypoints.yaml index c46fb91c..d6b2f979 100644 --- a/src/rc_demo/res/waypoints.yaml +++ b/src/rc_demo/res/waypoints.yaml @@ -19,6 +19,7 @@ waypoint_1: waypoint_outdoor: # This way point has been set from roslaunch simulation_environment mavros_posix_sitl_lidar.launch world:=$HOME/custom_worlds/add_competition_outfield.world x:=10.5 y:=-1 z:=1 vehicle:=px4vision lidar:=ouster [ + [0, 0, 1.59992909121], # hover [3.72308462946, 2.16384181223, 1.59992909121], # first plants # [7.20119367623, 2.13588568156, 2.61656869656], # second plants -> for simulation [7.20119367623, 2.13588568156, 1.71656869656], # second plants -> for rosbag From 7c707566aec11ffc361cfc23f1f7c3db2e4436ee Mon Sep 17 00:00:00 2001 From: svin3 Date: Wed, 5 Oct 2022 16:57:14 +0900 Subject: [PATCH 25/48] update fix waypoint function --- .../plan_manage/launch/run_in_flight.launch | 2 +- .../plan_manage/src/ego_replan_fsm.cpp | 103 +++++++++++++++--- src/rc_demo/launch/rc_flight.launch | 4 +- 3 files changed, 91 insertions(+), 18 deletions(-) diff --git a/src/planner/plan_manage/launch/run_in_flight.launch b/src/planner/plan_manage/launch/run_in_flight.launch index 72e4713f..77557bfc 100644 --- a/src/planner/plan_manage/launch/run_in_flight.launch +++ b/src/planner/plan_manage/launch/run_in_flight.launch @@ -6,7 +6,7 @@ - + diff --git a/src/planner/plan_manage/src/ego_replan_fsm.cpp b/src/planner/plan_manage/src/ego_replan_fsm.cpp index 701b9d52..1bce2f0c 100644 --- a/src/planner/plan_manage/src/ego_replan_fsm.cpp +++ b/src/planner/plan_manage/src/ego_replan_fsm.cpp @@ -246,6 +246,7 @@ namespace ego_planner init_pt_ = odom_pos_; end_pt_ << msg.poses[0].pose.position.x, msg.poses[0].pose.position.y, msg.poses[0].pose.position.z; + visualization_->displayGoalPoint(end_pt_, Eigen::Vector4d(0.25, 1.0, 0.25, 0.7), 0.3, 0); Eigen::Vector3d sub_xy(end_pt_.x() - init_pt_.x(), end_pt_.y() - init_pt_.y(), 0); if (sub_xy.norm() < no_replan_thresh_ && abs(end_pt_.z() - init_pt_.z()) > 0.1) { @@ -254,12 +255,12 @@ namespace ego_planner traj_pts_.push_back(end_pt_); traj_pts_.push_back(end_pt_); visualization_->displayTrajList(traj_pts_, 0); - visualization_->displayGoalPoint(end_pt_, Eigen::Vector4d(0, 0.5, 0.5, 1), 0.3, 0); + visualization_->displayGoalPoint(end_pt_, Eigen::Vector4d(0.25, 0.25, 1.0, 0.7), 0.3, 1); return; } end_pt_ << checkEnableWaypoint(odom_pos_, end_pt_); - visualization_->displayGoalPoint(end_pt_, Eigen::Vector4d(0, 0.5, 0.5, 1), 0.3, 0); + visualization_->displayGoalPoint(end_pt_, Eigen::Vector4d(0.25, 0.25, 1.0, 0.7), 0.3, 1); bool success = planner_manager_->planGlobalTraj(odom_pos_, odom_vel_, Eigen::Vector3d::Zero(), end_pt_, Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero()); @@ -601,45 +602,117 @@ namespace ego_planner // } } - Eigen::Vector3d EGOReplanFSM::checkEnableWaypoint(const Eigen::Vector3d &start, const Eigen::Vector3d &waypoint) { + Eigen::Vector3d EGOReplanFSM::checkEnableWaypoint(const Eigen::Vector3d &start, const Eigen::Vector3d &end) { + Eigen::Vector3d waypoint = end; + ros::Time time_1 = ros::Time::now(); + // first, check is waypoint validate + auto map = planner_manager_->grid_map_; + auto inflatePointWithoutObs = [map](const Eigen::Vector3d& pt, int step, vector& pts) { + pts.clear(); + double res = map->getResolution(); + /* ---------- all inflate ---------- */ + for (int x = -step; x <= step; ++x) + for (int y = -step; y <= step; ++y) + for (int z = -1; z <= 1; ++z) { + if (sqrt(x*x + y*y + z*z) > (double)step) continue; + Eigen::Vector3d new_pt(pt(0) + (double)x * res, pt(1) + (double)y * res, pt(2) + (double)z * res); + if (!map->getInflateOccupancy(new_pt)) { + pts.push_back(new_pt); + } + } + }; + + int inf_step = 7; // TODO: add inf_step parameter for this function + vector inf_pts; + inflatePointWithoutObs(waypoint, inf_step, inf_pts); + + if (inf_pts.size() > 0) { + double best_score = inf_pts.size(); + Eigen::Vector3d best_pt = waypoint; + + for (auto inf_pt : inf_pts) { + vector inf_pts_sub; + inflatePointWithoutObs(inf_pt, inf_step, inf_pts_sub); + double score = (double)inf_pts_sub.size(); + if (score > best_score) { + best_pt = inf_pt; + best_score = score; + } + } + + waypoint = best_pt; + } + + // TODO: check A* start path +ros::Time time_2 = ros::Time::now(); vector> a_star_pathes; vector a_star_path_H, a_star_path_V, a_star_path_A; double expected_length = (waypoint - start).norm() / planner_manager_->grid_map_->getResolution(); + + auto shrink_path = [&](vector a_star_path){ + if (a_star_path.size() < 2) return a_star_path; + + vector shrinked_path; + Eigen::Vector3d last_ptd = a_star_path.front(); + Eigen::Vector3i last_pti; + Eigen::Vector3i last_dir(0, 0, 0); + planner_manager_->grid_map_->posToIndex(last_ptd, last_pti); + + for (size_t i = 0; i < a_star_path.size(); i++) { + Eigen::Vector3i cur_pti; + planner_manager_->grid_map_->posToIndex(a_star_path[i], cur_pti); + Eigen::Vector3i cur_dir = cur_pti - last_pti; + if (last_dir != cur_dir) { + shrinked_path.push_back(last_ptd); + last_ptd = a_star_path[i-1]; + last_pti = cur_pti; + last_dir = cur_dir; + } else if (i == a_star_path.size() - 1) { + shrinked_path.push_back(last_ptd); + } else { + last_pti = cur_pti; + } + } + + shrinked_path.push_back(a_star_path.back()); + return shrinked_path; + }; if (expected_length > 1) { planner_manager_->grid_map_->setSearchAreaH(); - if (planner_manager_->bspline_optimizer_rebound_->a_star_->AstarSearch(0.1, odom_pos_, end_pt_)) { + if (planner_manager_->bspline_optimizer_rebound_->a_star_->AstarSearch(0.1, odom_pos_, waypoint)) { a_star_path_H = planner_manager_->bspline_optimizer_rebound_->a_star_->getPath(); } } - if (a_star_path_H.size() > expected_length * 1.1) { - planner_manager_->grid_map_->setSearchAreaV(end_pt_); - if (planner_manager_->bspline_optimizer_rebound_->a_star_->AstarSearch(0.1, odom_pos_, end_pt_)) { + if (a_star_path_H.size() > expected_length * 1.1 || a_star_path_H.size() == 0) { + planner_manager_->grid_map_->setSearchAreaV(waypoint); + if (planner_manager_->bspline_optimizer_rebound_->a_star_->AstarSearch(0.1, odom_pos_, waypoint)) { a_star_path_V = planner_manager_->bspline_optimizer_rebound_->a_star_->getPath(); } } - if (a_star_path_V.size() > expected_length * 1.1 || (a_star_path_V.size() == 0 && a_star_path_H.size() > expected_length * 1.1)) + if (a_star_path_V.size() > expected_length * 1.1 || (a_star_path_V.size() == 0 && a_star_path_H.size() > expected_length * 1.1) || a_star_path_H.size() == 0) { planner_manager_->grid_map_->resetSearchArea(); - if (planner_manager_->bspline_optimizer_rebound_->a_star_->AstarSearch(0.1, odom_pos_, end_pt_)) { + if (planner_manager_->bspline_optimizer_rebound_->a_star_->AstarSearch(0.1, odom_pos_, waypoint)) { a_star_path_A = planner_manager_->bspline_optimizer_rebound_->a_star_->getPath(); } } - a_star_pathes.push_back(a_star_path_H); - a_star_pathes.push_back(a_star_path_V); - a_star_pathes.push_back(a_star_path_A); + a_star_pathes.push_back(shrink_path(a_star_path_H)); + a_star_pathes.push_back(shrink_path(a_star_path_V)); + a_star_pathes.push_back(shrink_path(a_star_path_A)); planner_manager_->visualization_->displayAStarList(a_star_pathes, 0); planner_manager_->grid_map_->resetSearchArea(); -ros::Time time_2 = ros::Time::now(); +ros::Time time_3 = ros::Time::now(); // compute time check -std::cout << "AstarSearch time : " << (time_2 - time_1).toSec() << std::endl; -std::cout << "expected : " << (waypoint - start).norm() << ", "; +std::cout << "move goal time : " << (time_2 - time_1).toSec() << std::endl; +std::cout << "AstarSearch time : " << (time_3 - time_2).toSec() << std::endl; +std::cout << "expected : " << expected_length << ", "; std::cout << "H : " << a_star_path_H.size() << ", "; std::cout << "V : " << a_star_path_V.size() << ", "; std::cout << "A : " << a_star_path_A.size() << std::endl; diff --git a/src/rc_demo/launch/rc_flight.launch b/src/rc_demo/launch/rc_flight.launch index 2842ac5b..72c01d05 100644 --- a/src/rc_demo/launch/rc_flight.launch +++ b/src/rc_demo/launch/rc_flight.launch @@ -1,8 +1,8 @@ - - + + From b7696c63a7d45915eec9d5e2426a1ef52593bc55 Mon Sep 17 00:00:00 2001 From: svin3 Date: Wed, 5 Oct 2022 17:14:26 +0900 Subject: [PATCH 26/48] fix range of moving waypoint --- src/planner/plan_manage/src/ego_replan_fsm.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/planner/plan_manage/src/ego_replan_fsm.cpp b/src/planner/plan_manage/src/ego_replan_fsm.cpp index 1bce2f0c..28aa7daa 100644 --- a/src/planner/plan_manage/src/ego_replan_fsm.cpp +++ b/src/planner/plan_manage/src/ego_replan_fsm.cpp @@ -614,7 +614,7 @@ ros::Time time_1 = ros::Time::now(); /* ---------- all inflate ---------- */ for (int x = -step; x <= step; ++x) for (int y = -step; y <= step; ++y) - for (int z = -1; z <= 1; ++z) { + for (int z = -step/2; z <= step/2; ++z) { if (sqrt(x*x + y*y + z*z) > (double)step) continue; Eigen::Vector3d new_pt(pt(0) + (double)x * res, pt(1) + (double)y * res, pt(2) + (double)z * res); if (!map->getInflateOccupancy(new_pt)) { From f4b430f4eb2dcfdea38ba32202c921621bd80f28 Mon Sep 17 00:00:00 2001 From: JeiKeiLim Date: Wed, 5 Oct 2022 17:52:39 +0900 Subject: [PATCH 27/48] Add fixing waypoint for rc_flight.py from ego-planner. --- src/rc_demo/utils.py | 20 ++++++++++++++++++-- 1 file changed, 18 insertions(+), 2 deletions(-) diff --git a/src/rc_demo/utils.py b/src/rc_demo/utils.py index 3a8ba228..5c9813ff 100644 --- a/src/rc_demo/utils.py +++ b/src/rc_demo/utils.py @@ -7,7 +7,12 @@ from mavros_msgs.srv import SetMode from mavros_msgs.srv import CommandBool from nav_msgs.msg import Path, Odometry -from geometry_msgs.msg import PoseStamped +from geometry_msgs.msg import PoseStamped, Pose +from visualization_msgs.msg import Marker + + +def compute_distance(x: Pose, y: Pose) -> float: + return math.sqrt(math.pow(x.x - y.x, 2) + math.pow(x.y - y.y, 2) + math.pow(x.z - y.z, 2)) class ControlMessage: @@ -40,6 +45,9 @@ def __init__(self, distance_tolerance: float = 0.5) -> None: self.odom_sub = rospy.Subscriber('/Odometry', Odometry, self.callback_odometry) + self.goal_point_sub = rospy.Subscriber('/ego_planner_node/goal_point', + Marker, + self.callback_goal_point) self._timer_send_waypoint = rospy.Timer(rospy.Duration(3.0), self._loop_send_waypoint) self.distance_tolerance = distance_tolerance @@ -69,7 +77,7 @@ def _check_current_location(self) -> bool: target_point = self.target_position.pose.position current_point = self.current_position.pose.pose.position - distance = math.sqrt(math.pow(target_point.x - current_point.x, 2) + math.pow(target_point.y - current_point.y, 2) + math.pow(target_point.z - current_point.z, 2)) + distance = compute_distance(target_point, current_point) if distance < self.distance_tolerance: return True @@ -118,6 +126,14 @@ def callback_odometry(self, msg: Odometry) -> None: if self.target_position.pose.position.x == 0 and self.target_position.pose.position.y == 0 and self.target_position.pose.position.z == 0: self.target_position.pose = msg.pose.pose + def callback_goal_point(self, msg: Marker) -> None: + if msg.id != 1: + return + + position = msg.pose.position + if compute_distance(msg.pose.position, self.target_position.pose.position) < self.distance_tolerance: + self.target_position.pose = msg.pose + class MAVROSCommander: def __init__(self): From bbf0e846b457eba88da0325e24aeeced189cd349 Mon Sep 17 00:00:00 2001 From: svin3 Date: Wed, 5 Oct 2022 18:14:23 +0900 Subject: [PATCH 28/48] update fix start point function --- .../include/plan_manage/ego_replan_fsm.h | 1 + .../plan_manage/src/ego_replan_fsm.cpp | 56 ++++++++++++++++--- 2 files changed, 50 insertions(+), 7 deletions(-) diff --git a/src/planner/plan_manage/include/plan_manage/ego_replan_fsm.h b/src/planner/plan_manage/include/plan_manage/ego_replan_fsm.h index cf7e96eb..15688e0e 100644 --- a/src/planner/plan_manage/include/plan_manage/ego_replan_fsm.h +++ b/src/planner/plan_manage/include/plan_manage/ego_replan_fsm.h @@ -115,6 +115,7 @@ namespace ego_planner bool is_need_replan_ = false; int reboundReplan_fail_count_ = 0; Eigen::Vector3d checkEnableWaypoint(const Eigen::Vector3d &start, const Eigen::Vector3d &waypoint); + Eigen::Vector3d checkEnableStartpoint(const Eigen::Vector3d &start); public: EGOReplanFSM(/* args */) diff --git a/src/planner/plan_manage/src/ego_replan_fsm.cpp b/src/planner/plan_manage/src/ego_replan_fsm.cpp index 28aa7daa..eb151757 100644 --- a/src/planner/plan_manage/src/ego_replan_fsm.cpp +++ b/src/planner/plan_manage/src/ego_replan_fsm.cpp @@ -243,7 +243,7 @@ namespace ego_planner cout << "Triggered!" << endl; trigger_ = true; - init_pt_ = odom_pos_; + init_pt_ = checkEnableStartpoint(odom_pos_); end_pt_ << msg.poses[0].pose.position.x, msg.poses[0].pose.position.y, msg.poses[0].pose.position.z; visualization_->displayGoalPoint(end_pt_, Eigen::Vector4d(0.25, 1.0, 0.25, 0.7), 0.3, 0); @@ -259,10 +259,10 @@ namespace ego_planner return; } - end_pt_ << checkEnableWaypoint(odom_pos_, end_pt_); + end_pt_ << checkEnableWaypoint(init_pt_, end_pt_); visualization_->displayGoalPoint(end_pt_, Eigen::Vector4d(0.25, 0.25, 1.0, 0.7), 0.3, 1); - bool success = planner_manager_->planGlobalTraj(odom_pos_, odom_vel_, Eigen::Vector3d::Zero(), end_pt_, Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero()); + bool success = planner_manager_->planGlobalTraj(init_pt_, odom_vel_, Eigen::Vector3d::Zero(), end_pt_, Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero()); if (success) { @@ -387,7 +387,7 @@ namespace ego_planner case GEN_NEW_TRAJ: { - start_pt_ = odom_pos_; + start_pt_ = init_pt_; start_vel_ = odom_vel_; start_acc_.setZero(); traj_pts_.clear(); @@ -602,6 +602,48 @@ namespace ego_planner // } } + Eigen::Vector3d EGOReplanFSM::checkEnableStartpoint(const Eigen::Vector3d &start) { + // first, check is waypoint validate + auto map = planner_manager_->grid_map_; + auto inflatePointWithoutObs = [map](const Eigen::Vector3d& pt, int step, vector& pts) { + pts.clear(); + double res = map->getResolution(); + /* ---------- all inflate ---------- */ + for (int x = -step; x <= step; ++x) + for (int y = -step; y <= step; ++y) + for (int z = -step; z <= step; ++z) { + if (sqrt(x*x + y*y + z*z) > (double)step) continue; + Eigen::Vector3d new_pt(pt(0) + (double)x * res, pt(1) + (double)y * res, pt(2) + (double)z * res); + if (!map->getInflateOccupancy(new_pt)) { + pts.push_back(new_pt); + } + } + }; + + int inf_step = 2; // TODO: add inf_step parameter for this function + vector inf_pts; + inflatePointWithoutObs(start, inf_step, inf_pts); + + if (inf_pts.size() > 0) { + double best_score = inf_pts.size(); + Eigen::Vector3d best_pt = start; + + for (auto inf_pt : inf_pts) { + vector inf_pts_sub; + inflatePointWithoutObs(inf_pt, inf_step, inf_pts_sub); + double score = (double)inf_pts_sub.size(); + if (score > best_score) { + best_pt = inf_pt; + best_score = score; + } + } + + return best_pt; + } + + return start; + } + Eigen::Vector3d EGOReplanFSM::checkEnableWaypoint(const Eigen::Vector3d &start, const Eigen::Vector3d &end) { Eigen::Vector3d waypoint = end; @@ -681,14 +723,14 @@ ros::Time time_2 = ros::Time::now(); if (expected_length > 1) { planner_manager_->grid_map_->setSearchAreaH(); - if (planner_manager_->bspline_optimizer_rebound_->a_star_->AstarSearch(0.1, odom_pos_, waypoint)) { + if (planner_manager_->bspline_optimizer_rebound_->a_star_->AstarSearch(0.1, start, waypoint)) { a_star_path_H = planner_manager_->bspline_optimizer_rebound_->a_star_->getPath(); } } if (a_star_path_H.size() > expected_length * 1.1 || a_star_path_H.size() == 0) { planner_manager_->grid_map_->setSearchAreaV(waypoint); - if (planner_manager_->bspline_optimizer_rebound_->a_star_->AstarSearch(0.1, odom_pos_, waypoint)) { + if (planner_manager_->bspline_optimizer_rebound_->a_star_->AstarSearch(0.1, start, waypoint)) { a_star_path_V = planner_manager_->bspline_optimizer_rebound_->a_star_->getPath(); } } @@ -696,7 +738,7 @@ ros::Time time_2 = ros::Time::now(); if (a_star_path_V.size() > expected_length * 1.1 || (a_star_path_V.size() == 0 && a_star_path_H.size() > expected_length * 1.1) || a_star_path_H.size() == 0) { planner_manager_->grid_map_->resetSearchArea(); - if (planner_manager_->bspline_optimizer_rebound_->a_star_->AstarSearch(0.1, odom_pos_, waypoint)) { + if (planner_manager_->bspline_optimizer_rebound_->a_star_->AstarSearch(0.1, start, waypoint)) { a_star_path_A = planner_manager_->bspline_optimizer_rebound_->a_star_->getPath(); } } From ce2acda272792cf9c9a0c9ec3fe683c4736b6108 Mon Sep 17 00:00:00 2001 From: Haneol Kim Date: Wed, 5 Oct 2022 19:28:27 +0900 Subject: [PATCH 29/48] Add indoor first room waypoints --- src/rc_demo/res/waypoints.yaml | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/src/rc_demo/res/waypoints.yaml b/src/rc_demo/res/waypoints.yaml index d6b2f979..c615efd5 100644 --- a/src/rc_demo/res/waypoints.yaml +++ b/src/rc_demo/res/waypoints.yaml @@ -40,3 +40,14 @@ waypoint_indoor: [9.7, -0.3, 3.6], # Middle of stair right [3.6, -0.3, 5.7], # 2nd floor ] + +waypoint_indoor2: + [ + [3.5, -0.9, 0.7], # Right of stair + [4.4, 1.3, 0.8], # In front of stair + [9.7, 1.6, 3.4], # Middle of stair left + [9.7, -0.3, 3.6], # Middle of stair right + [3.6, -0.3, 5.7], # 2nd floor + [1.77, -3.07, 5.7], # first door + [5.88 , -2.87, 5.7], # first door + ] From c2c77417e28bdd85eac6bce4b9f47caf92e9e1e7 Mon Sep 17 00:00:00 2001 From: svin3 Date: Thu, 6 Oct 2022 10:12:49 +0900 Subject: [PATCH 30/48] update use_waypoint_yaw parameter --- .../plan_manage/launch/run_in_flight.launch | 1 + .../plan_manage/launch/run_in_px4sim.launch | 1 + src/planner/plan_manage/src/traj_server.cpp | 24 +++++++++++++++++-- 3 files changed, 24 insertions(+), 2 deletions(-) diff --git a/src/planner/plan_manage/launch/run_in_flight.launch b/src/planner/plan_manage/launch/run_in_flight.launch index 77557bfc..d19fe121 100644 --- a/src/planner/plan_manage/launch/run_in_flight.launch +++ b/src/planner/plan_manage/launch/run_in_flight.launch @@ -82,6 +82,7 @@ + diff --git a/src/planner/plan_manage/launch/run_in_px4sim.launch b/src/planner/plan_manage/launch/run_in_px4sim.launch index ace07c2a..5dd277b0 100644 --- a/src/planner/plan_manage/launch/run_in_px4sim.launch +++ b/src/planner/plan_manage/launch/run_in_px4sim.launch @@ -78,6 +78,7 @@ + diff --git a/src/planner/plan_manage/src/traj_server.cpp b/src/planner/plan_manage/src/traj_server.cpp index c467b853..95370c14 100644 --- a/src/planner/plan_manage/src/traj_server.cpp +++ b/src/planner/plan_manage/src/traj_server.cpp @@ -15,6 +15,8 @@ ros::Publisher setpoint_raw_pub; Eigen::Vector3d odom_pos_, odom_vel_; // odometry state double odom_yaw_ = 0; geometry_msgs::Pose control_; +geometry_msgs::Pose cur_waypoint_; +bool cur_waypoint_initialized_ = false; ros::Publisher pos_cmd_pub, ref_target_pub; @@ -36,7 +38,7 @@ int traj_id_; // yaw control double last_yaw_, last_yaw_dot_; double time_forward_; -bool use_velocity_control_, enable_rotate_head_; +bool use_velocity_control_, enable_rotate_head_, use_waypoint_yaw_; double forward_length_; double max_vel_; @@ -193,10 +195,21 @@ void cmdCallback(const ros::TimerEvent &e) Eigen::Vector3d sub_vector(refTarget.x() - odom_pos_.x(), refTarget.y() - odom_pos_.y(), refTarget.z() - odom_pos_.z()); Eigen::Vector3d sub_vector_xy(refTarget.x() - odom_pos_.x(), refTarget.y() - odom_pos_.y(), 0); + // std::cout << "use_waypoint_yaw_ : " << (use_waypoint_yaw_?"true":"false") << ", "; + // std::cout << "cur_waypoint_initialized_ : " << (cur_waypoint_initialized_?"true":"false") << std::endl; double ref_yaw = last_yaw_; if (enable_rotate_head_) { ref_yaw = atan2(refTarget_forward.y() - odom_pos_.y(), refTarget_forward.x() - odom_pos_.x()); last_yaw_ = ref_yaw; + } else if (use_waypoint_yaw_ && cur_waypoint_initialized_) { + Eigen::Quaterniond waypoint_orient; + waypoint_orient.x() = cur_waypoint_.orientation.x; + waypoint_orient.y() = cur_waypoint_.orientation.y; + waypoint_orient.z() = cur_waypoint_.orientation.z; + waypoint_orient.w() = cur_waypoint_.orientation.w; + Eigen::Vector3d rot_x = waypoint_orient.toRotationMatrix().block(0, 0, 3, 1); + ref_yaw = atan2(rot_x(1), rot_x(0)); + last_yaw_ = ref_yaw; } // visualize target @@ -304,6 +317,11 @@ void controlCallback(const geometry_msgs::PoseConstPtr &msg) { control_ = *msg; } +void waypointCallback(const nav_msgs::PathConstPtr &msg) { + cur_waypoint_ = msg->poses[0].pose; + cur_waypoint_initialized_ = true; +} + int main(int argc, char **argv) { ros::init(argc, argv, "traj_server"); @@ -313,7 +331,8 @@ int main(int argc, char **argv) // ros::Subscriber bspline_sub = node.subscribe("planning/bspline", 10, bsplineCallback); ros::Subscriber trajlist_sub = node.subscribe("/ego_planner_node/traj_list", 10, customTrajCallback); ros::Subscriber odom_sub = node.subscribe("/odom_world", 10, odom_callback); - ros::Subscriber waypoint_sub = node.subscribe("/waypoint_generator/waypoint_manual", 10, controlCallback); + ros::Subscriber control_sub = node.subscribe("/waypoint_generator/waypoint_manual", 10, controlCallback); + ros::Subscriber waypoint_sub = node.subscribe("/waypoint_generator/waypoints", 10, waypointCallback); pos_cmd_pub = node.advertise("/position_cmd", 50); setpoint_raw_pub = node.advertise("/mavros/setpoint_raw/local", 1); @@ -337,6 +356,7 @@ int main(int argc, char **argv) nh.param("traj_server/use_velocity_control", use_velocity_control_, false); nh.param("traj_server/forward_length", forward_length_, 1.0); nh.param("traj_server/enable_rotate_head", enable_rotate_head_, true); + nh.param("traj_server/use_waypoint_yaw", use_waypoint_yaw_, false); nh.param("traj_server/max_vel", max_vel_, 1.0); ros::Duration(1.0).sleep(); From 6507d6c04789081afa9bd51773e76c9d3a109c0a Mon Sep 17 00:00:00 2001 From: JeiKeiLim Date: Thu, 6 Oct 2022 10:42:44 +0900 Subject: [PATCH 31/48] Add waypoint with yaw direction. --- src/rc_demo/res/waypoints.yaml | 15 ++++++-- src/rc_demo/utils.py | 67 ++++++++++++++++++++++++++++++++-- 2 files changed, 75 insertions(+), 7 deletions(-) diff --git a/src/rc_demo/res/waypoints.yaml b/src/rc_demo/res/waypoints.yaml index c615efd5..de2b4c88 100644 --- a/src/rc_demo/res/waypoints.yaml +++ b/src/rc_demo/res/waypoints.yaml @@ -34,15 +34,24 @@ waypoint_outdoor: waypoint_indoor: [ - [3.5, -0.9, 0.7], # Right of stair - [4.4, 1.3, 0.8], # In front of stair - [9.7, 1.6, 3.4], # Middle of stair left + # x, y, z, yaw(optional) + # yaw 0: Y direction + # yaw 90: X direction + # yaw 180: -Y direction + # yaw 270: -X direction + # If yaw is not given, previous yaw is maintained. + [0, 0, 1.0, 135], # hover + [3.9, -0.9, 0.7], # Right of stair + [4.4, 1.3, 0.8, 45], # In front of stair + [9.9, 1.6, 3.4], # Middle of stair left [9.7, -0.3, 3.6], # Middle of stair right [3.6, -0.3, 5.7], # 2nd floor + [1.6, -4.0, 5.0], # 1st door ] waypoint_indoor2: [ + [1, 0, 1.0], # hover [3.5, -0.9, 0.7], # Right of stair [4.4, 1.3, 0.8], # In front of stair [9.7, 1.6, 3.4], # Middle of stair left diff --git a/src/rc_demo/utils.py b/src/rc_demo/utils.py index 5c9813ff..671dd325 100644 --- a/src/rc_demo/utils.py +++ b/src/rc_demo/utils.py @@ -15,6 +15,53 @@ def compute_distance(x: Pose, y: Pose) -> float: return math.sqrt(math.pow(x.x - y.x, 2) + math.pow(x.y - y.y, 2) + math.pow(x.z - y.z, 2)) +def get_quaternion_from_euler(roll: float, pitch: float, yaw: float) -> Tuple[float, float, float, float]: + """Convert an Euler angle to a quaternion. + + Args: + roll: The roll (rotation around x-axis) angle in radians. + pitch: The pitch (rotation around y-axis) angle in radians. + yaw: The yaw (rotation around z-axis) angle in radians. + + Return: + qx, qy, qz, qw: The orientation in quaternion [x,y,z,w] format + """ + qx = math.sin(roll/2) * math.cos(pitch/2) * math.cos(yaw/2) - math.cos(roll/2) * math.sin(pitch/2) * math.sin(yaw/2) + qy = math.cos(roll/2) * math.sin(pitch/2) * math.cos(yaw/2) + math.sin(roll/2) * math.cos(pitch/2) * math.sin(yaw/2) + qz = math.cos(roll/2) * math.cos(pitch/2) * math.sin(yaw/2) - math.sin(roll/2) * math.sin(pitch/2) * math.cos(yaw/2) + qw = math.cos(roll/2) * math.cos(pitch/2) * math.cos(yaw/2) + math.sin(roll/2) * math.sin(pitch/2) * math.sin(yaw/2) + + return (qx, qy, qz, qw) + + +def quaternion_to_euler(x: float, y: float, z: float, w: float) -> Tuple[float, float, float]: + """Convert a quaternion to Euler angle. + + Args: + x: quaternion x + y: quaternion y + z: quaternion z + w: quaternion w + + Return: + roll, pitch, yaw in radians. + """ + t0 = +2.0 * (w * x + y * z) + t1 = +1.0 - 2.0 * (x * x + y * y) + roll = math.atan2(t0, t1) + + t2 = +2.0 * (w * y - z * x) + t2 = +1.0 if t2 > +1.0 else t2 + t2 = -1.0 if t2 < -1.0 else t2 + pitch = math.asin(t2) + + t3 = +2.0 * (w * z + x * y) + t4 = +1.0 - 2.0 * (y * y + z * z) + yaw = math.atan2(t3, t4) + + return (roll, pitch, yaw) + + class ControlMessage: """Send control message via MavROS""" @@ -66,9 +113,15 @@ def clear_waypoint(self) -> None: def add_waypoint(self, x: float, y: float, z: float) -> None: self.waypoints.append((x, y, z)) + def add_waypoint_yaw(self, x: float, y: float, z: float, yaw: float) -> None: + self.waypoints.append((x, y, z, yaw)) + def add_waypoints(self, points: List[Tuple[float, float, float]]) -> None: for point in points: - self.add_waypoint(*point) + if len(point) > 3: + self.add_waypoint_yaw(*point) + else: + self.add_waypoint(*point) def _check_current_location(self) -> bool: if len(self.waypoints) < 1: @@ -106,7 +159,14 @@ def _loop_send_waypoint(self, event = None) -> None: pose.pose.position.x = target_point[0] pose.pose.position.y = target_point[1] pose.pose.position.z = target_point[2] - pose.pose.orientation.w = 1.0 + pose.pose.orientation = self.current_position.pose.pose.orientation + + if len(target_point) > 3: + quaternion = get_quaternion_from_euler(0, 0, math.radians(-target_point[3]+90)) + pose.pose.orientation.x = quaternion[0] + pose.pose.orientation.y = quaternion[1] + pose.pose.orientation.z = quaternion[2] + pose.pose.orientation.w = quaternion[3] else: pose = target_point @@ -130,9 +190,8 @@ def callback_goal_point(self, msg: Marker) -> None: if msg.id != 1: return - position = msg.pose.position if compute_distance(msg.pose.position, self.target_position.pose.position) < self.distance_tolerance: - self.target_position.pose = msg.pose + self.target_position.pose.position = msg.pose.position class MAVROSCommander: From c9879811ca0ee464cdd904de5dcd35889718ce4a Mon Sep 17 00:00:00 2001 From: JeiKeiLim Date: Thu, 6 Oct 2022 12:09:51 +0900 Subject: [PATCH 32/48] Fix to separate goal point and target point. --- src/rc_demo/utils.py | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) diff --git a/src/rc_demo/utils.py b/src/rc_demo/utils.py index 671dd325..cb86c53b 100644 --- a/src/rc_demo/utils.py +++ b/src/rc_demo/utils.py @@ -100,7 +100,10 @@ def __init__(self, distance_tolerance: float = 0.5) -> None: self.distance_tolerance = distance_tolerance self.current_position = Odometry() + """Target position is the designated waypoint""" self.target_position = PoseStamped() + """Goal position is modified waypoint from ego-planner""" + self.goal_position = Pose() self.is_moving = False self.waypoints = [] self.send_seq = 0 @@ -127,7 +130,8 @@ def _check_current_location(self) -> bool: if len(self.waypoints) < 1: return False - target_point = self.target_position.pose.position + # target_point = self.target_position.pose.position + target_point = self.goal_position.position current_point = self.current_position.pose.pose.position distance = compute_distance(target_point, current_point) @@ -176,6 +180,7 @@ def _loop_send_waypoint(self, event = None) -> None: msg.header.frame_id = "map" msg.poses.append(pose) self.target_position = pose + self.goal_position = pose.pose self.waypoint_pub.publish(msg) self.is_moving = True @@ -184,14 +189,16 @@ def callback_odometry(self, msg: Odometry) -> None: self.current_position = msg if self.target_position.pose.position.x == 0 and self.target_position.pose.position.y == 0 and self.target_position.pose.position.z == 0: + self.goal_position = msg.pose.pose self.target_position.pose = msg.pose.pose def callback_goal_point(self, msg: Marker) -> None: if msg.id != 1: return - if compute_distance(msg.pose.position, self.target_position.pose.position) < self.distance_tolerance: - self.target_position.pose.position = msg.pose.position + # if compute_distance(msg.pose.position, self.target_position.pose.position) < (self.distance_tolerance * 2): + # self.target_position.pose.position = msg.pose.position + self.goal_position.position = msg.pose.position class MAVROSCommander: From b7be3f7addf27757858232944635dfad443e1807 Mon Sep 17 00:00:00 2001 From: Haneol Kim Date: Thu, 6 Oct 2022 12:10:13 +0900 Subject: [PATCH 33/48] Fix second floor waypoints and modify rc_flight --- src/rc_demo/rc_flight.py | 2 ++ src/rc_demo/res/waypoints.yaml | 11 +++++++---- src/rc_demo/utils.py | 3 +++ 3 files changed, 12 insertions(+), 4 deletions(-) diff --git a/src/rc_demo/rc_flight.py b/src/rc_demo/rc_flight.py index e464eaff..0c0511d6 100755 --- a/src/rc_demo/rc_flight.py +++ b/src/rc_demo/rc_flight.py @@ -52,6 +52,8 @@ def __init__(self, 'waypoint_1': [[0.0, 0.0, 1.2]], } + print(f"[TARGET WAYPOINT]: {self.target_waypoint}") + def _start_waypoint(self) -> None: if self.is_waypoint_working: return diff --git a/src/rc_demo/res/waypoints.yaml b/src/rc_demo/res/waypoints.yaml index de2b4c88..f6c779f6 100644 --- a/src/rc_demo/res/waypoints.yaml +++ b/src/rc_demo/res/waypoints.yaml @@ -55,8 +55,11 @@ waypoint_indoor2: [3.5, -0.9, 0.7], # Right of stair [4.4, 1.3, 0.8], # In front of stair [9.7, 1.6, 3.4], # Middle of stair left - [9.7, -0.3, 3.6], # Middle of stair right - [3.6, -0.3, 5.7], # 2nd floor - [1.77, -3.07, 5.7], # first door - [5.88 , -2.87, 5.7], # first door + [9.7, -0.4, 3.6], # Middle of stair right + [1.7, 0.2, 5.7], # 2nd floor + [1.7, -0.8, 5.7, 90], # In front of first door + [1.8, -4.07, 5.7], # first door + [1.8, -4.07, 5.7, 0], # first door left turn + [1.8, -4.07, 5.7, 90], # first door + [3.58 , -7.87, 5.7, 180], # in front of second door ] diff --git a/src/rc_demo/utils.py b/src/rc_demo/utils.py index 671dd325..732bd96f 100644 --- a/src/rc_demo/utils.py +++ b/src/rc_demo/utils.py @@ -170,6 +170,7 @@ def _loop_send_waypoint(self, event = None) -> None: else: pose = target_point + msg = Path() msg.header.seq = self.send_seq msg.header.stamp = rospy.Time.now() @@ -177,6 +178,8 @@ def _loop_send_waypoint(self, event = None) -> None: msg.poses.append(pose) self.target_position = pose + print(f"[CURRENT WAYPOINT]: \n{pose.pose.position}") + print(f"[ ORIENTATION ]: \n{pose.pose.orientation}") self.waypoint_pub.publish(msg) self.is_moving = True From b3831e99f69cb384b19bab81336da146b05b1603 Mon Sep 17 00:00:00 2001 From: svin3 Date: Thu, 6 Oct 2022 13:50:24 +0900 Subject: [PATCH 34/48] update z_offset param in traj_server --- src/planner/plan_manage/launch/run_in_flight.launch | 3 ++- src/planner/plan_manage/launch/run_in_px4sim.launch | 1 + src/planner/plan_manage/src/traj_server.cpp | 10 +++++----- src/rc_demo/launch/rc_flight.launch | 2 +- 4 files changed, 9 insertions(+), 7 deletions(-) diff --git a/src/planner/plan_manage/launch/run_in_flight.launch b/src/planner/plan_manage/launch/run_in_flight.launch index d19fe121..5863d5b7 100644 --- a/src/planner/plan_manage/launch/run_in_flight.launch +++ b/src/planner/plan_manage/launch/run_in_flight.launch @@ -6,7 +6,7 @@ - + @@ -84,6 +84,7 @@ + diff --git a/src/planner/plan_manage/launch/run_in_px4sim.launch b/src/planner/plan_manage/launch/run_in_px4sim.launch index 5dd277b0..aab65174 100644 --- a/src/planner/plan_manage/launch/run_in_px4sim.launch +++ b/src/planner/plan_manage/launch/run_in_px4sim.launch @@ -80,6 +80,7 @@ + diff --git a/src/planner/plan_manage/src/traj_server.cpp b/src/planner/plan_manage/src/traj_server.cpp index 95370c14..109696e7 100644 --- a/src/planner/plan_manage/src/traj_server.cpp +++ b/src/planner/plan_manage/src/traj_server.cpp @@ -42,6 +42,7 @@ bool use_velocity_control_, enable_rotate_head_, use_waypoint_yaw_; double forward_length_; double max_vel_; +double z_offset_; void bsplineCallback(ego_planner::BsplineConstPtr msg) { @@ -188,15 +189,13 @@ void cmdCallback(const ros::TimerEvent &e) /* check receive traj_ before calculate target */ if (receive_traj_) { - Eigen::Vector3d z_offset = Eigen::Vector3d(0, 0, 0); + Eigen::Vector3d z_offset = Eigen::Vector3d(0, 0, z_offset_); Eigen::Vector3d closestPoint = findClosestPoint(odom_pos_, traj_[0]); Eigen::Vector3d refTarget = findClosestPoint(closestPoint, traj_[0], forward_length_) + z_offset; Eigen::Vector3d refTarget_forward = findClosestPoint(closestPoint, traj_[0], forward_length_ * 2.5) + z_offset; Eigen::Vector3d sub_vector(refTarget.x() - odom_pos_.x(), refTarget.y() - odom_pos_.y(), refTarget.z() - odom_pos_.z()); Eigen::Vector3d sub_vector_xy(refTarget.x() - odom_pos_.x(), refTarget.y() - odom_pos_.y(), 0); - - // std::cout << "use_waypoint_yaw_ : " << (use_waypoint_yaw_?"true":"false") << ", "; - // std::cout << "cur_waypoint_initialized_ : " << (cur_waypoint_initialized_?"true":"false") << std::endl; + double ref_yaw = last_yaw_; if (enable_rotate_head_) { ref_yaw = atan2(refTarget_forward.y() - odom_pos_.y(), refTarget_forward.x() - odom_pos_.x()); @@ -259,7 +258,7 @@ void cmdCallback(const ros::TimerEvent &e) } else if (sub_vector.norm() < 0.1) { msg.type_mask |= msg.IGNORE_YAW; receive_traj_ = false; - last_odom_pos_ = odom_pos_; + last_odom_pos_ = refTarget; } else { msg.type_mask |= msg.IGNORE_YAW; last_odom_pos_ = odom_pos_; @@ -358,6 +357,7 @@ int main(int argc, char **argv) nh.param("traj_server/enable_rotate_head", enable_rotate_head_, true); nh.param("traj_server/use_waypoint_yaw", use_waypoint_yaw_, false); nh.param("traj_server/max_vel", max_vel_, 1.0); + nh.param("traj_server/z_offset", z_offset_, 0.0); ros::Duration(1.0).sleep(); diff --git a/src/rc_demo/launch/rc_flight.launch b/src/rc_demo/launch/rc_flight.launch index 72c01d05..bf4dc3f2 100644 --- a/src/rc_demo/launch/rc_flight.launch +++ b/src/rc_demo/launch/rc_flight.launch @@ -2,7 +2,7 @@ - + From 7c35a4797f337314d709b7c237ca641df5fc44b3 Mon Sep 17 00:00:00 2001 From: JeiKeiLim Date: Thu, 6 Oct 2022 14:39:46 +0900 Subject: [PATCH 35/48] Modify to check angle difference between goal and current pose. --- src/rc_demo/rc_flight.py | 4 ++- src/rc_demo/utils.py | 65 ++++++++++++++++++++++++++++++---------- 2 files changed, 52 insertions(+), 17 deletions(-) diff --git a/src/rc_demo/rc_flight.py b/src/rc_demo/rc_flight.py index 0c0511d6..2c4798ff 100755 --- a/src/rc_demo/rc_flight.py +++ b/src/rc_demo/rc_flight.py @@ -52,7 +52,9 @@ def __init__(self, 'waypoint_1': [[0.0, 0.0, 1.2]], } - print(f"[TARGET WAYPOINT]: {self.target_waypoint}") + # Debugging log + if False: + rospy.loginfo(f"[TARGET WAYPOINT]: {self.target_waypoint}") def _start_waypoint(self) -> None: if self.is_waypoint_working: diff --git a/src/rc_demo/utils.py b/src/rc_demo/utils.py index 3279dac2..68d0b4b0 100644 --- a/src/rc_demo/utils.py +++ b/src/rc_demo/utils.py @@ -26,10 +26,20 @@ def get_quaternion_from_euler(roll: float, pitch: float, yaw: float) -> Tuple[fl Return: qx, qy, qz, qw: The orientation in quaternion [x,y,z,w] format """ - qx = math.sin(roll/2) * math.cos(pitch/2) * math.cos(yaw/2) - math.cos(roll/2) * math.sin(pitch/2) * math.sin(yaw/2) - qy = math.cos(roll/2) * math.sin(pitch/2) * math.cos(yaw/2) + math.sin(roll/2) * math.cos(pitch/2) * math.sin(yaw/2) - qz = math.cos(roll/2) * math.cos(pitch/2) * math.sin(yaw/2) - math.sin(roll/2) * math.sin(pitch/2) * math.cos(yaw/2) - qw = math.cos(roll/2) * math.cos(pitch/2) * math.cos(yaw/2) + math.sin(roll/2) * math.sin(pitch/2) * math.sin(yaw/2) + sin_roll = math.sin(roll/2) + cos_roll = math.cos(roll/2) + + sin_pitch = math.sin(pitch/2) + cos_pitch = math.cos(pitch/2) + + sin_yaw = math.sin(yaw/2) + cos_yaw = math.cos(yaw/2) + + + qx = sin_roll * cos_pitch * cos_yaw - cos_roll * sin_pitch * sin_yaw + qy = cos_roll * sin_pitch * cos_yaw + sin_roll * cos_pitch * sin_yaw + qz = cos_roll * cos_pitch * sin_yaw - sin_roll * sin_pitch * cos_yaw + qw = cos_roll * cos_pitch * cos_yaw + sin_roll * sin_pitch * sin_yaw return (qx, qy, qz, qw) @@ -102,8 +112,10 @@ def __init__(self, distance_tolerance: float = 0.5) -> None: self.current_position = Odometry() """Target position is the designated waypoint""" self.target_position = PoseStamped() + self.target_position.pose.orientation.w = 1.0 """Goal position is modified waypoint from ego-planner""" self.goal_position = Pose() + self.goal_position.orientation.w = 1.0 self.is_moving = False self.waypoints = [] self.send_seq = 0 @@ -130,13 +142,30 @@ def _check_current_location(self) -> bool: if len(self.waypoints) < 1: return False - # target_point = self.target_position.pose.position - target_point = self.goal_position.position - current_point = self.current_position.pose.pose.position - - distance = compute_distance(target_point, current_point) - - if distance < self.distance_tolerance: + # goal_point = self.target_position.pose + goal_point = self.goal_position + current_point = self.current_position.pose.pose + + distance = compute_distance(goal_point.position, current_point.position) + + rpy_goal = quaternion_to_euler(goal_point.orientation.x, + goal_point.orientation.y, + goal_point.orientation.w, + goal_point.orientation.z) + rpy_current = quaternion_to_euler(current_point.orientation.x, + current_point.orientation.y, + current_point.orientation.w, + current_point.orientation.z) + yaw_diff = min((2 * math.pi) - abs(rpy_goal[2]-rpy_current[2]), abs(rpy_goal[2]-rpy_current[2])) + yaw_diff = math.degrees(yaw_diff) + + # Debug logging + if False: + rospy.loginfo(f"[Distance to the goal] {distance:.3f}m, [Angle difference]: {yaw_diff:.3f} degree") + rospy.loginfo(f"[ GOAL POSE] x: {goal_point.position.x:8.3f}, y: {goal_point.position.y:8.3f}, z: {goal_point.position.z:8.3f}, R: {rpy_goal[0]:8.3f}, P: {rpy_goal[1]:8.3f}, Y: {rpy_goal[2]:8.3f}") + rospy.loginfo(f"[CURRENT POSE] x: {current_point.position.x:8.3f}, y: {current_point.position.y:8.3f}, z: {current_point.position.z:8.3f}, R: {rpy_current[0]:8.3f}, P: {rpy_current[1]:8.3f}, Y: {rpy_current[2]:8.3f}") + + if distance < self.distance_tolerance and yaw_diff < (self.distance_tolerance * 10): return True return False @@ -181,10 +210,14 @@ def _loop_send_waypoint(self, event = None) -> None: msg.header.frame_id = "map" msg.poses.append(pose) self.target_position = pose - self.goal_position = pose.pose + self.goal_position.position = pose.pose.position + self.goal_position.orientation = pose.pose.orientation + + # Debugging log + if False: + rospy.loginfo(f"[CURRENT WAYPOINT]: \n{pose.pose.position}") + rospy.loginfo(f"[ ORIENTATION ]: \n{pose.pose.orientation}") - print(f"[CURRENT WAYPOINT]: \n{pose.pose.position}") - print(f"[ ORIENTATION ]: \n{pose.pose.orientation}") self.waypoint_pub.publish(msg) self.is_moving = True @@ -199,9 +232,9 @@ def callback_goal_point(self, msg: Marker) -> None: if msg.id != 1: return - # if compute_distance(msg.pose.position, self.target_position.pose.position) < (self.distance_tolerance * 2): + if compute_distance(msg.pose.position, self.target_position.pose.position) < (self.distance_tolerance * 2): # self.target_position.pose.position = msg.pose.position - self.goal_position.position = msg.pose.position + self.goal_position.position = msg.pose.position class MAVROSCommander: From 3834308300d98a4e324d9b22cc3187e5bc529adc Mon Sep 17 00:00:00 2001 From: svin3 Date: Thu, 6 Oct 2022 14:40:11 +0900 Subject: [PATCH 36/48] fix hover-rotate cmd --- src/planner/plan_manage/src/traj_server.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/planner/plan_manage/src/traj_server.cpp b/src/planner/plan_manage/src/traj_server.cpp index 109696e7..b5b7b9ef 100644 --- a/src/planner/plan_manage/src/traj_server.cpp +++ b/src/planner/plan_manage/src/traj_server.cpp @@ -266,7 +266,6 @@ void cmdCallback(const ros::TimerEvent &e) } else { msg.type_mask |= msg.IGNORE_VX | msg.IGNORE_VY | msg.IGNORE_VZ; - last_yaw_ = odom_yaw_; Eigen::Vector3d waypoint_pose(control_.position.x, control_.position.y, 0); @@ -284,6 +283,7 @@ void cmdCallback(const ros::TimerEvent &e) if (abs(waypoint_yaw) > 0.1) { msg.type_mask |= msg.IGNORE_YAW; msg.yaw_rate = waypoint_yaw; + last_yaw_ = odom_yaw_; } else { msg.type_mask |= msg.IGNORE_YAW_RATE; msg.yaw = last_yaw_; From 0a94ba5a6d9a61274222650eb0c1b1db26c3f109 Mon Sep 17 00:00:00 2001 From: JeiKeiLim Date: Thu, 6 Oct 2022 15:13:24 +0900 Subject: [PATCH 37/48] Modify rc_flight.launch and run_in_flight.launch to have angle_tolerance parameter. Change arrive_tolerance to distance_tolerance. --- .../plan_manage/launch/run_in_flight.launch | 5 +++-- src/rc_demo/launch/rc_flight.launch | 6 +++-- src/rc_demo/rc_flight.py | 10 +++++---- src/rc_demo/utils.py | 22 ++++++++++++++----- 4 files changed, 30 insertions(+), 13 deletions(-) diff --git a/src/planner/plan_manage/launch/run_in_flight.launch b/src/planner/plan_manage/launch/run_in_flight.launch index 5863d5b7..6d9df7d8 100644 --- a/src/planner/plan_manage/launch/run_in_flight.launch +++ b/src/planner/plan_manage/launch/run_in_flight.launch @@ -6,7 +6,8 @@ - + + @@ -99,7 +100,7 @@ - + diff --git a/src/rc_demo/launch/rc_flight.launch b/src/rc_demo/launch/rc_flight.launch index bf4dc3f2..1ff43366 100644 --- a/src/rc_demo/launch/rc_flight.launch +++ b/src/rc_demo/launch/rc_flight.launch @@ -2,12 +2,14 @@ - + + - + + diff --git a/src/rc_demo/rc_flight.py b/src/rc_demo/rc_flight.py index 2c4798ff..7c34f15b 100755 --- a/src/rc_demo/rc_flight.py +++ b/src/rc_demo/rc_flight.py @@ -20,14 +20,15 @@ def __init__(self, use_rc_control: bool = True, waypoint_path: str = "", target_waypoint: str = "waypoint_0", - arrive_tolerance: float = 0.5) -> None: + distance_tolerance: float = 0.5, + angle_tolerance: float = 10.0) -> None: self.use_rc_control = use_rc_control self.target_waypoint = target_waypoint self.rc_sub = rospy.Subscriber("/mavros/rc/in", RCIn, self.callback_rc_in) self.waypoint_trigger_sub = rospy.Subscriber("/custom/waypoint_trigger", String, self.callback_waypoint_trigger) self.control_msg = ControlMessage() - self.waypoint_msg = WaypointMessage(arrive_tolerance) + self.waypoint_msg = WaypointMessage(distance_tolerance, angle_tolerance) self.is_waypoint_working = False @@ -148,9 +149,10 @@ def callback_rc_in(self, msg: RCIn) -> None: use_rc_control = rospy.get_param('~rc_control', False) waypoint_path = rospy.get_param('~waypoint', "") target_waypoint = rospy.get_param('~target_waypoint', "waypoint_0") - arrive_tolerance = rospy.get_param('~arrive_tolerance', 0.5) + distance_tolerance = rospy.get_param('~distance_tolerance', 0.5) + angle_tolerance = rospy.get_param('~angle_tolerance', 10) - rc_handler = RCHandler(use_rc_control, waypoint_path, target_waypoint, arrive_tolerance) + rc_handler = RCHandler(use_rc_control, waypoint_path, target_waypoint, distance_tolerance, angle_tolerance) rospy.spin() diff --git a/src/rc_demo/utils.py b/src/rc_demo/utils.py index 68d0b4b0..c07f02c1 100644 --- a/src/rc_demo/utils.py +++ b/src/rc_demo/utils.py @@ -95,7 +95,7 @@ def send_control(self, roll: float, pitch: float, yaw: float, throttle: float) - class WaypointMessage: """Send waypoint message via MavROS""" - def __init__(self, distance_tolerance: float = 0.5) -> None: + def __init__(self, distance_tolerance: float = 0.5, angle_tolerance: float = 10.0) -> None: self.waypoint_pub = rospy.Publisher('/waypoint_generator/waypoints', Path, queue_size=1) @@ -108,6 +108,7 @@ def __init__(self, distance_tolerance: float = 0.5) -> None: self._timer_send_waypoint = rospy.Timer(rospy.Duration(3.0), self._loop_send_waypoint) self.distance_tolerance = distance_tolerance + self.angle_tolerance = angle_tolerance self.current_position = Odometry() """Target position is the designated waypoint""" @@ -165,7 +166,7 @@ def _check_current_location(self) -> bool: rospy.loginfo(f"[ GOAL POSE] x: {goal_point.position.x:8.3f}, y: {goal_point.position.y:8.3f}, z: {goal_point.position.z:8.3f}, R: {rpy_goal[0]:8.3f}, P: {rpy_goal[1]:8.3f}, Y: {rpy_goal[2]:8.3f}") rospy.loginfo(f"[CURRENT POSE] x: {current_point.position.x:8.3f}, y: {current_point.position.y:8.3f}, z: {current_point.position.z:8.3f}, R: {rpy_current[0]:8.3f}, P: {rpy_current[1]:8.3f}, Y: {rpy_current[2]:8.3f}") - if distance < self.distance_tolerance and yaw_diff < (self.distance_tolerance * 10): + if distance < self.distance_tolerance and yaw_diff < self.angle_tolerance: return True return False @@ -192,7 +193,16 @@ def _loop_send_waypoint(self, event = None) -> None: pose.pose.position.x = target_point[0] pose.pose.position.y = target_point[1] pose.pose.position.z = target_point[2] - pose.pose.orientation = self.current_position.pose.pose.orientation + if self.current_position.pose.pose.orientation.x == 0 and \ + self.current_position.pose.pose.orientation.y == 0 and \ + self.current_position.pose.pose.orientation.z == 0 and \ + self.current_position.pose.pose.orientation.w == 0: + pose.pose.orientation.w = 1.0 + else: + pose.pose.orientation.x = self.current_position.pose.pose.orientation.x + pose.pose.orientation.y = self.current_position.pose.pose.orientation.y + pose.pose.orientation.z = self.current_position.pose.pose.orientation.z + pose.pose.orientation.w = self.current_position.pose.pose.orientation.w if len(target_point) > 3: quaternion = get_quaternion_from_euler(0, 0, math.radians(-target_point[3]+90)) @@ -225,8 +235,10 @@ def callback_odometry(self, msg: Odometry) -> None: self.current_position = msg if self.target_position.pose.position.x == 0 and self.target_position.pose.position.y == 0 and self.target_position.pose.position.z == 0: - self.goal_position = msg.pose.pose - self.target_position.pose = msg.pose.pose + self.goal_position.position = msg.pose.pose.position + self.goal_position.orientation = msg.pose.pose.orientation + self.target_position.pose.position = msg.pose.pose.position + self.target_position.pose.orientation = msg.pose.pose.orientation def callback_goal_point(self, msg: Marker) -> None: if msg.id != 1: From c746c1ad8f85727337e9514080d17c520c653f67 Mon Sep 17 00:00:00 2001 From: Haneol Kim Date: Thu, 6 Oct 2022 15:40:19 +0900 Subject: [PATCH 38/48] Add yaw check option in rc_flight and waypoint to hallway --- src/rc_demo/launch/rc_flight.launch | 2 ++ src/rc_demo/rc_flight.py | 8 +++++--- src/rc_demo/res/waypoints.yaml | 13 +++++++------ src/rc_demo/utils.py | 15 ++++++++++++--- 4 files changed, 26 insertions(+), 12 deletions(-) diff --git a/src/rc_demo/launch/rc_flight.launch b/src/rc_demo/launch/rc_flight.launch index bf4dc3f2..823229d5 100644 --- a/src/rc_demo/launch/rc_flight.launch +++ b/src/rc_demo/launch/rc_flight.launch @@ -3,11 +3,13 @@ + + diff --git a/src/rc_demo/rc_flight.py b/src/rc_demo/rc_flight.py index 2c4798ff..906e005e 100755 --- a/src/rc_demo/rc_flight.py +++ b/src/rc_demo/rc_flight.py @@ -20,14 +20,15 @@ def __init__(self, use_rc_control: bool = True, waypoint_path: str = "", target_waypoint: str = "waypoint_0", - arrive_tolerance: float = 0.5) -> None: + arrive_tolerance: float = 0.5, + check_yaw: bool = True) -> None: self.use_rc_control = use_rc_control self.target_waypoint = target_waypoint self.rc_sub = rospy.Subscriber("/mavros/rc/in", RCIn, self.callback_rc_in) self.waypoint_trigger_sub = rospy.Subscriber("/custom/waypoint_trigger", String, self.callback_waypoint_trigger) self.control_msg = ControlMessage() - self.waypoint_msg = WaypointMessage(arrive_tolerance) + self.waypoint_msg = WaypointMessage(arrive_tolerance, check_yaw) self.is_waypoint_working = False @@ -149,8 +150,9 @@ def callback_rc_in(self, msg: RCIn) -> None: waypoint_path = rospy.get_param('~waypoint', "") target_waypoint = rospy.get_param('~target_waypoint', "waypoint_0") arrive_tolerance = rospy.get_param('~arrive_tolerance', 0.5) + check_yaw = rospy.get_param("~check_yaw", True) - rc_handler = RCHandler(use_rc_control, waypoint_path, target_waypoint, arrive_tolerance) + rc_handler = RCHandler(use_rc_control, waypoint_path, target_waypoint, arrive_tolerance, check_yaw) rospy.spin() diff --git a/src/rc_demo/res/waypoints.yaml b/src/rc_demo/res/waypoints.yaml index f6c779f6..a45a6eee 100644 --- a/src/rc_demo/res/waypoints.yaml +++ b/src/rc_demo/res/waypoints.yaml @@ -51,15 +51,16 @@ waypoint_indoor: waypoint_indoor2: [ - [1, 0, 1.0], # hover + [1, -0.5, 0.7], # hover [3.5, -0.9, 0.7], # Right of stair [4.4, 1.3, 0.8], # In front of stair [9.7, 1.6, 3.4], # Middle of stair left [9.7, -0.4, 3.6], # Middle of stair right [1.7, 0.2, 5.7], # 2nd floor - [1.7, -0.8, 5.7, 90], # In front of first door - [1.8, -4.07, 5.7], # first door - [1.8, -4.07, 5.7, 0], # first door left turn - [1.8, -4.07, 5.7, 90], # first door - [3.58 , -7.87, 5.7, 180], # in front of second door + [1.7, -0.8, 5.0, 90], # In front of first door + [1.8, -4.07, 5.0], # first room + [1.8, -4.07, 5.0, 0], # first room left turn + [1.8, -4.07, 5.0, 90], # first door + [3.58 , -7.87, 5.0, 180], # in front of second door + [-0.07, -7.44, 5.0] # after second door, hallway ] diff --git a/src/rc_demo/utils.py b/src/rc_demo/utils.py index 68d0b4b0..d4a20acf 100644 --- a/src/rc_demo/utils.py +++ b/src/rc_demo/utils.py @@ -95,7 +95,11 @@ def send_control(self, roll: float, pitch: float, yaw: float, throttle: float) - class WaypointMessage: """Send waypoint message via MavROS""" - def __init__(self, distance_tolerance: float = 0.5) -> None: + def __init__( + self, + distance_tolerance: float = 0.5, + check_yaw: bool = True, + ) -> None: self.waypoint_pub = rospy.Publisher('/waypoint_generator/waypoints', Path, queue_size=1) @@ -108,6 +112,7 @@ def __init__(self, distance_tolerance: float = 0.5) -> None: self._timer_send_waypoint = rospy.Timer(rospy.Duration(3.0), self._loop_send_waypoint) self.distance_tolerance = distance_tolerance + self.check_yaw = check_yaw self.current_position = Odometry() """Target position is the designated waypoint""" @@ -165,8 +170,12 @@ def _check_current_location(self) -> bool: rospy.loginfo(f"[ GOAL POSE] x: {goal_point.position.x:8.3f}, y: {goal_point.position.y:8.3f}, z: {goal_point.position.z:8.3f}, R: {rpy_goal[0]:8.3f}, P: {rpy_goal[1]:8.3f}, Y: {rpy_goal[2]:8.3f}") rospy.loginfo(f"[CURRENT POSE] x: {current_point.position.x:8.3f}, y: {current_point.position.y:8.3f}, z: {current_point.position.z:8.3f}, R: {rpy_current[0]:8.3f}, P: {rpy_current[1]:8.3f}, Y: {rpy_current[2]:8.3f}") - if distance < self.distance_tolerance and yaw_diff < (self.distance_tolerance * 10): - return True + if self.check_yaw: + if distance < self.distance_tolerance and yaw_diff < (self.distance_tolerance * 10): + return True + else: + if distance < self.distance_tolerance: + return True return False From 367f40593e91a2e69dd957ce1d1e2c1b1781d387 Mon Sep 17 00:00:00 2001 From: svin3 Date: Thu, 6 Oct 2022 16:23:54 +0900 Subject: [PATCH 39/48] [rc_flight] update offset parameters --- src/rc_demo/launch/rc_flight.launch | 8 ++++++++ src/rc_demo/rc_flight.py | 14 ++++++++++++-- src/rc_demo/utils.py | 24 ++++++++++++++++++++---- 3 files changed, 40 insertions(+), 6 deletions(-) diff --git a/src/rc_demo/launch/rc_flight.launch b/src/rc_demo/launch/rc_flight.launch index 1ff43366..dd78f92d 100644 --- a/src/rc_demo/launch/rc_flight.launch +++ b/src/rc_demo/launch/rc_flight.launch @@ -4,6 +4,10 @@ + + + + @@ -11,5 +15,9 @@ + + + + diff --git a/src/rc_demo/rc_flight.py b/src/rc_demo/rc_flight.py index 7c34f15b..0338a8e7 100755 --- a/src/rc_demo/rc_flight.py +++ b/src/rc_demo/rc_flight.py @@ -21,7 +21,11 @@ def __init__(self, waypoint_path: str = "", target_waypoint: str = "waypoint_0", distance_tolerance: float = 0.5, - angle_tolerance: float = 10.0) -> None: + angle_tolerance: float = 10.0, + offset_x: float = 0.0, + offset_y: float = 0.0, + offset_z: float = 0.0, + offset_Y: float = 0.0) -> None: self.use_rc_control = use_rc_control self.target_waypoint = target_waypoint @@ -29,6 +33,7 @@ def __init__(self, self.waypoint_trigger_sub = rospy.Subscriber("/custom/waypoint_trigger", String, self.callback_waypoint_trigger) self.control_msg = ControlMessage() self.waypoint_msg = WaypointMessage(distance_tolerance, angle_tolerance) + self.waypoint_msg.set_offset(offset_x, offset_y, offset_z, offset_Y) self.is_waypoint_working = False @@ -151,8 +156,13 @@ def callback_rc_in(self, msg: RCIn) -> None: target_waypoint = rospy.get_param('~target_waypoint', "waypoint_0") distance_tolerance = rospy.get_param('~distance_tolerance', 0.5) angle_tolerance = rospy.get_param('~angle_tolerance', 10) + offset_x = rospy.get_param('~offset_x', 0) + offset_y = rospy.get_param('~offset_y', 0) + offset_z = rospy.get_param('~offset_z', 0) + offset_Y = rospy.get_param('~offset_Y', 0) - rc_handler = RCHandler(use_rc_control, waypoint_path, target_waypoint, distance_tolerance, angle_tolerance) + + rc_handler = RCHandler(use_rc_control, waypoint_path, target_waypoint, distance_tolerance, angle_tolerance, offset_x, offset_y, offset_z, offset_Y) rospy.spin() diff --git a/src/rc_demo/utils.py b/src/rc_demo/utils.py index c07f02c1..e5aaa174 100644 --- a/src/rc_demo/utils.py +++ b/src/rc_demo/utils.py @@ -121,6 +121,17 @@ def __init__(self, distance_tolerance: float = 0.5, angle_tolerance: float = 10. self.waypoints = [] self.send_seq = 0 + self.offset_x = 0.0 + self.offset_y = 0.0 + self.offset_z = 0.0 + self.offset_Y = 0.0 + + def set_offset(self, x: float, y: float, z: float, Y: float) -> None: + self.offset_x = x + self.offset_y = y + self.offset_z = z + self.offset_Y = Y + def clear_waypoint(self) -> None: self.waypoints.clear() self.target_position.pose = self.current_position.pose.pose @@ -186,13 +197,18 @@ def _loop_send_waypoint(self, event = None) -> None: if target_point == None: target_point = self.waypoints.pop(0) + # apply offset + temp_x = target_point[0] - self.offset_x + temp_y = target_point[1] - self.offset_y + temp_rad = self.offset_Y / 180.0 * math.pi + pose = PoseStamped() pose.header.seq = self.send_seq pose.header.stamp = rospy.Time.now() pose.header.frame_id = "map" - pose.pose.position.x = target_point[0] - pose.pose.position.y = target_point[1] - pose.pose.position.z = target_point[2] + pose.pose.position.x = temp_x * math.cos(temp_rad) - temp_y * math.sin(temp_rad) + pose.pose.position.y = temp_x * math.sin(temp_rad) + temp_y * math.cos(temp_rad) + pose.pose.position.z = target_point[2] - self.offset_z if self.current_position.pose.pose.orientation.x == 0 and \ self.current_position.pose.pose.orientation.y == 0 and \ self.current_position.pose.pose.orientation.z == 0 and \ @@ -205,7 +221,7 @@ def _loop_send_waypoint(self, event = None) -> None: pose.pose.orientation.w = self.current_position.pose.pose.orientation.w if len(target_point) > 3: - quaternion = get_quaternion_from_euler(0, 0, math.radians(-target_point[3]+90)) + quaternion = get_quaternion_from_euler(0, 0, math.radians(-(target_point[3] - self.offset_Y)+90)) pose.pose.orientation.x = quaternion[0] pose.pose.orientation.y = quaternion[1] pose.pose.orientation.z = quaternion[2] From f04b27ed4c2396448074381ff005650904e6d34d Mon Sep 17 00:00:00 2001 From: JeiKeiLim Date: Thu, 6 Oct 2022 16:27:44 +0900 Subject: [PATCH 40/48] Add indoor waypoint for gazebo simulation. --- src/rc_demo/res/waypoints.yaml | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/src/rc_demo/res/waypoints.yaml b/src/rc_demo/res/waypoints.yaml index a45a6eee..081c974a 100644 --- a/src/rc_demo/res/waypoints.yaml +++ b/src/rc_demo/res/waypoints.yaml @@ -64,3 +64,17 @@ waypoint_indoor2: [3.58 , -7.87, 5.0, 180], # in front of second door [-0.07, -7.44, 5.0] # after second door, hallway ] + +waypoint_indoor2_sim: +# This way point has been generated from + [ + [0, 0, 1.6, 0], # hover + [0.9, 3.5, 0.7], # Right of stair + [-1.3, 4.4, 0.8], # In front of stair + [-1.6, 10.5, 3.4], # Middle of stair left + [0.5, 10.5, 4.6, 90], # Middle of stair right + [0.5, 3.0, 6.7, 180], # 2nd floor + [4.07, 3.5, 7.0, 90], # first door + [4.07, 3.5, 7.0, 0], # head rotation + [5.07, 3.5, 7.0, 180], # before 2nd door + ] From 615e63a0f5dad915f06fb3f772232b1d967e6a1d Mon Sep 17 00:00:00 2001 From: svin3 Date: Thu, 6 Oct 2022 16:31:07 +0900 Subject: [PATCH 41/48] [rc_flight] update offset parameters --- src/rc_demo/launch/rc_flight.launch | 8 +++++--- src/rc_demo/rc_flight.py | 11 ++++++++--- src/rc_demo/res/waypoints.yaml | 13 +++++++------ src/rc_demo/utils.py | 16 +++++++++++++--- 4 files changed, 33 insertions(+), 15 deletions(-) diff --git a/src/rc_demo/launch/rc_flight.launch b/src/rc_demo/launch/rc_flight.launch index dd78f92d..66ff9a65 100644 --- a/src/rc_demo/launch/rc_flight.launch +++ b/src/rc_demo/launch/rc_flight.launch @@ -4,10 +4,11 @@ - - + + - + + @@ -19,5 +20,6 @@ + diff --git a/src/rc_demo/rc_flight.py b/src/rc_demo/rc_flight.py index 0338a8e7..7ffb9e3d 100755 --- a/src/rc_demo/rc_flight.py +++ b/src/rc_demo/rc_flight.py @@ -25,14 +25,15 @@ def __init__(self, offset_x: float = 0.0, offset_y: float = 0.0, offset_z: float = 0.0, - offset_Y: float = 0.0) -> None: + offset_Y: float = 0.0, + check_yaw: bool = True) -> None: self.use_rc_control = use_rc_control self.target_waypoint = target_waypoint self.rc_sub = rospy.Subscriber("/mavros/rc/in", RCIn, self.callback_rc_in) self.waypoint_trigger_sub = rospy.Subscriber("/custom/waypoint_trigger", String, self.callback_waypoint_trigger) self.control_msg = ControlMessage() - self.waypoint_msg = WaypointMessage(distance_tolerance, angle_tolerance) + self.waypoint_msg = WaypointMessage(distance_tolerance, angle_tolerance, check_yaw) self.waypoint_msg.set_offset(offset_x, offset_y, offset_z, offset_Y) self.is_waypoint_working = False @@ -156,13 +157,17 @@ def callback_rc_in(self, msg: RCIn) -> None: target_waypoint = rospy.get_param('~target_waypoint', "waypoint_0") distance_tolerance = rospy.get_param('~distance_tolerance', 0.5) angle_tolerance = rospy.get_param('~angle_tolerance', 10) + offset_x = rospy.get_param('~offset_x', 0) offset_y = rospy.get_param('~offset_y', 0) offset_z = rospy.get_param('~offset_z', 0) offset_Y = rospy.get_param('~offset_Y', 0) + check_yaw = rospy.get_param("~check_yaw", True) + distance_tolerance = rospy.get_param('~distance_tolerance', 0.5) + angle_tolerance = rospy.get_param('~angle_tolerance', 10) - rc_handler = RCHandler(use_rc_control, waypoint_path, target_waypoint, distance_tolerance, angle_tolerance, offset_x, offset_y, offset_z, offset_Y) + rc_handler = RCHandler(use_rc_control, waypoint_path, target_waypoint, distance_tolerance, angle_tolerance, offset_x, offset_y, offset_z, offset_Y, check_yaw) rospy.spin() diff --git a/src/rc_demo/res/waypoints.yaml b/src/rc_demo/res/waypoints.yaml index f6c779f6..a45a6eee 100644 --- a/src/rc_demo/res/waypoints.yaml +++ b/src/rc_demo/res/waypoints.yaml @@ -51,15 +51,16 @@ waypoint_indoor: waypoint_indoor2: [ - [1, 0, 1.0], # hover + [1, -0.5, 0.7], # hover [3.5, -0.9, 0.7], # Right of stair [4.4, 1.3, 0.8], # In front of stair [9.7, 1.6, 3.4], # Middle of stair left [9.7, -0.4, 3.6], # Middle of stair right [1.7, 0.2, 5.7], # 2nd floor - [1.7, -0.8, 5.7, 90], # In front of first door - [1.8, -4.07, 5.7], # first door - [1.8, -4.07, 5.7, 0], # first door left turn - [1.8, -4.07, 5.7, 90], # first door - [3.58 , -7.87, 5.7, 180], # in front of second door + [1.7, -0.8, 5.0, 90], # In front of first door + [1.8, -4.07, 5.0], # first room + [1.8, -4.07, 5.0, 0], # first room left turn + [1.8, -4.07, 5.0, 90], # first door + [3.58 , -7.87, 5.0, 180], # in front of second door + [-0.07, -7.44, 5.0] # after second door, hallway ] diff --git a/src/rc_demo/utils.py b/src/rc_demo/utils.py index e5aaa174..610dccde 100644 --- a/src/rc_demo/utils.py +++ b/src/rc_demo/utils.py @@ -95,7 +95,12 @@ def send_control(self, roll: float, pitch: float, yaw: float, throttle: float) - class WaypointMessage: """Send waypoint message via MavROS""" - def __init__(self, distance_tolerance: float = 0.5, angle_tolerance: float = 10.0) -> None: + def __init__( + self, + distance_tolerance: float = 0.5, + angle_tolerance: float = 10.0, + check_yaw: bool = True, + ) -> None: self.waypoint_pub = rospy.Publisher('/waypoint_generator/waypoints', Path, queue_size=1) @@ -108,6 +113,7 @@ def __init__(self, distance_tolerance: float = 0.5, angle_tolerance: float = 10. self._timer_send_waypoint = rospy.Timer(rospy.Duration(3.0), self._loop_send_waypoint) self.distance_tolerance = distance_tolerance + self.check_yaw = check_yaw self.angle_tolerance = angle_tolerance self.current_position = Odometry() @@ -177,8 +183,12 @@ def _check_current_location(self) -> bool: rospy.loginfo(f"[ GOAL POSE] x: {goal_point.position.x:8.3f}, y: {goal_point.position.y:8.3f}, z: {goal_point.position.z:8.3f}, R: {rpy_goal[0]:8.3f}, P: {rpy_goal[1]:8.3f}, Y: {rpy_goal[2]:8.3f}") rospy.loginfo(f"[CURRENT POSE] x: {current_point.position.x:8.3f}, y: {current_point.position.y:8.3f}, z: {current_point.position.z:8.3f}, R: {rpy_current[0]:8.3f}, P: {rpy_current[1]:8.3f}, Y: {rpy_current[2]:8.3f}") - if distance < self.distance_tolerance and yaw_diff < self.angle_tolerance: - return True + if self.check_yaw: + if distance < self.distance_tolerance and yaw_diff < self.angle_tolerance: + return True + else: + if distance < self.distance_tolerance: + return True return False From 06ee015e6cb36aeb0dd6b656d6d11f4da2917dad Mon Sep 17 00:00:00 2001 From: JeiKeiLim Date: Thu, 6 Oct 2022 18:59:43 +0900 Subject: [PATCH 42/48] Add indoor waypoint for simulation environment. --- src/rc_demo/res/waypoints.yaml | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/src/rc_demo/res/waypoints.yaml b/src/rc_demo/res/waypoints.yaml index a45a6eee..cd360174 100644 --- a/src/rc_demo/res/waypoints.yaml +++ b/src/rc_demo/res/waypoints.yaml @@ -64,3 +64,21 @@ waypoint_indoor2: [3.58 , -7.87, 5.0, 180], # in front of second door [-0.07, -7.44, 5.0] # after second door, hallway ] + +waypoint_indoor2_sim: +# This way point has been generated from roslaunch simulation_environment mavros_posix_sitl_lidar.launch world:=$HOME/custom_worlds/add_competition_map.world x:=6 y:=11 z:=2 Y:=0 vehicle:=px4vision lidar:=ouster + [ + [0, 0, 1.6, 0], # hover + [0.9, 3.5, 0.7], # Right of stair + [-1.3, 4.4, 0.8], # In front of stair + [-1.6, 10.5, 3.4], # Middle of stair left + [1.0, 10.5, 4.6, 90], # Middle of stair right + [1.0, 3.0, 6.7, 180], # 2nd floor + [4.07, 3.5, 7.0, 90], # first door + [4.07, 3.5, 7.0, 0], # head rotation + [7.7, 3.0, 7.0, 180], # before 2nd door + [7.7, 0.7, 7.0, 180], # after 2nd door + [7.3, 0.7, 7.0, 90], # after 2nd door + [7.3, 0.7, 7.0, 270], # after 2nd door + ] + From b48a005c6c8937851aced7728e06ef2df00eac5a Mon Sep 17 00:00:00 2001 From: svin3 Date: Wed, 12 Oct 2022 17:04:17 +0900 Subject: [PATCH 43/48] update forward_length param --- src/planner/plan_manage/launch/run_in_flight.launch | 1 + src/planner/plan_manage/launch/run_in_px4sim.launch | 1 + src/planner/plan_manage/src/traj_server.cpp | 2 +- 3 files changed, 3 insertions(+), 1 deletion(-) diff --git a/src/planner/plan_manage/launch/run_in_flight.launch b/src/planner/plan_manage/launch/run_in_flight.launch index 6d9df7d8..79e2279b 100644 --- a/src/planner/plan_manage/launch/run_in_flight.launch +++ b/src/planner/plan_manage/launch/run_in_flight.launch @@ -86,6 +86,7 @@ + diff --git a/src/planner/plan_manage/launch/run_in_px4sim.launch b/src/planner/plan_manage/launch/run_in_px4sim.launch index aab65174..3b81f8c5 100644 --- a/src/planner/plan_manage/launch/run_in_px4sim.launch +++ b/src/planner/plan_manage/launch/run_in_px4sim.launch @@ -81,6 +81,7 @@ + diff --git a/src/planner/plan_manage/src/traj_server.cpp b/src/planner/plan_manage/src/traj_server.cpp index b5b7b9ef..60584258 100644 --- a/src/planner/plan_manage/src/traj_server.cpp +++ b/src/planner/plan_manage/src/traj_server.cpp @@ -252,7 +252,7 @@ void cmdCallback(const ros::TimerEvent &e) msg.velocity.z = refVel.z(); } - if (sub_vector_xy.norm() > 0.5) { + if (sub_vector_xy.norm() > forward_length_ * 0.6) { msg.type_mask |= msg.IGNORE_YAW_RATE; msg.yaw = ref_yaw; } else if (sub_vector.norm() < 0.1) { From e921e9939e937a7848d57ee23e74b638ee55847d Mon Sep 17 00:00:00 2001 From: Haneol Kim Date: Wed, 12 Oct 2022 18:43:53 +0900 Subject: [PATCH 44/48] Add merged waypoints --- .../plan_manage/launch/run_in_px4sim.launch | 1 - src/rc_demo/res/waypoints.yaml | 49 +++++++++++++++++-- 2 files changed, 45 insertions(+), 5 deletions(-) diff --git a/src/planner/plan_manage/launch/run_in_px4sim.launch b/src/planner/plan_manage/launch/run_in_px4sim.launch index 3b81f8c5..aab65174 100644 --- a/src/planner/plan_manage/launch/run_in_px4sim.launch +++ b/src/planner/plan_manage/launch/run_in_px4sim.launch @@ -81,7 +81,6 @@ - diff --git a/src/rc_demo/res/waypoints.yaml b/src/rc_demo/res/waypoints.yaml index cd360174..8835bbe6 100644 --- a/src/rc_demo/res/waypoints.yaml +++ b/src/rc_demo/res/waypoints.yaml @@ -27,9 +27,10 @@ waypoint_outdoor: [10.70119367623, 2.13588568156, 3.21656869656], # before wall [11.0070259193, 2.08961550795, 2.43913609922], # wall [13.4681804363, -0.525301411499, 1.50505519304], # before net - [18.8681804363, -0.525301411499, 1.50505519304], # after net - [18.8681804363, 0.0, 1.50505519304], # after net - # [25, 0.00, 1.8005] # car + # [18.8681804363, -0.525301411499, 1.50505519304], # after net + [18.5681804363, -0.525301411499, 1.50505519304], # after net + # [18.8681804363, 0.0, 1.50505519304], # after net + [25, 0.00, 1.8005] # car ] waypoint_indoor: @@ -40,7 +41,7 @@ waypoint_indoor: # yaw 180: -Y direction # yaw 270: -X direction # If yaw is not given, previous yaw is maintained. - [0, 0, 1.0, 135], # hover + [0, 0, 1.0, 1.35], # hover [3.9, -0.9, 0.7], # Right of stair [4.4, 1.3, 0.8, 45], # In front of stair [9.9, 1.6, 3.4], # Middle of stair left @@ -51,6 +52,7 @@ waypoint_indoor: waypoint_indoor2: [ + [0.0, 0.0, 1], # takeoff (for simulation) [1, -0.5, 0.7], # hover [3.5, -0.9, 0.7], # Right of stair [4.4, 1.3, 0.8], # In front of stair @@ -82,3 +84,42 @@ waypoint_indoor2_sim: [7.3, 0.7, 7.0, 270], # after 2nd door ] +waypoint_merged: + [ + [0, 0, 1.59992909121], # hover + [3.72308462946, 2.16384181223, 1.59992909121], # first plants + # [7.20119367623, 2.13588568156, 2.61656869656], # second plants -> for simulation + [7.20119367623, 2.13588568156, 1.71656869656], # second plants -> for rosbag + [8.10119367623, 2.13588568156, 3.21656869656], # before wall + [10.70119367623, 2.13588568156, 3.21656869656], # before wall + [11.0070259193, 2.08961550795, 2.43913609922], # wall + [13.4681804363, -0.525301411499, 1.50505519304], # before net + [18.5681804363, -0.525301411499, 1.50505519304], # after net + [25, 0.00, 1.8005], # car + [28, 1.00, 1.5, 0], + [28, 5.0, 1.5], + [28.9, 8.5, 1], # Right of stair + [26.7, 9.4, 1], # In front of stair + # [26.7, 15.2, 3.4], # Middle of stair left y: 1.47 + # [28.7, 15.2, 3.7], # Middle of stair right + # [28, 5.7, 5.4], # 2nd floor + # [5.7, 28.8, 5.2, 90], # In front of first door + # [28, 5.7, 5.4], # 2nd floor + # Return to base + # [28.7, 15.2, 3.7], # Middle of stair right + # [26.7, 15.2, 3.4], # Middle of stair left y: 1.47 + # [26.7, 9.4, 1], # In front of stair + [28.9, 8.5, 1], # Right of stair + [28, 5.0, 1.5], + [28, 1.00, 1.5, 270], + [25, 0.00, 1.8005], # car + [18.5681804363, -0.525301411499, 1.50505519304], # after net + [13.4681804363, -0.525301411499, 1.50505519304], # before net + [11.0070259193, 2.08961550795, 2.43913609922], # wall + [10.70119367623, 2.13588568156, 3.21656869656], # before wall + [8.10119367623, 2.13588568156, 3.21656869656], # before wall + [7.20119367623, 2.13588568156, 1.71656869656], # second plants -> for rosbag + [3.72308462946, 2.16384181223, 1.59992909121], # first plants + [0, 0, 1.59992909121], # hover + [0, 0, -1], # landing + ] From 36ced76b2c47405e9471b898c94744f1d34b4100 Mon Sep 17 00:00:00 2001 From: Haneol Kim Date: Wed, 12 Oct 2022 18:45:35 +0900 Subject: [PATCH 45/48] Add merged waypoints --- src/planner/plan_manage/launch/run_in_px4sim.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/planner/plan_manage/launch/run_in_px4sim.launch b/src/planner/plan_manage/launch/run_in_px4sim.launch index aab65174..5a881b2d 100644 --- a/src/planner/plan_manage/launch/run_in_px4sim.launch +++ b/src/planner/plan_manage/launch/run_in_px4sim.launch @@ -79,7 +79,7 @@ - + From 703cb1d039258e1a6c0464b885e028aebf9ac421 Mon Sep 17 00:00:00 2001 From: Haneol Kim Date: Wed, 12 Oct 2022 18:48:23 +0900 Subject: [PATCH 46/48] [FIX] fix max_vel in run_in_px4sim --- src/planner/plan_manage/launch/run_in_px4sim.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/planner/plan_manage/launch/run_in_px4sim.launch b/src/planner/plan_manage/launch/run_in_px4sim.launch index 5a881b2d..aab65174 100644 --- a/src/planner/plan_manage/launch/run_in_px4sim.launch +++ b/src/planner/plan_manage/launch/run_in_px4sim.launch @@ -79,7 +79,7 @@ - + From 7c0589e02da927864062c02b6c2f0f782c2669d1 Mon Sep 17 00:00:00 2001 From: Haneol Kim Date: Thu, 13 Oct 2022 16:58:35 +0900 Subject: [PATCH 47/48] Add landing --- src/rc_demo/res/waypoints.yaml | 20 +++++++++++--------- src/rc_demo/utils.py | 6 ++++++ 2 files changed, 17 insertions(+), 9 deletions(-) diff --git a/src/rc_demo/res/waypoints.yaml b/src/rc_demo/res/waypoints.yaml index 8835bbe6..43110766 100644 --- a/src/rc_demo/res/waypoints.yaml +++ b/src/rc_demo/res/waypoints.yaml @@ -13,7 +13,9 @@ waypoint_0: waypoint_1: [ - [0.0, 0.0, 0.0], + [0.0, 0.0, 1.0], + [0.0, 0.0, 1.0, 180], + [0.0, 0.0, -1.0], ] waypoint_outdoor: @@ -100,15 +102,15 @@ waypoint_merged: [28, 5.0, 1.5], [28.9, 8.5, 1], # Right of stair [26.7, 9.4, 1], # In front of stair - # [26.7, 15.2, 3.4], # Middle of stair left y: 1.47 - # [28.7, 15.2, 3.7], # Middle of stair right - # [28, 5.7, 5.4], # 2nd floor - # [5.7, 28.8, 5.2, 90], # In front of first door - # [28, 5.7, 5.4], # 2nd floor + [26.7, 15.2, 3.4], # Middle of stair left y: 1.47 + [28.7, 15.2, 3.7], # Middle of stair right + [28, 5.7, 5.4], # 2nd floor + [5.7, 28.8, 5.2, 90], # In front of first door + [28, 5.7, 5.4], # 2nd floor # Return to base - # [28.7, 15.2, 3.7], # Middle of stair right - # [26.7, 15.2, 3.4], # Middle of stair left y: 1.47 - # [26.7, 9.4, 1], # In front of stair + [28.7, 15.2, 3.7], # Middle of stair right + [26.7, 15.2, 3.4], # Middle of stair left y: 1.47 + [26.7, 9.4, 1], # In front of stair [28.9, 8.5, 1], # Right of stair [28, 5.0, 1.5], [28, 1.00, 1.5, 270], diff --git a/src/rc_demo/utils.py b/src/rc_demo/utils.py index 610dccde..6674b46f 100644 --- a/src/rc_demo/utils.py +++ b/src/rc_demo/utils.py @@ -132,6 +132,8 @@ def __init__( self.offset_z = 0.0 self.offset_Y = 0.0 + self.commander = MAVROSCommander() + def set_offset(self, x: float, y: float, z: float, Y: float) -> None: self.offset_x = x self.offset_y = y @@ -202,6 +204,10 @@ def _loop_send_waypoint(self, event = None) -> None: if len(self.waypoints) < 1: target_point = self.target_position + if target_point.pose.position.z == -1: + """change to land""" + self.commander.set_mode("AUTO.LAND") + self.send_seq += 1 if target_point == None: From 521aa81a7c89bf47059a363df0d8b3f5addbeebb Mon Sep 17 00:00:00 2001 From: JeiKeiLim Date: Mon, 17 Oct 2022 19:14:39 +0900 Subject: [PATCH 48/48] Fix to ignore offset of waypoint when waypoint is (0, 0) --- src/planner/plan_manage/launch/run_in_flight.launch | 13 ++++++++++--- src/rc_demo/utils.py | 4 ++++ 2 files changed, 14 insertions(+), 3 deletions(-) diff --git a/src/planner/plan_manage/launch/run_in_flight.launch b/src/planner/plan_manage/launch/run_in_flight.launch index 79e2279b..7da5b485 100644 --- a/src/planner/plan_manage/launch/run_in_flight.launch +++ b/src/planner/plan_manage/launch/run_in_flight.launch @@ -8,6 +8,10 @@ + + + + @@ -102,6 +106,10 @@ + + + + @@ -115,7 +123,6 @@ --> - + + diff --git a/src/rc_demo/utils.py b/src/rc_demo/utils.py index 6674b46f..d3e79333 100644 --- a/src/rc_demo/utils.py +++ b/src/rc_demo/utils.py @@ -218,6 +218,10 @@ def _loop_send_waypoint(self, event = None) -> None: temp_y = target_point[1] - self.offset_y temp_rad = self.offset_Y / 180.0 * math.pi + # Ignore offset if waypoint is (0, 0) + if target_point[0] == 0 and target_point[1] == 0: + temp_x = temp_y = 0 + pose = PoseStamped() pose.header.seq = self.send_seq pose.header.stamp = rospy.Time.now()