diff --git a/wild_visual_navigation_jackal/CMakeLists.txt b/wild_visual_navigation_jackal/CMakeLists.txt index b303635d..3b1676a2 100644 --- a/wild_visual_navigation_jackal/CMakeLists.txt +++ b/wild_visual_navigation_jackal/CMakeLists.txt @@ -21,6 +21,7 @@ install(DIRECTORY launch Media worlds ############ ## Python ## ############ -catkin_install_python(PROGRAMS scripts/jackal_state_converter_node.py +catkin_install_python(PROGRAMS scripts/carrot_follower.py + scripts/jackal_state_converter_node.py scripts/gazebo_world_publisher.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) \ No newline at end of file diff --git a/wild_visual_navigation_jackal/config/rviz/open_source.rviz b/wild_visual_navigation_jackal/config/rviz/open_source.rviz index 9a1976be..0c7fe4c9 100644 --- a/wild_visual_navigation_jackal/config/rviz/open_source.rviz +++ b/wild_visual_navigation_jackal/config/rviz/open_source.rviz @@ -7,7 +7,6 @@ Panels: - /Global Options1 - /TF1/Tree1 - /World1/Status1 - - /Robot Info1 - /Wild Visual Navigation1 - /Wild Visual Navigation1/Self Supervision1 - /Wild Visual Navigation1/Cameras Resized1 @@ -65,6 +64,8 @@ Visualization Manager: Frame Timeout: 1000 Frames: All Enabled: false + base: + Value: true base_link: Value: true chassis_link: @@ -97,6 +98,8 @@ Visualization Manager: Value: true rear_right_wheel_link: Value: true + world: + Value: true Marker Alpha: 1 Marker Scale: 0.5 Name: TF @@ -104,6 +107,8 @@ Visualization Manager: Show Axes: true Show Names: true Tree: + base: + {} odom: base_link: chassis_link: @@ -130,6 +135,8 @@ Visualization Manager: {} rear_right_wheel_link: {} + world: + {} Update Interval: 0 Value: true - Class: rviz/Marker @@ -795,11 +802,27 @@ Visualization Manager: Value: false Enabled: true Name: GridMaps + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Class: rviz/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Name: Pose + Queue Size: 10 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Shape: Arrow + Topic: /move_base_simple/goal + Unreliable: false + Value: true Enabled: true Global Options: Background Color: 255; 255; 255 Default Light: true - Fixed Frame: odom + Fixed Frame: world Frame Rate: 30 Name: root Tools: @@ -823,7 +846,7 @@ Visualization Manager: Views: Current: Class: rviz/ThirdPersonFollower - Distance: 19.5626220703125 + Distance: 47.735103607177734 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -831,17 +854,17 @@ Visualization Manager: Value: false Field of View: 0.7853981852531433 Focal Point: - X: 6.84149169921875 - Y: -0.8593425750732422 - Z: 7.152557373046875e-07 + X: 6.462728023529053 + Y: -0.07755613327026367 + Z: 9.5367431640625e-07 Focal Shape Fixed Size: false Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.6603981852531433 + Pitch: 0.7597965598106384 Target Frame: base - Yaw: 5.903587341308594 + Yaw: 6.278591632843018 Saved: ~ Window Geometry: AB Wide Angle Front: @@ -863,7 +886,7 @@ Window Geometry: collapsed: false Learning Mask: collapsed: false - QMainWindow State: 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 + QMainWindow State: 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 R Confidence Overlay: collapsed: false R Traversability Overlay: diff --git a/wild_visual_navigation_jackal/launch/sim.launch b/wild_visual_navigation_jackal/launch/sim.launch index 8eeab1f5..a8442eca 100644 --- a/wild_visual_navigation_jackal/launch/sim.launch +++ b/wild_visual_navigation_jackal/launch/sim.launch @@ -8,7 +8,7 @@ - + @@ -29,7 +29,9 @@ - - + + + + diff --git a/wild_visual_navigation_jackal/scripts/carrot_follower.py b/wild_visual_navigation_jackal/scripts/carrot_follower.py new file mode 100644 index 00000000..91e23b27 --- /dev/null +++ b/wild_visual_navigation_jackal/scripts/carrot_follower.py @@ -0,0 +1,89 @@ +#!/usr/bin/python3 +# +# Copyright (c) 2022-2024, ETH Zurich, Matias Mattamala, Jonas Frey. +# All rights reserved. Licensed under the MIT license. +# See LICENSE file in the project root for details. +# +# This script implements a simple carrot follower scheme. +# Given a 2D Nav Goal from Rviz, it implements a simple P-control law +# in x, y and yaw to reach the goal. +# Control is done in the "world" frame (see gazebo_world_publisher) +# so there is no state estimation drift for simplicity + +from geometry_msgs.msg import Twist, PoseStamped +from gazebo_msgs.msg import LinkStates +import rospy +import math +import numpy as np +import tf.transformations as tr + + +goal_x, goal_y = (None, None) + +MAX_LINEAR_VEL = 0.5 +MAX_ANGULAR_VEL = 0.7 +GAIN_LINEAR = 1.0 +GAIN_ANGULAR = 1.0 +DIST_THR = 0.1 # 10 cms + + +def wrap_angle(angle): + angle = np.fmod(angle + np.pi, 2 * np.pi) + if angle < 0: + angle = angle + 2 * np.pi + return angle - np.pi + + +def msg_to_se2(pose): + x = pose.position.x + y = pose.position.y + eul = tr.euler_from_quaternion( + np.array([pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w]) + ) + yaw = eul[2] + return x, y, yaw + + +def goal_callback(goal_msg): + rospy.loginfo("New goal received") + global goal_x, goal_y + goal_x, goal_y, _ = msg_to_se2(goal_msg.pose) + + +def gazebo_callback(msg): + global goal_x, goal_y + robot_x, robot_y, robot_yaw = msg_to_se2(msg.pose[1]) + if goal_x is None or goal_y is None: + return + compute_cmd(goal_x, goal_y, robot_x, robot_y, robot_yaw) + + +def compute_cmd(goal_x, goal_y, robot_x, robot_y, robot_yaw): + if goal_x is None or goal_y is None: + return + + # Get angular difference + yaw_diff = wrap_angle(math.atan2(goal_y - robot_y, goal_x - robot_x) - robot_yaw) + # Get distance to goal + dist_diff = math.sqrt(((goal_y - robot_y) ** 2) + (goal_x - robot_x) ** 2) + + twist_cmd = Twist() + if dist_diff > DIST_THR: + twist_cmd.angular.z = np.clip(GAIN_ANGULAR * yaw_diff, -MAX_ANGULAR_VEL, MAX_ANGULAR_VEL) + twist_cmd.linear.x = np.clip(GAIN_LINEAR * dist_diff, -MAX_LINEAR_VEL, MAX_LINEAR_VEL) + else: + twist_cmd.angular.z = 0.0 + twist_cmd.linear.x = 0.0 + + # Compute control law + cmd_pub.publish(twist_cmd) + + +if __name__ == "__main__": + rospy.init_node("jackal_carrot_follower") + gazebo_sub = rospy.Subscriber("/gazebo/link_states/", LinkStates, gazebo_callback, queue_size=5) + goal_sub = rospy.Subscriber("/move_base_simple/goal", PoseStamped, goal_callback, queue_size=5) + + cmd_pub = rospy.Publisher("/cmd_vel", Twist, queue_size=5) + rospy.loginfo("[jackal_carrot_follower] ready") + rospy.spin() diff --git a/wild_visual_navigation_jackal/scripts/gazebo_world_publisher.py b/wild_visual_navigation_jackal/scripts/gazebo_world_publisher.py index 42b4086b..00038861 100644 --- a/wild_visual_navigation_jackal/scripts/gazebo_world_publisher.py +++ b/wild_visual_navigation_jackal/scripts/gazebo_world_publisher.py @@ -4,13 +4,75 @@ # All rights reserved. Licensed under the MIT license. # See LICENSE file in the project root for details. # +# This script publishes the simulation scene from the COLLADA (.dae) file +# To be visualized on RViz. It also publishes the "world" frame, as a child +# of the "base_link" frame, using the relative transformation between +# both obtained from gazebo from visualization_msgs.msg import Marker +from gazebo_msgs.msg import LinkStates +from geometry_msgs.msg import Pose, Transform, TransformStamped + import rospkg import rospy +import tf2_ros +import numpy as np +import tf.transformations as tr + + +def msg_to_se3(msg): + """Conversion from geometric ROS messages into SE(3) + Based on Jarvis Schultz's: https://answers.ros.org/question/332407/transformstamped-to-transformation-matrix-python/ + + @param msg: Message to transform. Acceptable types - C{geometry_msgs/Pose}, C{geometry_msgs/PoseStamped}, + C{geometry_msgs/Transform}, or C{geometry_msgs/TransformStamped} + @return: a 4x4 SE(3) matrix as a numpy array + @note: Throws TypeError if we receive an incorrect type. + """ + if isinstance(msg, Pose): + p = np.array([msg.position.x, msg.position.y, msg.position.z]) + q = np.array([msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w]) + elif isinstance(msg, Transform): + p = np.array([msg.translation.x, msg.translation.y, msg.translation.z]) + q = np.array([msg.rotation.x, msg.rotation.y, msg.rotation.z, msg.rotation.w]) + elif isinstance(msg, TransformStamped): + p = np.array([msg.transform.translation.x, msg.transform.translation.y, msg.transform.translation.z]) + q = np.array( + [msg.transform.rotation.x, msg.transform.rotation.y, msg.transform.rotation.z, msg.transform.rotation.w] + ) + + norm = np.linalg.norm(q) + if np.abs(norm - 1.0) > 1e-3: + raise ValueError( + "Received un-normalized quaternion (q = {0:s} ||q|| = {1:3.6f})".format(str(q), np.linalg.norm(q)) + ) + elif np.abs(norm - 1.0) > 1e-6: + q = q / norm + g = tr.quaternion_matrix(q) + g[0:3, -1] = p + return g -def callback(event): +def gazebo_callback(msg): + stamp = rospy.Time.now() + T_world_base = msg_to_se3(msg.pose[1]) # this is the base_link pose in world frame (from gazebo) + T_base_world = np.linalg.inv(T_world_base) + + br = tf2_ros.TransformBroadcaster() + t = TransformStamped() + t.header.stamp = stamp + t.header.frame_id = "base_link" + t.child_frame_id = "world" + t.transform.translation.x = T_base_world[0, 3] + t.transform.translation.y = T_base_world[1, 3] + t.transform.translation.z = T_base_world[2, 3] + q = tr.quaternion_from_matrix(T_base_world) + t.transform.rotation.x = q[0] + t.transform.rotation.y = q[1] + t.transform.rotation.z = q[2] + t.transform.rotation.w = q[3] + br.sendTransform(t) + pub.publish(marker) @@ -22,8 +84,12 @@ def callback(event): pkg_path = rospack.get_path("wild_visual_navigation_jackal") default_model_file = f"{pkg_path}/Media/models/outdoor.dae" + # Initialize tf + tf_buffer = tf2_ros.Buffer() + tf_listener = tf2_ros.TransformListener(tf_buffer) + marker = Marker() - marker.header.frame_id = "odom" + marker.header.frame_id = "world" marker.header.stamp = rospy.Time.now() marker.id = 0 marker.ns = "world" @@ -37,10 +103,12 @@ def callback(event): marker.type = Marker.MESH_RESOURCE marker.mesh_resource = f"file://{default_model_file}" + # Set subscriber of gazebo links + gazebo_sub = rospy.Subscriber("/gazebo/link_states/", LinkStates, gazebo_callback, queue_size=10) + # Set publisher pub = rospy.Publisher("/wild_visual_navigation_jackal/simulation_world", Marker, queue_size=10) # Set timer to publish - rospy.Timer(rospy.Duration(5), callback) rospy.loginfo("[gazebo_world_publisher] Published world!") rospy.spin() diff --git a/wild_visual_navigation_jackal/worlds/outdoor.world b/wild_visual_navigation_jackal/worlds/outdoor.world index baa0a315..83ed109a 100644 --- a/wild_visual_navigation_jackal/worlds/outdoor.world +++ b/wild_visual_navigation_jackal/worlds/outdoor.world @@ -23,7 +23,7 @@ true - 3 3 -1 0 0 0 + 0 0 0 0 0 0