diff --git a/ansible/roles/build/tasks/main.yml b/ansible/roles/build/tasks/main.yml index 86c7f3797..2dd9c216e 100644 --- a/ansible/roles/build/tasks/main.yml +++ b/ansible/roles/build/tasks/main.yml @@ -89,8 +89,6 @@ - ros-{{ ros_distro }}-rosbash - libbullet-dev - libglfw3-dev - - xorg-dev - - libx11-xcb-dev - libnl-3-dev - libnl-route-3-dev diff --git a/launch/simulator.launch b/launch/simulator.launch index 78ecfbae8..4e0aa26d1 100644 --- a/launch/simulator.launch +++ b/launch/simulator.launch @@ -1,16 +1,20 @@ - + - - - - + - + + + + - + - - \ No newline at end of file + + + + + + diff --git a/scripts/build_dawn.sh b/scripts/build_dawn.sh index c741e9fde..0fdfefd49 100755 --- a/scripts/build_dawn.sh +++ b/scripts/build_dawn.sh @@ -18,5 +18,7 @@ CC=clang CXX=clang++ cmake \ -D DAWN_ENABLE_INSTALL=ON \ -D BUILD_SHARED_LIBS=ON \ -D DAWN_USE_GLFW=OFF \ - -D TINT_BUILD_CMD_TOOLS=OFF + -D TINT_BUILD_CMD_TOOLS=OFF \ + -D TINT_BUILD_DOCS=OFF \ + -D TINT_BUILD_TESTS=OFF cmake --build out/Release diff --git a/src/navigation/approach_post.py b/src/navigation/approach_post.py index b3ebfb676..83bf74490 100644 --- a/src/navigation/approach_post.py +++ b/src/navigation/approach_post.py @@ -24,7 +24,7 @@ def on_loop(self, context) -> State: :param ud: State machine user data :return: Next state """ - fid_pos = context.env.current_fid_pos() + fid_pos = context.env.current_tag_pos() if fid_pos is None: return search.SearchState() @@ -38,7 +38,7 @@ def on_loop(self, context) -> State: ) if arrived: context.env.arrived_at_post = True - context.env.last_post_location = context.env.current_fid_pos(odom_override=False) + context.env.last_post_location = context.env.current_tag_pos(odom_override=False) context.course.increment_waypoint() return waypoint.WaypointState() context.rover.send_drive_command(cmd_vel) diff --git a/src/navigation/context.py b/src/navigation/context.py index 0edfb9cc3..962cbc700 100644 --- a/src/navigation/context.py +++ b/src/navigation/context.py @@ -55,50 +55,50 @@ def get_pose_with_time(self) -> Tuple[SE3, Time]: class Environment: """ Context class to represent the rover's environment - Information such as locations of fiducials or obstacles + Information such as locations of tags or obstacles """ ctx: Context - NO_FIDUCIAL: ClassVar[int] = -1 + NO_TAG: ClassVar[int] = -1 arrived_at_post: bool = False last_post_location: Optional[np.ndarray] = None - def get_fid_pos(self, fid_id: int, in_odom_frame: bool = True) -> Optional[np.ndarray]: + def get_tag_pos(self, tag_id: int, in_odom_frame: bool = True) -> Optional[np.ndarray]: """ - Retrieves the pose of the given fiducial ID from the TF tree in the odom frame + Retrieves the pose of the given tag ID from the TF tree in the odom frame if in_odom_frame is True otherwise in the world frame if it exists and is more recent than TAG_EXPIRATION_TIME_SECONDS, otherwise returns None """ try: parent_frame = self.ctx.odom_frame if in_odom_frame else self.ctx.world_frame fid_pose, time = SE3.from_tf_time( - self.ctx.tf_buffer, parent_frame=parent_frame, child_frame=f"fiducial{fid_id}" + self.ctx.tf_buffer, parent_frame=parent_frame, child_frame=f"fiducial{tag_id}" ) now = rospy.Time.now() if now.to_sec() - time.to_sec() >= TAG_EXPIRATION_TIME_SECONDS: - print(f"TAG EXPIRED {fid_id}!") + rospy.logwarn(f"Tag expired: {tag_id}!") return None except ( tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException, - ) as e: + ): return None return fid_pose.position - def current_fid_pos(self, odom_override: bool = True) -> Optional[np.ndarray]: + def current_tag_pos(self, odom_override: bool = True) -> Optional[np.ndarray]: """ - Retrieves the position of the current fiducial (and we are looking for it) + Retrieves the position of the current tag (and we are looking for it) :param: odom_override if false will force it to be in the map frame """ assert self.ctx.course in_odom = self.ctx.use_odom and odom_override current_waypoint = self.ctx.course.current_waypoint() if current_waypoint is None: - print("CURRENT WAYPOINT IS NONE") + rospy.logwarn("Current waypoint is empty!") return None - return self.get_fid_pos(current_waypoint.tag_id, in_odom) + return self.get_tag_pos(current_waypoint.tag_id, in_odom) @dataclass @@ -128,8 +128,7 @@ def current_waypoint(self) -> Optional[Waypoint]: """ Returns the currently active waypoint - :param ud: State machine user data - :return: Next waypoint to reach if we have an active course + :return: Next waypoint to reach if we have an active course """ if self.course_data is None or self.waypoint_index >= len(self.course_data.waypoints): return None diff --git a/src/navigation/failure_identification/failure_identification.py b/src/navigation/failure_identification/failure_identification.py index 844aca39a..56d60935b 100755 --- a/src/navigation/failure_identification/failure_identification.py +++ b/src/navigation/failure_identification/failure_identification.py @@ -69,7 +69,6 @@ def __init__(self): + wheel_velocity_variables + command_variables ) - print(self.cols) self._df = pd.DataFrame(columns=self.cols) self.watchdog = WatchDog() self.path_name = None # type: ignore @@ -80,7 +79,7 @@ def write_to_csv(self): """ if self.actively_collecting and self.data_collecting_mode: # append to csv if csv exists else write to csv - rospy.loginfo("writing to file") + rospy.loginfo("Writing to file") if self.path_name is None: path = Path.cwd() / "failure_data" path.mkdir(exist_ok=True) @@ -91,7 +90,7 @@ def write_to_csv(self): self.path_name = path self._df.to_csv(path) - rospy.loginfo("===== failure data written to csv =====") + rospy.loginfo("===== Failure data written to CSV =====") else: self._df.to_csv(self.path_name, mode="a", header=False) @@ -163,7 +162,7 @@ def update(self, nav_status: StateMachineStateUpdate, drive_status: MotorsStatus cur_row[f"wheel_{wheel_num}_velocity"] = drive_status.joint_states.velocity[wheel_num] # update the data frame with the cur row - self._df = pd.concat([self._df, pd.DataFrame([cur_row])]) + self._df = pd.concat([self._df, pd.DataFrame([cur_row])]) if self._df.size else pd.DataFrame([cur_row]) if len(self._df) == DATAFRAME_MAX_SIZE: self.write_to_csv() diff --git a/src/navigation/failure_identification/watchdog.py b/src/navigation/failure_identification/watchdog.py index a3adc0d0c..8d4924b51 100644 --- a/src/navigation/failure_identification/watchdog.py +++ b/src/navigation/failure_identification/watchdog.py @@ -1,5 +1,6 @@ from typing import Tuple +import rospy import numpy as np import pandas as pd @@ -50,18 +51,18 @@ def check_angular_stuck(self, delta_time, delta_rot, dataframe) -> bool: ) linear_are_all_zero = dataframe["cmd_vel_x"].eq(0).all() angular_are_non_zero = dataframe["cmd_vel_twist"].ne(0).all() - print( - f"turn all same sign: f{turn_are_all_same_sign}, linear_are_all_zero: f{linear_are_all_zero}, angular are non zero: f{angular_are_non_zero}" + rospy.logdebug( + f"Turn all same sign: f{turn_are_all_same_sign}, linear all zero: f{linear_are_all_zero}, angular all nonzero: f{angular_are_non_zero}" ) if not turn_are_all_same_sign or not linear_are_all_zero: - print("not turning") + rospy.logdebug("Not turning") return False if not angular_are_non_zero: - print("not turning") + rospy.logdebug("Not turning") return False # check if the angular velocity is less than the threshold for the entire dataframe angular_velocity = delta_rot / delta_time - print(f"angular velocity: {angular_velocity}, thresh: {ANGULAR_THRESHOLD}") + rospy.logdebug(f"{angular_velocity=}, {ANGULAR_THRESHOLD=}") return abs(angular_velocity) < ANGULAR_THRESHOLD def check_linear_stuck(self, delta_time, delta_pos, dataframe) -> bool: @@ -70,14 +71,14 @@ def check_linear_stuck(self, delta_time, delta_pos, dataframe) -> bool: dataframe["cmd_vel_x"].apply(np.sign).eq(np.sign(dataframe["cmd_vel_x"].iloc[0])).all() ) linear_are_all_non_zero = dataframe["cmd_vel_x"].ne(0).all() - print(linear_are_all_same_sign, linear_are_all_non_zero) + rospy.logdebug(f"{linear_are_all_same_sign=}, {linear_are_all_non_zero=}") if not linear_are_all_same_sign or not linear_are_all_non_zero: - print("not driving straight") + rospy.logdebug("Not driving straight") return False # check if the average linear velocity is less than the threshold for the entire dataframe linear_velocity = delta_pos / delta_time linear_velocity = np.linalg.norm(linear_velocity) - print(linear_velocity, LINEAR_THRESHOLD) + rospy.logdebug(f"{linear_velocity=}, {LINEAR_THRESHOLD=}") return linear_velocity < LINEAR_THRESHOLD def is_stuck(self, dataframe: pd.DataFrame) -> bool: @@ -88,7 +89,7 @@ def is_stuck(self, dataframe: pd.DataFrame) -> bool: start_rot, end_rot = self.get_start_end_rotations(dataframe_sliced) # get the start and end time start_time, end_time = self.get_start_end_time(dataframe_sliced) - print(start_time, end_time) + rospy.logdebug(f"{start_time=}, {end_time=}") # get the delta time and delta position delta_time = end_time - start_time delta_pos = end_pos - start_pos @@ -96,7 +97,7 @@ def is_stuck(self, dataframe: pd.DataFrame) -> bool: delta_rot = start_rot.rot_distance_to(end_rot) # check if the rover is stuck if self.check_angular_stuck(delta_time, delta_rot, dataframe_sliced) or self.check_linear_stuck( - delta_time, delta_pos, dataframe_sliced + delta_time, delta_pos, dataframe_sliced ): return True return False diff --git a/src/navigation/post_backup.py b/src/navigation/post_backup.py index b7c51246f..07c73f420 100644 --- a/src/navigation/post_backup.py +++ b/src/navigation/post_backup.py @@ -4,6 +4,7 @@ from typing import Optional import numpy as np +import rospy import tf2_ros from shapely.geometry import Point, LineString @@ -49,7 +50,6 @@ def avoid_post_trajectory(rover_pose: SE3, post_pos: np.ndarray, waypoint_pos: n rover_pos = rover_pose.position rover_direction = rover_pose.rotation.direction_vector() - print(rover_pos, post_pos, waypoint_pos) # Converting to 2d arrays post_pos = post_pos[:2] @@ -88,7 +88,6 @@ def avoid_post_trajectory(rover_pose: SE3, post_pos: np.ndarray, waypoint_pos: n # add a z coordinate of 0 to all the coords coords = np.hstack((coords, np.zeros((coords.shape[0], 1)))) - print(coords) return AvoidPostTrajectory(coords) @@ -128,7 +127,7 @@ def on_loop(self, context) -> State: drive_back=drive_backwards, ) if arrived: - print(f"ARRIVED AT POINT {point_index}") + rospy.loginfo(f"Arrived at point indexed: {point_index}") if self.traj.increment_point(): self.traj = None return waypoint.WaypointState() @@ -141,8 +140,8 @@ def on_loop(self, context) -> State: context.rover.send_drive_command(cmd_vel) return self except ( - tf2_ros.LookupException, - tf2_ros.ConnectivityException, - tf2_ros.ExtrapolationException, + tf2_ros.LookupException, + tf2_ros.ConnectivityException, + tf2_ros.ExtrapolationException, ): return self diff --git a/src/navigation/search.py b/src/navigation/search.py index 2b0d8ee3b..25bb174dc 100644 --- a/src/navigation/search.py +++ b/src/navigation/search.py @@ -134,6 +134,6 @@ def on_loop(self, context) -> State: ) context.rover.send_drive_command(cmd_vel) - if context.env.current_fid_pos() is not None and context.course.look_for_post(): + if context.env.current_tag_pos() is not None and context.course.look_for_post(): return approach_post.ApproachPostState() return self diff --git a/src/navigation/waypoint.py b/src/navigation/waypoint.py index 0a964dc48..bba359c33 100644 --- a/src/navigation/waypoint.py +++ b/src/navigation/waypoint.py @@ -35,7 +35,7 @@ def on_loop(self, context) -> State: return post_backup.PostBackupState() if context.course.look_for_post(): - if context.env.current_fid_pos() is not None: + if context.env.current_tag_pos() is not None: return approach_post.ApproachPostState() # Attempt to find the waypoint in the TF tree and drive to it diff --git a/src/simulator/simulator.cpp b/src/simulator/simulator.cpp index f3a536b2a..2ba97f9b6 100644 --- a/src/simulator/simulator.cpp +++ b/src/simulator/simulator.cpp @@ -25,13 +25,28 @@ namespace mrover { mIkTargetPub = mNh.advertise("/arm_ik", 1); - initWindow(); + mIsHeadless = mPnh.param("headless", false); + mEnablePhysics = mIsHeadless; + { + XmlRpc::XmlRpcValue gpsLinearization; + mNh.getParam("gps_linearization", gpsLinearization); + if (gpsLinearization.getType() != XmlRpc::XmlRpcValue::TypeStruct) throw std::invalid_argument{"GPS lineraization must be a struct. Did you rosparam load a localization config file properly?"}; + + mGpsLinerizationReferencePoint = { + xmlRpcValueToTypeOrDefault(gpsLinearization, "reference_point_latitude"), + xmlRpcValueToTypeOrDefault(gpsLinearization, "reference_point_longitude"), + xmlRpcValueToTypeOrDefault(gpsLinearization, "reference_point_altitude"), + }; + mGpsLinerizationReferenceHeading = xmlRpcValueToTypeOrDefault(gpsLinearization, "reference_heading"); + } + + if (!mIsHeadless) initWindow(); initPhysics(); initRender(); - parseParams(); + initUrdfsFromParams(); twistCallback(boost::make_shared()); @@ -58,18 +73,21 @@ namespace mrover { // Note(quintin): // Apple sucks and does not support polling on a non-main thread (which we are currently on) // Instead add to ROS's global callback queue which I think will be served by the main thread + if (!mIsHeadless) { #ifdef __APPLE__ - struct PollGlfw : ros::CallbackInterface { - auto call() -> CallResult override { - glfwPollEvents(); - return Success; - } - }; - ros::getGlobalCallbackQueue()->addCallback(boost::make_shared()); + struct PollGlfw : ros::CallbackInterface { + auto call() -> CallResult override { + glfwPollEvents(); + return Success; + } + }; + ros::getGlobalCallbackQueue()->addCallback(boost::make_shared()); #else - glfwPollEvents(); + glfwPollEvents(); #endif - // glfwSetInputMode(mWindow.get(), GLFW_CURSOR, mInGui ? GLFW_CURSOR_NORMAL : GLFW_CURSOR_DISABLED); + } + // Comments this out while debugging segfaults, otherwise it captures your cursor + glfwSetInputMode(mWindow.get(), GLFW_CURSOR, mInGui ? GLFW_CURSOR_NORMAL : GLFW_CURSOR_DISABLED); mLoopProfiler.measureEvent("GLFW Events"); userControls(dt); @@ -109,4 +127,4 @@ namespace mrover { } } -} // namespace mrover +} // namespace mrover \ No newline at end of file diff --git a/src/simulator/simulator.hpp b/src/simulator/simulator.hpp index 8a1782f52..89a7a83cd 100644 --- a/src/simulator/simulator.hpp +++ b/src/simulator/simulator.hpp @@ -193,7 +193,7 @@ namespace mrover { float mLookSense = 0.004f; float mFovDegrees = 60.0f; btVector3 mGravityAcceleration{0.0f, 0.0f, -9.81f}; - bool mEnablePhysics = false; + bool mEnablePhysics{}; bool mRenderModels = true; bool mRenderWireframeColliders = false; double mPublishHammerDistanceThreshold = 4; @@ -228,6 +228,8 @@ namespace mrover { PeriodicTask mGpsTask; PeriodicTask mImuTask; + bool mIsHeadless{}; + // Rendering GlfwInstance mGlfwInstance; @@ -346,7 +348,7 @@ namespace mrover { auto initPhysics() -> void; - auto parseParams() -> void; + auto initUrdfsFromParams() -> void; auto onInit() -> void override; diff --git a/src/simulator/simulator.params.cpp b/src/simulator/simulator.params.cpp deleted file mode 100644 index 094138ea6..000000000 --- a/src/simulator/simulator.params.cpp +++ /dev/null @@ -1,40 +0,0 @@ -#include "simulator.hpp" - -namespace mrover { - - auto SimulatorNodelet::parseParams() -> void { - { - XmlRpc::XmlRpcValue objects; - mNh.getParam("objects", objects); - if (objects.getType() != XmlRpc::XmlRpcValue::TypeArray) throw std::invalid_argument{"URDFs to load must be an array. Did you rosparam load a simulator config file properly?"}; - - for (int i = 0; i < objects.size(); ++i) { // NOLINT(*-loop-convert) - XmlRpc::XmlRpcValue const& object = objects[i]; - - auto type = xmlRpcValueToTypeOrDefault(object, "type"); - auto name = xmlRpcValueToTypeOrDefault(object, "name", ""); - - NODELET_INFO_STREAM(std::format("Loading object: {} of type: {}", name, type)); - - if (type == "urdf") { - if (auto [_, was_added] = mUrdfs.try_emplace(name, *this, object); !was_added) { - throw std::invalid_argument{std::format("Duplicate object name: {}", name)}; - } - } - } - } - { - XmlRpc::XmlRpcValue gpsLinearization; - mNh.getParam("gps_linearization", gpsLinearization); - if (gpsLinearization.getType() != XmlRpc::XmlRpcValue::TypeStruct) throw std::invalid_argument{"GPS lineraization must be a struct. Did you rosparam load a localization config file properly?"}; - - mGpsLinerizationReferencePoint = { - xmlRpcValueToTypeOrDefault(gpsLinearization, "reference_point_latitude"), - xmlRpcValueToTypeOrDefault(gpsLinearization, "reference_point_longitude"), - xmlRpcValueToTypeOrDefault(gpsLinearization, "reference_point_altitude"), - }; - mGpsLinerizationReferenceHeading = xmlRpcValueToTypeOrDefault(gpsLinearization, "reference_heading"); - } - } - -} // namespace mrover diff --git a/src/simulator/simulator.render.cpp b/src/simulator/simulator.render.cpp index dfc5c6845..30543b1ef 100644 --- a/src/simulator/simulator.render.cpp +++ b/src/simulator/simulator.render.cpp @@ -248,7 +248,7 @@ namespace mrover { mWgpuInstance = wgpu::createInstance(descriptor); if (!mWgpuInstance) throw std::runtime_error("Failed to create WGPU instance"); } - { + if (!mIsHeadless) { mSurface = glfwGetWGPUSurface(mWgpuInstance, mWindow.get()); if (!mSurface) throw std::runtime_error("Failed to create WGPU surface"); } @@ -277,10 +277,11 @@ namespace mrover { mQueue = mDevice.getQueue(); if (!mQueue) throw std::runtime_error("Failed to get WGPU queue"); - int width, height; - glfwGetFramebufferSize(mWindow.get(), &width, &height); - - makeFramebuffers(width, height); + if (!mIsHeadless) { + int width, height; + glfwGetFramebufferSize(mWindow.get(), &width, &height); + makeFramebuffers(width, height); + } { wgpu::ShaderModuleWGSLDescriptor shaderSourceDescriptor; @@ -647,9 +648,6 @@ namespace mrover { } auto SimulatorNodelet::renderUpdate() -> void { - wgpu::TextureView nextTexture = mSwapChain.getCurrentTextureView(); - if (!nextTexture) throw std::runtime_error("Failed to get WGPU next texture view"); - std::array colorAttachments{}; auto& [colorAttachment, normalAttachment] = colorAttachments; colorAttachment.loadOp = wgpu::LoadOp::Clear; @@ -676,7 +674,10 @@ namespace mrover { camerasUpdate(encoder, colorAttachment, normalAttachment, depthStencilAttachment, renderPassDescriptor); - { + if (!mIsHeadless) { + wgpu::TextureView nextTexture = mSwapChain.getCurrentTextureView(); + if (!nextTexture) throw std::runtime_error("Failed to get WGPU next texture view"); + colorAttachment.view = nextTexture; normalAttachment.view = mNormalTextureView; depthStencilAttachment.view = mDepthTextureView; @@ -718,14 +719,14 @@ namespace mrover { pass.release(); bindGroup.release(); - } - nextTexture.release(); + nextTexture.release(); + } wgpu::CommandBuffer commands = encoder.finish(); mQueue.submit(commands); - mSwapChain.present(); + if (!mIsHeadless) mSwapChain.present(); // TODO(quintin): Remote duplicate code for (StereoCamera& stereoCamera: mStereoCameras) { diff --git a/src/simulator/urdf.cpp b/src/simulator/urdf.cpp index 0afaf4b46..981c40b25 100644 --- a/src/simulator/urdf.cpp +++ b/src/simulator/urdf.cpp @@ -30,6 +30,29 @@ namespace mrover { return btVector3{static_cast(inertia->ixx), static_cast(inertia->iyy), static_cast(inertia->izz)} * INERTIA_MULTIPLIER; } + auto SimulatorNodelet::initUrdfsFromParams() -> void { + { + XmlRpc::XmlRpcValue objects; + mNh.getParam("objects", objects); + if (objects.getType() != XmlRpc::XmlRpcValue::TypeArray) throw std::invalid_argument{"URDFs to load must be an array. Did you rosparam load a simulator config file properly?"}; + + for (int i = 0; i < objects.size(); ++i) { // NOLINT(*-loop-convert) + XmlRpc::XmlRpcValue const& object = objects[i]; + + auto type = xmlRpcValueToTypeOrDefault(object, "type"); + auto name = xmlRpcValueToTypeOrDefault(object, "name", ""); + + NODELET_INFO_STREAM(std::format("Loading object: {} of type: {}", name, type)); + + if (type == "urdf") { + if (auto [_, was_added] = mUrdfs.try_emplace(name, *this, object); !was_added) { + throw std::invalid_argument{std::format("Duplicate object name: {}", name)}; + } + } + } + } + } + auto URDF::makeCollisionShapeForLink(SimulatorNodelet& simulator, urdf::LinkConstSharedPtr const& link) -> btCollisionShape* { boost::container::static_vector, 4> shapes; for (urdf::CollisionSharedPtr const& collision: link->collision_array) { diff --git a/src/util/state_lib/state_machine.py b/src/util/state_lib/state_machine.py index db6c466c7..5553835fa 100644 --- a/src/util/state_lib/state_machine.py +++ b/src/util/state_lib/state_machine.py @@ -5,6 +5,7 @@ from threading import Lock, Event from typing import DefaultDict, Set, List, Callable, TypeVar, Optional, Generic +import rospy from util.state_lib.state import State, ExitState @@ -43,7 +44,7 @@ def __init__( name: str, context: ContextType, log_level: LogLevel = LogLevel.DEBUG, - logger: Callable[[str], None] = print, + logger: Callable[[str], None] = rospy.loginfo, ): self.current_state = initial_state self.state_lock = Lock() diff --git a/test/integration/integration.py b/test/integration/integration.py index 32b7c917a..6257cddb3 100755 --- a/test/integration/integration.py +++ b/test/integration/integration.py @@ -17,39 +17,53 @@ class TestIntegration(unittest.TestCase): def test_integration(self): - rospy.logdebug("Integration Test Starting") + rospy.logdebug("Autonomy integration testing starting...") rospy.init_node("integration_test") - rospy.loginfo("Integration Test Ready") + rospy.loginfo("Ready") - waypoint_in_world = SE3(position=np.array([-5.5, -5.5, 0.0])) waypoints = [ ( Waypoint(tag_id=0, type=WaypointType(val=WaypointType.NO_SEARCH)), - waypoint_in_world, + SE3(position=np.array([4, 4, 0])), ), + ( + Waypoint(tag_id=0, type=WaypointType(val=WaypointType.POST)), + SE3(position=np.array([-2, -2, 0])), + ), + ( + Waypoint(tag_id=1, type=WaypointType(val=WaypointType.POST)), + SE3(position=np.array([11, -10, 0])), + ) ] - - publish_waypoints([convert_waypoint_to_gps(waypoint) for waypoint in waypoints]) + publish_waypoints(list(map(convert_waypoint_to_gps, waypoints))) tf_buffer = tf2_ros.Buffer() tf2_ros.TransformListener(tf_buffer) - has_reached_target = False - while not rospy.is_shutdown(): - try: - rover_in_world = SE3.from_tf_tree(tf_buffer, parent_frame="map", child_frame="base_link") - distance_to_target = waypoint_in_world.pos_distance_to(rover_in_world) - rospy.logdebug(distance_to_target) - if distance_to_target < COMPLETION_TOLERANCE: - has_reached_target = True - if has_reached_target: - rospy.signal_shutdown("Finished test") - break - except tf2_ros.TransformException as exception: - pass + rate = rospy.Rate(10) + for waypoint in waypoints: + rospy.loginfo(f"Moving to waypoint: {waypoint}") + + def distance_to_current_waypoint(): + try: + rover_in_world = SE3.from_tf_tree(tf_buffer, parent_frame="map", child_frame="base_link") + waypoint_in_world = waypoint[1] + distance_to_target = waypoint_in_world.pos_distance_to(rover_in_world) + rospy.logdebug(distance_to_target) + return distance_to_target + except tf2_ros.TransformException: + rospy.logwarn_throttle(1, "Transform exception encountered") + return float("inf") + + while distance_to_current_waypoint() > COMPLETION_TOLERANCE: + rospy.loginfo(f"Distance: {distance_to_current_waypoint()}") + rate.sleep() + + rospy.loginfo("Waypoint reached") + rospy.signal_shutdown("Test Complete") if __name__ == "__main__": diff --git a/test/integration/integration.test b/test/integration/integration.test index 28724c7bf..7bddd212b 100644 --- a/test/integration/integration.test +++ b/test/integration/integration.test @@ -3,20 +3,10 @@ drive to waypoint functionality. --> - - + + + + - - - - - - - - - - - - - + diff --git a/urdf/meshes/arm_b.fbx b/urdf/meshes/arm_b.fbx index e9360724a..59968f67e 100644 --- a/urdf/meshes/arm_b.fbx +++ b/urdf/meshes/arm_b.fbx @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:0578a7fe0557e7bc86e3949cc748275386e5a472d564367a00f722f0060d47c1 -size 1127196 +oid sha256:27acf7d3fe472656b65d6185876002487b03a3aa6f26aea2bc2c49525dd5df3e +size 1128780 diff --git a/urdf/meshes/arm_c.fbx b/urdf/meshes/arm_c.fbx index 5fbded9a7..37c8aa17a 100644 --- a/urdf/meshes/arm_c.fbx +++ b/urdf/meshes/arm_c.fbx @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:e3bad4fb02574c6faf02cac83665d7a48972640301d0ff3089d1a7200f9b81ff -size 883884 +oid sha256:14f152298e48eb526c1d28c96d3342a8e9ae0e449a5824026bb9bc36c07a5a6e +size 885340 diff --git a/urdf/meshes/rover_left_bogie.fbx b/urdf/meshes/rover_left_bogie.fbx index 4d3a9e550..a50ee8540 100644 --- a/urdf/meshes/rover_left_bogie.fbx +++ b/urdf/meshes/rover_left_bogie.fbx @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:ad295bd7c739fd5b8e4d05f02db3494db37253b4315c59b60e06942fb93e3a8f -size 1569308 +oid sha256:3c5ecacbf35e5a24e0f6530972d579c2171fd67f87fa727960630e2cd697e47e +size 1574524 diff --git a/urdf/meshes/rover_left_rocker.fbx b/urdf/meshes/rover_left_rocker.fbx index dcfad7ae4..c0af1b801 100644 --- a/urdf/meshes/rover_left_rocker.fbx +++ b/urdf/meshes/rover_left_rocker.fbx @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:70a8a1a16c7223473ea44751b1a42d2f5e2cbfe41e312cdf1172ccfa0f8c8bcc -size 1062556 +oid sha256:7da666d555f7b32538a016f6cceb1bb45f625f0b6ae15c0a43e0eea4c753c4e0 +size 1064220 diff --git a/urdf/meshes/rover_right_bogie.fbx b/urdf/meshes/rover_right_bogie.fbx index 93b721e5a..3c161339f 100644 --- a/urdf/meshes/rover_right_bogie.fbx +++ b/urdf/meshes/rover_right_bogie.fbx @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:607783822c09899857fe0e556d6f8241130f3fbae96d12356ba4beb8f79bad06 -size 1567468 +oid sha256:2329d7c9272175888328413b929e51a7896a103a701adea225281b1ecaebb80c +size 1572668 diff --git a/urdf/meshes/rover_right_rocker.fbx b/urdf/meshes/rover_right_rocker.fbx index 73f8b8f0e..28cc1dc5d 100644 --- a/urdf/meshes/rover_right_rocker.fbx +++ b/urdf/meshes/rover_right_rocker.fbx @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:f90f0f99210eea2963f2ac4a257ff8f908f6906b82dc4bd57fa61f590786da24 -size 1062780 +oid sha256:ce1307454a58a91b5bbf431cfa4c1bf289b62164595ed5c7d54b4af259f701b0 +size 1064460 diff --git a/urdf/staging/rover.blend b/urdf/staging/rover.blend index 7b627847d..798e742f3 100644 --- a/urdf/staging/rover.blend +++ b/urdf/staging/rover.blend @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:4f8a338ae84b84b07cf1a66941771f6a4f94c902ba904bbf8a51e8e0f833d43b -size 23795628 +oid sha256:59d66d9b143897000f0e51c419bdee3b3a6a686e916a87ddb4c4a7a3f2697d95 +size 25108020