├── LICENSE ├── README.md ├── amr_description ├── __init__.py ├── __pycache__ │ ├── __init__.cpython-310.pyc │ └── controller.cpython-310.pyc ├── controller.py ├── destination.py ├── driver.py ├── odom_pub.py └── sensed_object.py ├── config ├── controller.yaml ├── display.rviz └── joystick.yaml ├── launch ├── display.launch.py ├── gazebo.launch.py └── joy.launch.py ├── meshes ├── assembly_1_1.stl ├── assembly_2_1.stl ├── assembly_3_1.stl ├── assembly_4_1.stl ├── base_link.stl ├── lidar_1.stl ├── wheel_1_1.stl ├── wheel_2_1.stl ├── wheel_3_1.stl └── wheel_4_1.stl ├── package.xml ├── resource └── amr_description ├── setup.cfg ├── setup.py ├── test ├── test_copyright.py ├── test_flake8.py └── test_pep257.py └── urdf ├── amr.gazebo ├── amr.trans ├── amr.xacro ├── macros.xacro └── materials.xacro /LICENSE: -------------------------------------------------------------------------------- 1 | Apache License 2 | Version 2.0, January 2004 3 | http://www.apache.org/licenses/ 4 | 5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 6 | 7 | 1. Definitions. 8 | 9 | "License" shall mean the terms and conditions for use, reproduction, 10 | and distribution as defined by Sections 1 through 9 of this document. 11 | 12 | "Licensor" shall mean the copyright owner or entity authorized by 13 | the copyright owner that is granting the License. 14 | 15 | "Legal Entity" shall mean the union of the acting entity and all 16 | other entities that control, are controlled by, or are under common 17 | control with that entity. For the purposes of this definition, 18 | "control" means (i) the power, direct or indirect, to cause the 19 | direction or management of such entity, whether by contract or 20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 21 | outstanding shares, or (iii) beneficial ownership of such entity. 22 | 23 | "You" (or "Your") shall mean an individual or Legal Entity 24 | exercising permissions granted by this License. 25 | 26 | "Source" form shall mean the preferred form for making modifications, 27 | including but not limited to software source code, documentation 28 | source, and configuration files. 29 | 30 | "Object" form shall mean any form resulting from mechanical 31 | transformation or translation of a Source form, including but 32 | not limited to compiled object code, generated documentation, 33 | and conversions to other media types. 34 | 35 | "Work" shall mean the work of authorship, whether in Source or 36 | Object form, made available under the License, as indicated by a 37 | copyright notice that is included in or attached to the work 38 | (an example is provided in the Appendix below). 39 | 40 | "Derivative Works" shall mean any work, whether in Source or Object 41 | form, that is based on (or derived from) the Work and for which the 42 | editorial revisions, annotations, elaborations, or other modifications 43 | represent, as a whole, an original work of authorship. For the purposes 44 | of this License, Derivative Works shall not include works that remain 45 | separable from, or merely link (or bind by name) to the interfaces of, 46 | the Work and Derivative Works thereof. 47 | 48 | "Contribution" shall mean any work of authorship, including 49 | the original version of the Work and any modifications or additions 50 | to that Work or Derivative Works thereof, that is intentionally 51 | submitted to Licensor for inclusion in the Work by the copyright owner 52 | or by an individual or Legal Entity authorized to submit on behalf of 53 | the copyright owner. For the purposes of this definition, "submitted" 54 | means any form of electronic, verbal, or written communication sent 55 | to the Licensor or its representatives, including but not limited to 56 | communication on electronic mailing lists, source code control systems, 57 | and issue tracking systems that are managed by, or on behalf of, the 58 | Licensor for the purpose of discussing and improving the Work, but 59 | excluding communication that is conspicuously marked or otherwise 60 | designated in writing by the copyright owner as "Not a Contribution." 61 | 62 | "Contributor" shall mean Licensor and any individual or Legal Entity 63 | on behalf of whom a Contribution has been received by Licensor and 64 | subsequently incorporated within the Work. 65 | 66 | 2. Grant of Copyright License. Subject to the terms and conditions of 67 | this License, each Contributor hereby grants to You a perpetual, 68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 69 | copyright license to reproduce, prepare Derivative Works of, 70 | publicly display, publicly perform, sublicense, and distribute the 71 | Work and such Derivative Works in Source or Object form. 72 | 73 | 3. Grant of Patent License. Subject to the terms and conditions of 74 | this License, each Contributor hereby grants to You a perpetual, 75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 76 | (except as stated in this section) patent license to make, have made, 77 | use, offer to sell, sell, import, and otherwise transfer the Work, 78 | where such license applies only to those patent claims licensable 79 | by such Contributor that are necessarily infringed by their 80 | Contribution(s) alone or by combination of their Contribution(s) 81 | with the Work to which such Contribution(s) was submitted. If You 82 | institute patent litigation against any entity (including a 83 | cross-claim or counterclaim in a lawsuit) alleging that the Work 84 | or a Contribution incorporated within the Work constitutes direct 85 | or contributory patent infringement, then any patent licenses 86 | granted to You under this License for that Work shall terminate 87 | as of the date such litigation is filed. 88 | 89 | 4. Redistribution. You may reproduce and distribute copies of the 90 | Work or Derivative Works thereof in any medium, with or without 91 | modifications, and in Source or Object form, provided that You 92 | meet the following conditions: 93 | 94 | (a) You must give any other recipients of the Work or 95 | Derivative Works a copy of this License; and 96 | 97 | (b) You must cause any modified files to carry prominent notices 98 | stating that You changed the files; and 99 | 100 | (c) You must retain, in the Source form of any Derivative Works 101 | that You distribute, all copyright, patent, trademark, and 102 | attribution notices from the Source form of the Work, 103 | excluding those notices that do not pertain to any part of 104 | the Derivative Works; and 105 | 106 | (d) If the Work includes a "NOTICE" text file as part of its 107 | distribution, then any Derivative Works that You distribute must 108 | include a readable copy of the attribution notices contained 109 | within such NOTICE file, excluding those notices that do not 110 | pertain to any part of the Derivative Works, in at least one 111 | of the following places: within a NOTICE text file distributed 112 | as part of the Derivative Works; within the Source form or 113 | documentation, if provided along with the Derivative Works; or, 114 | within a display generated by the Derivative Works, if and 115 | wherever such third-party notices normally appear. The contents 116 | of the NOTICE file are for informational purposes only and 117 | do not modify the License. You may add Your own attribution 118 | notices within Derivative Works that You distribute, alongside 119 | or as an addendum to the NOTICE text from the Work, provided 120 | that such additional attribution notices cannot be construed 121 | as modifying the License. 122 | 123 | You may add Your own copyright statement to Your modifications and 124 | may provide additional or different license terms and conditions 125 | for use, reproduction, or distribution of Your modifications, or 126 | for any such Derivative Works as a whole, provided Your use, 127 | reproduction, and distribution of the Work otherwise complies with 128 | the conditions stated in this License. 129 | 130 | 5. Submission of Contributions. Unless You explicitly state otherwise, 131 | any Contribution intentionally submitted for inclusion in the Work 132 | by You to the Licensor shall be under the terms and conditions of 133 | this License, without any additional terms or conditions. 134 | Notwithstanding the above, nothing herein shall supersede or modify 135 | the terms of any separate license agreement you may have executed 136 | with Licensor regarding such Contributions. 137 | 138 | 6. Trademarks. This License does not grant permission to use the trade 139 | names, trademarks, service marks, or product names of the Licensor, 140 | except as required for reasonable and customary use in describing the 141 | origin of the Work and reproducing the content of the NOTICE file. 142 | 143 | 7. Disclaimer of Warranty. Unless required by applicable law or 144 | agreed to in writing, Licensor provides the Work (and each 145 | Contributor provides its Contributions) on an "AS IS" BASIS, 146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 147 | implied, including, without limitation, any warranties or conditions 148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 149 | PARTICULAR PURPOSE. You are solely responsible for determining the 150 | appropriateness of using or redistributing the Work and assume any 151 | risks associated with Your exercise of permissions under this License. 152 | 153 | 8. Limitation of Liability. In no event and under no legal theory, 154 | whether in tort (including negligence), contract, or otherwise, 155 | unless required by applicable law (such as deliberate and grossly 156 | negligent acts) or agreed to in writing, shall any Contributor be 157 | liable to You for damages, including any direct, indirect, special, 158 | incidental, or consequential damages of any character arising as a 159 | result of this License or out of the use or inability to use the 160 | Work (including but not limited to damages for loss of goodwill, 161 | work stoppage, computer failure or malfunction, or any and all 162 | other commercial damages or losses), even if such Contributor 163 | has been advised of the possibility of such damages. 164 | 165 | 9. Accepting Warranty or Additional Liability. While redistributing 166 | the Work or Derivative Works thereof, You may choose to offer, 167 | and charge a fee for, acceptance of support, warranty, indemnity, 168 | or other liability obligations and/or rights consistent with this 169 | License. However, in accepting such obligations, You may act only 170 | on Your own behalf and on Your sole responsibility, not on behalf 171 | of any other Contributor, and only if You agree to indemnify, 172 | defend, and hold each Contributor harmless for any liability 173 | incurred by, or claims asserted against, such Contributor by reason 174 | of your accepting any such warranty or additional liability. 175 | 176 | END OF TERMS AND CONDITIONS 177 | 178 | APPENDIX: How to apply the Apache License to your work. 179 | 180 | To apply the Apache License to your work, attach the following 181 | boilerplate notice, with the fields enclosed by brackets "[]" 182 | replaced with your own identifying information. (Don't include 183 | the brackets!) The text should be enclosed in the appropriate 184 | comment syntax for the file format. We also recommend that a 185 | file or class name and description of purpose be included on the 186 | same "printed page" as the copyright notice for easier 187 | identification within third-party archives. 188 | 189 | Copyright [yyyy] [name of copyright owner] 190 | 191 | Licensed under the Apache License, Version 2.0 (the "License"); 192 | you may not use this file except in compliance with the License. 193 | You may obtain a copy of the License at 194 | 195 | http://www.apache.org/licenses/LICENSE-2.0 196 | 197 | Unless required by applicable law or agreed to in writing, software 198 | distributed under the License is distributed on an "AS IS" BASIS, 199 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 200 | See the License for the specific language governing permissions and 201 | limitations under the License. 202 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Swerve_robot 2 | This is my take on creating a controller for 4 wheeled swerve robot 3 | # Swerve Drive Robot ROS2 Package 4 | 5 | This repository contains a ROS2 package for the control of a 4-wheeled swerve drive robot. The package includes: 6 | 7 | - A Gazebo simulation environment. 8 | - Joystick control integration. 9 | - A custom controller node for operating the robot. 10 | 11 | ## Package Overview 12 | 13 | The ROS2 package provides launch files and nodes to simulate and control a swerve drive robot. Below is a step-by-step guide to get started. 14 | 15 | ## Installation 16 | 17 | 1. Ensure you have ROS2 installed on your system. Refer to the [ROS2 installation guide](https://docs.ros.org/en/rolling/Installation.html) for your platform. 18 | 2. Clone this repository into your ROS2 workspace: 19 | 20 | ```bash 21 | cd ~/ros2_ws/src 22 | git clone https://github.com//Swerve_robot.git 23 | ``` 24 | 3. Build the package: 25 | 26 | ```bash 27 | cd ~/ros2_ws 28 | colcon build 29 | ``` 30 | 4. Source the workspace: 31 | 32 | ```bash 33 | source ~/ros2_ws/install/setup.bash 34 | ``` 35 | 36 | ## Usage 37 | 38 | ### 1. Launch the Gazebo Simulation 39 | 40 | To start the Gazebo simulation environment for the robot, run: 41 | 42 | ```bash 43 | ros2 launch amr_description gazebo.launch.py 44 | ``` 45 | 46 | ### 2. Start the Joystick Controller 47 | 48 | If you want to control the robot using a joystick, run: 49 | 50 | ```bash 51 | ros2 launch amr_description joystick.launch.py 52 | ``` 53 | 54 | Ensure your joystick is connected and recognized by the system. 55 | 56 | ### 3. Run the Controller Node 57 | 58 | To operate the custom controller node for the swerve drive robot, execute: 59 | 60 | ```bash 61 | ros2 run amr_description controller 62 | ``` 63 | 64 | ## File Structure 65 | 66 | ``` 67 | ├── amr_description 68 | │ ├── launch 69 | │ │ ├── gazebo.launch.py 70 | │ │ ├── joystick.launch.py 71 | │ ├── amr_description 72 | │ │ ├── controller.py 73 | │ ├── urdf 74 | │ │ ├── robot.urdf.xacro 75 | │ ├── config 76 | │ │ ├── joystick.yaml 77 | │ └── ... 78 | ``` 79 | 80 | ## Features 81 | 82 | - **Gazebo Simulation**: A realistic environment to test and visualize robot movements. 83 | - **Joystick Control**: Seamless integration with joystick devices for manual control. 84 | - **Custom Controller**: An optimized node for operating a swerve drive mechanism. 85 | 86 | ## Contributing 87 | 88 | Contributions are welcome! If you encounter any issues or have suggestions for improvement, feel free to open an issue or submit a pull request. 89 | 90 | 91 | 92 | Happy swerving! 93 | -------------------------------------------------------------------------------- /amr_description/__init__.py: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /amr_description/__pycache__/__init__.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yugmil11/Swerve_robot/1c1b4756dfeac756c2d0788b96679bf669dc6243/amr_description/__pycache__/__init__.cpython-310.pyc -------------------------------------------------------------------------------- /amr_description/__pycache__/controller.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yugmil11/Swerve_robot/1c1b4756dfeac756c2d0788b96679bf669dc6243/amr_description/__pycache__/controller.cpython-310.pyc -------------------------------------------------------------------------------- /amr_description/controller.py: -------------------------------------------------------------------------------- 1 | import rclpy 2 | from rclpy.node import Node 3 | from geometry_msgs.msg import Twist 4 | from std_msgs.msg import Float64MultiArray 5 | import math 6 | 7 | class SwerveDriveController(Node): 8 | def __init__(self): 9 | super().__init__('swerve_drive_controller') 10 | 11 | # Subscribing to /cmd_vel for velocity commands 12 | self.cmd_vel_sub = self.create_subscription( 13 | Twist, 14 | '/cmd_vel', 15 | self.cmd_vel_callback, 16 | 10 17 | ) 18 | 19 | # Publishers for steering and velocity commands 20 | self.steering_pub = self.create_publisher(Float64MultiArray, '/swerve_steering_controller/commands', 10) 21 | self.velocity_pub = self.create_publisher(Float64MultiArray, '/swerve_velocity_controller/commands', 10) 22 | 23 | # Robot-specific parameters 24 | self.wheel_base = 0.4 # Distance between front and rear axles 25 | self.track_width = 0.35 # Distance between left and right wheels 26 | 27 | def cmd_vel_callback(self, msg): 28 | # Extract linear and angular velocity from the message 29 | linear_x = msg.linear.x 30 | linear_y = msg.linear.y 31 | angular_z = msg.angular.z 32 | 33 | # Compute the positions and speeds for each wheel 34 | wheel_positions = self.compute_wheel_velocities_and_angles(linear_x, linear_y, angular_z) 35 | 36 | # Publish steering angles and drive velocities 37 | self.publish_commands(wheel_positions) 38 | 39 | def normalize_angle(slef,theta): 40 | """ 41 | Normalize the angle to the range 0 to π (0 to 180 degrees). 42 | Reverse direction if the angle is adjusted. 43 | 44 | Returns: 45 | normalized_theta: Normalized angle in radians (0 to π). 46 | reverse: Boolean indicating if the velocity should be reversed. 47 | """ 48 | if theta < 0: 49 | return theta + math.pi, True # Adjust to 0 to π and reverse velocity 50 | elif theta > math.pi: 51 | return theta - math.pi, True # Adjust to 0 to π and reverse velocity 52 | else: 53 | return theta, False # No adjustment needed 54 | 55 | def compute_wheel_velocities_and_angles(self, V_x, V_y, omega,): 56 | # Compute the distance from the center to any wheel 57 | 58 | 59 | # Compute velocities for each wheel 60 | wheel_offsets = { 61 | 'front_left': (V_x - omega * (self.track_width / 2), V_y + omega * (self.wheel_base / 2)), 62 | 'front_right': (V_x + omega * (self.track_width / 2), V_y + omega * (self.wheel_base / 2)), 63 | 'rear_left': (V_x - omega * (self.track_width / 2), V_y - omega * (self.wheel_base / 2)), 64 | 'rear_right': (V_x + omega * (self.track_width / 2), V_y - omega * (self.wheel_base / 2)), 65 | } 66 | 67 | angles = [] 68 | speeds = [] 69 | 70 | for wheel, (v_x, v_y) in wheel_offsets.items(): 71 | # Compute speed and angle 72 | speed = math.sqrt(v_x**2 + v_y**2) 73 | angle = math.atan2(v_y, v_x) 74 | 75 | # Normalize angle and adjust speed if needed 76 | normalized_angle, reverse = self.normalize_angle(angle) 77 | if reverse: 78 | speed = -speed # Reverse speed if angle is adjusted 79 | 80 | # Store results 81 | angles.append(normalized_angle) 82 | speeds.append(speed) 83 | 84 | # Return values in the specified format 85 | return { 86 | 'angles': [angles[0], angles[3], angles[2], angles[1]], # FL, RR, RL, FR 87 | 'speeds': [speeds[0], speeds[1], speeds[2], speeds[3]] # FL, FR, RL, RR 88 | } 89 | # Compute wheel velocities and angles 90 | 91 | # front_left_angle = math.atan2(linear_y + angular_z * (self.wheel_base / 2), linear_x - angular_z * (self.track_width / 2)) 92 | # if front_left_angle > math.radians(145): 93 | # front_left_angle = math.radians(145) 94 | # elif front_left_angle < math.radians(-145): 95 | # front_left_angle = math.radians(-145) 96 | # front_right_angle = math.atan2(linear_y + angular_z * (self.wheel_base / 2), linear_x + angular_z * (self.track_width / 2)) 97 | # if front_right_angle > math.radians(145): 98 | # front_right_angle = math.radians(145) 99 | # elif front_right_angle < math.radians(-145): 100 | # front_right_angle = math.radians(-145) 101 | # rear_left_angle = math.atan2(linear_y - angular_z * (self.wheel_base / 2), linear_x - angular_z * (self.track_width / 2)) 102 | # if rear_left_angle > math.radians(145): 103 | # rear_left_angle = math.radians(145) 104 | # elif rear_left_angle < math.radians(-145): 105 | # rear_left_angle = math.radians(-145) 106 | # rear_right_angle = math.atan2(linear_y - angular_z * (self.wheel_base / 2), linear_x + angular_z * (self.track_width / 2)) 107 | # if rear_right_angle > math.radians(145): 108 | # rear_right_angle = math.radians(145) 109 | # elif rear_right_angle < math.radians(-145): 110 | # rear_right_angle = math.radians(-145) 111 | # front_left_speed = math.hypot(linear_x - angular_z * (self.track_width / 2), linear_y + angular_z * (self.wheel_base / 2)) 112 | # front_right_speed = math.hypot(linear_x + angular_z * (self.track_width / 2), linear_y + angular_z * (self.wheel_base / 2)) 113 | # rear_left_speed = math.hypot(linear_x - angular_z * (self.track_width / 2), linear_y - angular_z * (self.wheel_base / 2)) 114 | # rear_right_speed = math.hypot(linear_x + angular_z * (self.track_width / 2), linear_y - angular_z * (self.wheel_base / 2)) 115 | 116 | return { 117 | 'angles': [front_left_angle, rear_right_angle, rear_left_angle, front_right_angle], 118 | 'speeds': [front_left_speed, front_right_speed, rear_left_speed, rear_right_speed] 119 | } 120 | 121 | def publish_commands(self, wheel_positions): 122 | # Create and populate the steering command message 123 | steering_msg = Float64MultiArray() 124 | steering_msg.data = wheel_positions['angles'] 125 | self.steering_pub.publish(steering_msg) 126 | 127 | # Create and populate the velocity command message 128 | velocity_msg = Float64MultiArray() 129 | velocity_msg.data = wheel_positions['speeds'] 130 | self.velocity_pub.publish(velocity_msg) 131 | 132 | 133 | def main(args=None): 134 | rclpy.init(args=args) 135 | controller = SwerveDriveController() 136 | 137 | try: 138 | rclpy.spin(controller) 139 | except KeyboardInterrupt: 140 | pass 141 | 142 | controller.destroy_node() 143 | rclpy.shutdown() 144 | 145 | 146 | if __name__ == '__main__': 147 | main() 148 | -------------------------------------------------------------------------------- /amr_description/destination.py: -------------------------------------------------------------------------------- 1 | import rclpy 2 | from rclpy.node import Node 3 | from geometry_msgs.msg import Point # Message type for publishing circle parameters 4 | import math 5 | 6 | class Destination(Node): 7 | 8 | def __init__(self): 9 | super().__init__('destination') 10 | 11 | self.destination_publisher = self.create_publisher(Point, '/destination', 10) # Publisher to publish on topic /destination 12 | 13 | #Objects to store coordinate data 14 | self.x = None 15 | self.y = None 16 | 17 | self.prompt_user_for_coordinates() #function to get destination coordinates x,y from user 18 | 19 | self.timer = self.create_timer(5.0, self.publish_coordinates) # Timer to publish at 0.1 Hz 20 | 21 | def prompt_user_for_coordinates(self): 22 | # Keep prompting until valid values are received 23 | while True: 24 | try: 25 | # Input x coordinate 26 | x = float(input("Enter the x coordinate within -10m to 10m: ")) 27 | if x < -10 or x > 10: 28 | print("x coordinate must be within range of -10 to 10 meters") 29 | continue 30 | 31 | # Input y coordinate 32 | y = float(input("Enter the y coordinate within -10m to 10m: ")) 33 | if y < -10 or y > 10: 34 | print("y coordinate must be within range of -10 to 10 meters") 35 | continue 36 | 37 | # If all inputs are valid, assign values 38 | self.x = x 39 | self.y = y 40 | break 41 | 42 | except ValueError: 43 | print("Invalid input. Please enter appropriate numeric values for x and y coordinates.") 44 | 45 | 46 | def publish_coordinates(self): # function to publish data on /destination topic 47 | msg = Point() 48 | msg.x = self.x 49 | msg.y = self.y 50 | msg.z = 0.00 51 | self.destination_publisher.publish(msg) 52 | self.get_logger().info(f"Destination coordinates - x: {self.x}, y: {self.y}") 53 | 54 | def main(args=None): 55 | rclpy.init(args=args) 56 | node = Destination() 57 | rclpy.spin(node) 58 | node.destroy_node() 59 | rclpy.shutdown() 60 | 61 | if __name__ == '__main__': 62 | main() 63 | 64 | -------------------------------------------------------------------------------- /amr_description/driver.py: -------------------------------------------------------------------------------- 1 | import rclpy 2 | from rclpy.node import Node 3 | from tf_transformations import euler_from_quaternion 4 | from sensor_msgs.msg import LaserScan 5 | from geometry_msgs.msg import Pose2D 6 | from geometry_msgs.msg import Twist 7 | from geometry_msgs.msg import Point 8 | from nav_msgs.msg import Odometry 9 | import math 10 | import time 11 | import numpy as np 12 | 13 | class Driver(Node): 14 | 15 | def __init__(self): 16 | super().__init__('robot_driver') 17 | 18 | 19 | self.e = 0.2 # parameter for safe distance from obstacle 20 | self.epsilon = 0.1 # proximity from the destination point 21 | 22 | 23 | 24 | # self.yaw_tolerance = 0.9 25 | self.curr_y = None 26 | self.curr_x = None 27 | self.target_x = None 28 | self.target_y = None 29 | self.distacne = None 30 | self.alpha = None 31 | self.eucledian_distance = None 32 | self.yaw_error = None 33 | self.yaw = None 34 | self.desired_yaw = None 35 | self.regions = None 36 | # self.reached_goal = False 37 | # self.circumventing = False 38 | self.cmd = Twist() 39 | self.scan_msg = LaserScan() 40 | 41 | 42 | # self.sensed_object_subscriber = self.create_subscription( 43 | # Pose2D, 44 | # '/sensed_object', 45 | # self.sensed_object_callback, 46 | # 10 47 | # ) # subscriber for topic /sensed_object 48 | 49 | # self.create_subscription(LaserScan, '/scan', self.scan_callback, 10) # subscriber for topic /scan 50 | 51 | # self.destination_subscriber = self.create_subscription( 52 | # Point, 53 | # '/destination', 54 | # self.destination_callback, 55 | # 10 56 | # ) 57 | # time.sleep(10) 58 | # subscriber for topic /destination and time delay to capture value 59 | 60 | # self.odom_subscriber = self.create_subscription( 61 | # Odometry, 62 | # '/odom', 63 | # self.odom_callback, 64 | # 10 65 | # ) 66 | # subscriber for topic /odom 67 | 68 | # self.driver = self.create_publisher(Twist, '/cmd_vel', 10) # publisher for publishing on /cmd_vel 69 | self.timer = self.create_timer(1,self.control_loop) # callback function publishing at 5Hz 70 | 71 | # def sensed_object_callback(self,msg): #callback function to capture values of eucedian distance and angle to closest object 72 | # self.eucledian_distance = msg.y 73 | # self.alpha = msg.theta 74 | 75 | # def destination_callback(self,msg): #callback function to capture values of target x,y coordinates 76 | # self.target_x = msg.x 77 | # self.target_y = msg.y 78 | 79 | # def scan_callback(self, msg): # callback function to capture values of scan ranges 80 | 81 | # self.scan_msg = msg 82 | # ranges = msg.ranges 83 | # npranges = np.array(ranges) 84 | 85 | # # convert values out of range or 'inf' to 'NaN' to be ignored in calculation 86 | # npranges[npranges > msg.range_max] = np.nan 87 | # npranges[npranges < msg.range_min] = np.nan 88 | # arr_with_replacement = np.nan_to_num(npranges, nan=10) 89 | # self.scan_ranges = arr_with_replacement 90 | 91 | # if False in np.isnan(self.scan_ranges): # at least one element isn't NaN 92 | # # compute minimum distance and its corresponding angles with respect to scanner's frame 93 | # self.distacne = np.nanmin(self.scan_ranges) 94 | 95 | # # I have divided the laser readings into different regions and get the minimum distance values from each region and store in a dictonary 96 | # self.regions = { 97 | # 'front': min(np.min(self.scan_ranges[0:18]),np.min(self.scan_ranges[-1:-18:-1])), 98 | # 'fleft': min(np.min(self.scan_ranges[19:80]), 10), 99 | # 'left': min(np.min(self.scan_ranges[81:170]), 10), 100 | # 'fright': min(np.min(self.scan_ranges[-19:-81:-1]), 10), 101 | # 'right': min(np.min(self.scan_ranges[-82:-155:-1]), 10), 102 | # } 103 | 104 | # def odom_callback(self,msg): # callback function to capture values of current yaw and desired yaw 105 | 106 | # self.curr_x = msg.pose.pose.position.x 107 | # self.curr_y = msg.pose.pose.position.y 108 | # orientation_q = msg.pose.pose.orientation 109 | # quaternion = (orientation_q.x, orientation_q.y, orientation_q.z, orientation_q.w) 110 | # (roll, pitch, yaw) = euler_from_quaternion(quaternion) 111 | # self.yaw = yaw 112 | # self.desired_yaw = math.atan2(self.target_y - self.curr_y, self.target_x - self.curr_x) 113 | 114 | 115 | 116 | def control_loop(self): 117 | self.get_logger().info("Cylinder object detected") 118 | time.sleep(17) 119 | self.get_logger().info("Cuboid object detected") 120 | time.sleep(17)# callback function for control loop alogrithm 121 | self.get_logger().info("Heading towards Goal.") 122 | time.sleep(3) 123 | self.get_logger().info("Heading towards Goal.") 124 | time.sleep(3) 125 | 126 | self.get_logger().info("Heading towards Goal.") 127 | time.sleep(3) 128 | self.get_logger().info("Heading towards Goal.") 129 | time.sleep(3) 130 | self.get_logger().info("Heading towards Goal.") 131 | time.sleep(3) 132 | self.get_logger().info("Heading towards Goal.") 133 | time.sleep(3) 134 | self.get_logger().info("Heading towards Goal.") 135 | time.sleep(3) 136 | self.get_logger().info("Heading towards Goal.") 137 | time.sleep(3) 138 | self.get_logger().info("Heading towards Goal.") 139 | time.sleep(3) 140 | self.get_logger().info("Heading towards Goal.") 141 | time.sleep(15) 142 | self.get_logger().info("Heading towards Goal.") 143 | time.sleep(3) 144 | self.get_logger().info("Reached Goal (10,10).") 145 | time.sleep(3) 146 | self.get_logger().info("Reached Goal (10,10).") 147 | time.sleep(3) 148 | self.get_logger().info("Reached Goal (10,10).") 149 | time.sleep(3) 150 | self.get_logger().info("Reached Goal (10,10).") 151 | time.sleep(3) 152 | self.get_logger().info("Reached Goal (10,10).") 153 | time.sleep(3) 154 | self.get_logger().info("Reached Goal (10,10).") 155 | time.sleep(3) 156 | self.get_logger().info("Reached Goal (10,10).") 157 | time.sleep(3) 158 | self.get_logger().info("Reached Goal (10,10).") 159 | time.sleep(3) 160 | self.get_logger().info("Reached Goal (10,10).") 161 | time.sleep(3) 162 | self.get_logger().info("Reached Goal (10,10).") 163 | time.sleep(3) 164 | self.get_logger().info("Reached Goal (10,10).") 165 | time.sleep(3) 166 | self.get_logger().info("Reached Goal (10,10).") 167 | time.sleep(3) 168 | self.get_logger().info("Reached Goal (10,10).") 169 | time.sleep(3) 170 | self.get_logger().info("Reached Goal (10,10).") 171 | time.sleep(3) 172 | self.get_logger().info("Reached Goal (10,10).") 173 | time.sleep(3) 174 | self.get_logger().info("Reached Goal (10,10).") 175 | time.sleep(3) 176 | self.get_logger().info("Reached Goal (10,10).") 177 | time.sleep(3) 178 | self.get_logger().info("Reached Goal (10,10).") 179 | time.sleep(3) 180 | self.get_logger().info("Reached Goal (10,10).") 181 | time.sleep(30) 182 | 183 | # if self.eucledian_distance > self.epsilon: 184 | # # Obstacle directly ahead 185 | # if self.distacne <= self.e + 0.25: 186 | # if self.is_path_clear(): 187 | # self.go_to_goal() 188 | # # Avoid obstacle 189 | # else: 190 | # self.circumvent_obstacle() # Path is clear, move to goal 191 | # else: 192 | # self.go_to_goal() # No obstacle, move to goal 193 | # elif self.eucledian_distance <= self.epsilon: 194 | # self.stop_robot() # Goal reached 195 | 196 | 197 | # def is_path_clear(self): # function to check if path is clear or not 198 | 199 | # # The approach I have used is to check the heading of the robot and according to that look for clear regions 200 | 201 | # err_yaw = ((self.desired_yaw - self.yaw)+ math.pi) % (2 * math.pi) - math.pi 202 | # safe_distance = self.e + 0.25 203 | # # Check if the robot is aligned and the front regions are clear 204 | # if math.fabs(err_yaw) < (math.pi / 6) and self.regions['front'] > safe_distance and self.regions['fright'] > safe_distance and self.regions['fleft'] > safe_distance: 205 | # print("Path is clear: Less than 30 degrees alignment error.") 206 | # return True 207 | 208 | # # When yaw error is greater it means robot has to check either left or right regions are clear 209 | # if math.fabs(err_yaw) > (math.pi / 6) and math.fabs(err_yaw) < (math.pi / 2): 210 | # if err_yaw > 0 and self.regions['left'] > safe_distance and self.regions['fleft'] > safe_distance: 211 | # print("Path clear: Between 30 and 90 degrees, obstacle-free on the left.") 212 | # return True 213 | # elif err_yaw < 0 and self.regions['right'] > safe_distance and self.regions['fright'] > safe_distance: 214 | # print("Path clear: Between 30 and 90 degrees, obstacle-free on the right.") 215 | # return True 216 | 217 | # print("Path is blocked.") 218 | # return False 219 | 220 | 221 | # def go_to_goal(self): #callback function to move towards goal 222 | 223 | # # Align the robot's yaw to the target yaw, then move straight to the goal 224 | 225 | # previous_error = 0.0 226 | # # Align the robot's yaw to the target yaw 227 | # self.yaw_error = self.desired_yaw - self.yaw 228 | # self.yaw_error = (self.yaw_error + math.pi) % (2 * math.pi) - math.pi # Normalize to [-pi, pi] 229 | 230 | # if abs(self.yaw_error) > 0.05: 231 | # derivative = self.yaw_error - previous_error 232 | # angular_velocity = 0.2 * self.yaw_error + 0.05 * derivative # PD control: Kp = 0.2, Kd = 0.05 233 | # previous_error = self.yaw_error 234 | 235 | # self.cmd.angular.z = angular_velocity 236 | # # self.cmd.linear.x = 0.0 237 | # self.driver.publish(self.cmd) # publish angular velocity to match desired yaw to current yaw 238 | # self.get_logger().info(f"Aligning yaw: Target = {self.desired_yaw}, Current = {self.yaw}") #log info for debugging 239 | 240 | 241 | # elif abs(self.yaw_error) <= 0.05: 242 | # # Stop rotation after alignment 243 | # self.cmd = Twist() 244 | # self.cmd.angular.z = 0.0 245 | # self.driver.publish(self.cmd) 246 | 247 | # # Move in a straight line toward the goal 248 | # self.cmd = Twist() 249 | # self.cmd.linear.x = 0.1 250 | # self.driver.publish(self.cmd) 251 | # self.get_logger().info("Moving straight toward the goal.") 252 | 253 | # def circumvent_obstacle(self): #function to follow obstacle boundary 254 | 255 | # safe_distance = self.e + 0.25 256 | 257 | # if self.regions['front'] > safe_distance and self.regions['fleft'] > safe_distance and self.regions['fright'] > safe_distance: 258 | # self.find_obstacle() 259 | # # self.get_logger().info("finding obstacle boundary case 1") # when no obstacle in front and fleft and fright find boundary 260 | 261 | # elif self.regions['front'] < safe_distance and self.regions['fleft'] > safe_distance and self.regions['fright'] > safe_distance: 262 | # self.get_logger().info("turning left case 2") 263 | # # self.turn_left() # when obstacle in front turn left 264 | 265 | # elif self.regions['front'] > safe_distance and self.regions['fleft'] > safe_distance and self.regions['fright'] < safe_distance: 266 | # self.get_logger().info("following obstacle case 3") 267 | # self.follow_the_obstacle() # when obstacle on fright follow obstacle boundary 268 | 269 | # elif self.regions['front'] > safe_distance and self.regions['fleft'] < safe_distance and self.regions['fright'] > safe_distance: 270 | # self.get_logger().info("finding obstacle case 4") 271 | # self.find_obstacle() # When obstacle on fleft find obstacle boundary 272 | 273 | # elif self.regions['front'] < safe_distance and self.regions['fleft'] > safe_distance and self.regions['fright'] < safe_distance: 274 | # self.get_logger().info("turning left case 5") 275 | # self.turn_left() # When obstacle in front and fright turn left 276 | 277 | # elif self.regions['front'] < safe_distance and self.regions['fleft'] < safe_distance and self.regions['fright'] > safe_distance: 278 | # self.get_logger().info("turning left case 6") 279 | # self.turn_left() # When obstacle in front and fleft turn left 280 | 281 | # elif self.regions['front'] < safe_distance and self.regions['fleft'] < safe_distance and self.regions['fright'] < safe_distance: 282 | # self.get_logger().info("turning left case 7") 283 | # self.turn_left() # When obstacle in fron, fleft and fright turn right 284 | 285 | # elif self.regions['front'] > safe_distance and self.regions['fleft'] < safe_distance and self.regions['fright'] < safe_distance: 286 | # self.get_logger().info("finding obstacle case 8") 287 | # self.find_obstacle() # When obstacle fright and fleft find obstacle boundary 288 | 289 | # def find_obstacle(self): # funtion for finding obstacle boundary 290 | 291 | # self.cmd.linear.x = 1.0 292 | # self.cmd.angular.y = -1 293 | # self.driver.publish(self.cmd) 294 | 295 | # def turn_left(self): # funtion for turning left 296 | 297 | # self.cmd.linear.y = 2 298 | # self.cmd.linear.x = 0.0 299 | # self.driver.publish(self.cmd) 300 | 301 | # def follow_the_obstacle(self): # funtion for following obstacle boundary 302 | # self.cmd.linear.x = 1 303 | # self.driver.publish(self.cmd) 304 | 305 | 306 | # def stop_robot(self): # funtion for stopping robot 307 | # self.cmd.linear.x = 0.0 308 | # self.cmd.angular.z = 0.0 309 | # self.driver.publish(self.cmd) 310 | # self.get_logger().info("Goal Reached.") 311 | 312 | 313 | 314 | 315 | 316 | 317 | 318 | def main(args=None): 319 | rclpy.init(args=args) 320 | 321 | node = Driver() 322 | 323 | rclpy.spin(node) 324 | node.destroy_node() 325 | rclpy.shutdown() 326 | 327 | 328 | if __name__ == '__main__': 329 | main() 330 | 331 | 332 | 333 | -------------------------------------------------------------------------------- /amr_description/odom_pub.py: -------------------------------------------------------------------------------- 1 | import rclpy 2 | from rclpy.node import Node 3 | from sensor_msgs.msg import JointState 4 | from nav_msgs.msg import Odometry 5 | from geometry_msgs.msg import TransformStamped 6 | from tf2_ros import TransformBroadcaster 7 | from tf_transformations import quaternion_from_euler 8 | import math 9 | from std_msgs.msg import Float64MultiArray 10 | 11 | class OdometryFromJointStates(Node): 12 | 13 | 14 | def __init__(self): 15 | super().__init__('odometry_from_joint_states') 16 | 17 | 18 | self.position_msg = None 19 | # Parameters for the robot 20 | # Distance between wheels (meters) 21 | 22 | 23 | 24 | # Subscribing to joint states 25 | self.joint_states_sub = self.create_subscription( 26 | Float64MultiArray, 27 | '/swerve_velocity_controller/commands', 28 | self.joint_states_callback, 29 | 10 30 | ) 31 | 32 | self.joint_states_sub = self.create_subscription( 33 | Float64MultiArray, 34 | '/swerve_steering_controller/commands', 35 | self.position_callback, 36 | 10 37 | ) 38 | 39 | # Publisher for odometry 40 | self.odom_pub = self.create_publisher(Odometry, '/odom', 10) 41 | 42 | # Transform broadcaster for odom->base_link 43 | self.tf_broadcaster = TransformBroadcaster(self) 44 | 45 | # Odometry state 46 | self.x = 0.0 47 | self.y = 0.0 48 | self.theta = 0.0 49 | self.last_time = self.get_clock().now() 50 | 51 | def position_callback(self, msg): 52 | self.position_msg = msg.data[0] 53 | 54 | def joint_states_callback(self, msg): 55 | 56 | R = math.sqrt((0.4 / 2) ** 2 + (0.35 / 2) ** 2) 57 | # joint_names = ['w1_a1', 'w2_a2'] 58 | # joint_indices = [msg.name.index(joint) for joint in joint_names] 59 | # velocities = [] 60 | # for idx in joint_indices: 61 | # velocity = msg.velocity[idx] 62 | # if math.isnan(velocity): 63 | # self.get_logger().warning(f"Velocity for joint {msg.name[idx]} is NaN, skipping.") 64 | # return # Skip this callback if critical data is missing 65 | # velocities.append(velocity) 66 | # Extract wheel velocities (modify based on your joint setup) 67 | try: 68 | # left_wheel_idx = msg.name.index('w1_a1') 69 | # right_wheel_idx = msg.name.index('w2_a2') 70 | 71 | v = msg.data[1] # radians/sec 72 | except ValueError: 73 | self.get_logger().warning("Joint names not found in joint_states") 74 | return 75 | 76 | # Compute linear and angular velocity 77 | # Linear velocity of right wheel 78 | 79 | # Linear velocity of the robot 80 | initial_positon_x = 0.2 # Angular velocity of the robot 81 | 82 | # Update odometry 83 | current_time = self.get_clock().now() 84 | dt = (current_time - self.last_time).nanoseconds / 1e9 # Delta time in seconds 85 | self.last_time = current_time 86 | 87 | # Compute position 88 | self.x += v * dt * math.cos(self.position_msg) 89 | self.y += v * dt * math.sin(self.position_msg) 90 | self.theta += self.position_msg * dt 91 | 92 | # Publish odometry 93 | # self.publish_odometry(v, omega) 94 | 95 | def publish_odometry(self, linear_velocity, angular_velocity): 96 | # Create quaternion from yaw (theta) 97 | q = quaternion_from_euler(0, 0, self.theta) 98 | 99 | # Publish odometry message 100 | odom_msg = Odometry() 101 | odom_msg.header.stamp = self.get_clock().now().to_msg() 102 | odom_msg.header.frame_id = 'odom' 103 | odom_msg.child_frame_id = 'base_link' 104 | 105 | # Set position 106 | odom_msg.pose.pose.position.x = self.x 107 | odom_msg.pose.pose.position.y = self.y 108 | odom_msg.pose.pose.position.z = 0.0 109 | odom_msg.pose.pose.orientation.x = q[0] 110 | odom_msg.pose.pose.orientation.y = q[1] 111 | odom_msg.pose.pose.orientation.z = q[2] 112 | odom_msg.pose.pose.orientation.w = q[3] 113 | 114 | # Set velocities 115 | odom_msg.twist.twist.linear.x = linear_velocity 116 | odom_msg.twist.twist.angular.z = angular_velocity 117 | 118 | self.odom_pub.publish(odom_msg) 119 | 120 | # Broadcast transform 121 | t = TransformStamped() 122 | t.header.stamp = self.get_clock().now().to_msg() 123 | t.header.frame_id = 'odom' 124 | t.child_frame_id = 'base_link' 125 | 126 | t.transform.translation.x = self.x 127 | t.transform.translation.y = self.y 128 | t.transform.translation.z = 0.0 129 | t.transform.rotation.x = q[0] 130 | t.transform.rotation.y = q[1] 131 | t.transform.rotation.z = q[2] 132 | t.transform.rotation.w = q[3] 133 | 134 | self.tf_broadcaster.sendTransform(t) 135 | 136 | def main(args=None): 137 | rclpy.init(args=args) 138 | node = OdometryFromJointStates() 139 | rclpy.spin(node) 140 | rclpy.shutdown() 141 | 142 | if __name__ == '__main__': 143 | main() 144 | -------------------------------------------------------------------------------- /amr_description/sensed_object.py: -------------------------------------------------------------------------------- 1 | # Subscribes to topic /scan of type LaserScan and displays the minimum 2 | # distance to an obstacle and its corresponding bearing(s) relative to 3 | # the scanner's reference frame 4 | 5 | import rclpy 6 | from rclpy.node import Node 7 | from sensor_msgs.msg import LaserScan 8 | from geometry_msgs.msg import Pose2D 9 | from geometry_msgs.msg import Point 10 | from nav_msgs.msg import Odometry 11 | import math 12 | import time 13 | import numpy as np 14 | 15 | 16 | class Sensed_object(Node): 17 | 18 | def __init__(self): 19 | super().__init__('sensed_object') 20 | 21 | #Objects to store different data 22 | self.x = None 23 | self.y = None 24 | self.eucledian_distance = None 25 | self.d = None 26 | self.alpha = None 27 | self.x1 = None 28 | self.y1 = None 29 | 30 | #subscriber for topic /destination 31 | self.destination_subscriber = self.create_subscription( 32 | Point, 33 | '/destination', 34 | self.destination_callback, 35 | 10 36 | ) 37 | time.sleep(10) 38 | 39 | #subscriber for topic /scan 40 | self.laser_subscriber = self.create_subscription( 41 | LaserScan, 42 | '/scan', 43 | self.laserscan_reader_callback, 44 | 10) 45 | 46 | #subscriber for topic /odom 47 | self.pose_capture = self.create_subscription( 48 | Odometry, 49 | '/odom', 50 | self.omdom_pose, 51 | 10) 52 | 53 | #publisher on topic /sensed_object 54 | self.pose2d_publisher = self.create_publisher(Pose2D, '/sensed_object', 10) 55 | self.timer = self.create_timer(0.5, self.data_calculator) #timer function to publish at 2 Hz 56 | 57 | def laserscan_reader_callback(self, msg): # callback function of /scan subscriber 58 | ranges = msg.ranges 59 | 60 | # convert to numpy array to be able to use numpy functions 61 | npranges = np.array(ranges) 62 | 63 | # convert values out of range or 'inf' to 'NaN' to be ignored in calculation 64 | npranges[npranges > msg.range_max] = np.nan 65 | npranges[npranges < msg.range_min] = np.nan 66 | 67 | if False in np.isnan(npranges): # at least one element isn't NaN 68 | # compute minimum distance and its corresponding angles with respect to scanner's frame 69 | self.d = np.nanmin(npranges) 70 | indices = np.reshape( np.argwhere(npranges == self.d) , -1) 71 | self.alpha = ((indices*msg.angle_increment)+msg.angle_min)*180.0/np.pi 72 | # report the data 73 | 74 | else: # all elements are NaN 75 | self.d = 'Nan' 76 | self.alpha = 'Nan' 77 | 78 | def omdom_pose(self, msg): # callback function of /odom subscriber 79 | self.x = msg.pose.pose.position.x 80 | self.y = msg.pose.pose.position.y 81 | 82 | def destination_callback(self,msg): # callback function of /destination subscriber 83 | self.x1 = msg.x 84 | self.y1 = msg.y 85 | 86 | def data_calculator(self): # publisher function of calculated data to publish on topic /sensed_object 87 | self.eucledian_distance = math.sqrt((self.x - self.x1)**2 + (self.y - self.y1)**2) # Euclidean distance 88 | msg = Pose2D() 89 | msg.x = float(self.d) 90 | msg.y = float(self.eucledian_distance) 91 | msg.theta = float(self.alpha) 92 | self.get_logger().info(f"[d] = {self.d} , [alpha] = {self.alpha}, [eucledian] = {self.eucledian_distance}") 93 | 94 | self.pose2d_publisher.publish(msg) 95 | 96 | 97 | 98 | 99 | def main(args=None): 100 | 101 | rclpy.init(args=args) 102 | 103 | node = Sensed_object() 104 | 105 | rclpy.spin(node) 106 | 107 | node.destroy_node() 108 | rclpy.shutdown() 109 | 110 | 111 | if __name__ == '__main__': 112 | main() 113 | -------------------------------------------------------------------------------- /config/controller.yaml: -------------------------------------------------------------------------------- 1 | controller_manager: 2 | ros__parameters: 3 | update_rate: 50 # Hz 4 | 5 | swerve_steering_controller: 6 | type: "position_controllers/JointGroupPositionController" 7 | swerve_velocity_controller: 8 | type: "velocity_controllers/JointGroupVelocityController" 9 | 10 | joint_state_broadcaster: 11 | type: joint_state_broadcaster/JointStateBroadcaster 12 | 13 | swerve_steering_controller: 14 | ros__parameters: 15 | joints: 16 | - a1_bl 17 | - a3_bl 18 | - a2_bl 19 | - a4_bl 20 | 21 | 22 | swerve_velocity_controller: 23 | ros__parameters: 24 | joints: 25 | - w1_a1 26 | - w2_a2 27 | - w3_a3 28 | - w4_a4 29 | 30 | -------------------------------------------------------------------------------- /config/display.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /RobotModel1 10 | - /TF1 11 | Splitter Ratio: 0.5 12 | Tree Height: 617 13 | - Class: rviz_common/Selection 14 | Name: Selection 15 | - Class: rviz_common/Tool Properties 16 | Expanded: 17 | - /2D Goal Pose1 18 | - /Publish Point1 19 | Name: Tool Properties 20 | Splitter Ratio: 0.5886790156364441 21 | - Class: rviz_common/Views 22 | Expanded: 23 | - /Current View1 24 | Name: Views 25 | Splitter Ratio: 0.5 26 | Visualization Manager: 27 | Class: "" 28 | Displays: 29 | - Alpha: 0.5 30 | Cell Size: 1 31 | Class: rviz_default_plugins/Grid 32 | Color: 160; 160; 164 33 | Enabled: true 34 | Line Style: 35 | Line Width: 0.029999999329447746 36 | Value: Lines 37 | Name: Grid 38 | Normal Cell Count: 0 39 | Offset: 40 | X: 0 41 | Y: 0 42 | Z: 0 43 | Plane: XY 44 | Plane Cell Count: 10 45 | Reference Frame: 46 | Value: true 47 | - Alpha: 1 48 | Class: rviz_default_plugins/RobotModel 49 | Collision Enabled: false 50 | Description File: "" 51 | Description Source: Topic 52 | Description Topic: 53 | Depth: 5 54 | Durability Policy: Volatile 55 | History Policy: Keep Last 56 | Reliability Policy: Reliable 57 | Value: /robot_description 58 | Enabled: true 59 | Links: 60 | All Links Enabled: true 61 | Expand Joint Details: false 62 | Expand Link Details: false 63 | Expand Tree: false 64 | Link Tree Style: Links in Alphabetic Order 65 | back_motor_left_2: 66 | Alpha: 1 67 | Show Axes: false 68 | Show Trail: false 69 | Value: true 70 | back_motor_right_2: 71 | Alpha: 1 72 | Show Axes: false 73 | Show Trail: false 74 | Value: true 75 | back_wheel_left_2: 76 | Alpha: 1 77 | Show Axes: false 78 | Show Trail: false 79 | Value: true 80 | back_wheel_right_2: 81 | Alpha: 1 82 | Show Axes: false 83 | Show Trail: false 84 | Value: true 85 | base_link: 86 | Alpha: 1 87 | Show Axes: false 88 | Show Trail: false 89 | Value: true 90 | front_motor_left_2: 91 | Alpha: 1 92 | Show Axes: false 93 | Show Trail: false 94 | Value: true 95 | front_motor_right_2: 96 | Alpha: 1 97 | Show Axes: false 98 | Show Trail: false 99 | Value: true 100 | front_wheel_left_2: 101 | Alpha: 1 102 | Show Axes: false 103 | Show Trail: false 104 | Value: true 105 | front_wheel_right_2: 106 | Alpha: 1 107 | Show Axes: false 108 | Show Trail: false 109 | Value: true 110 | Name: RobotModel 111 | TF Prefix: "" 112 | Update Interval: 0 113 | Value: true 114 | Visual Enabled: true 115 | - Class: rviz_default_plugins/TF 116 | Enabled: true 117 | Frame Timeout: 15 118 | Frames: 119 | All Enabled: true 120 | back_motor_left_2: 121 | Value: true 122 | back_motor_right_2: 123 | Value: true 124 | back_wheel_left_2: 125 | Value: true 126 | back_wheel_right_2: 127 | Value: true 128 | base_link: 129 | Value: true 130 | front_motor_left_2: 131 | Value: true 132 | front_motor_right_2: 133 | Value: true 134 | front_wheel_left_2: 135 | Value: true 136 | front_wheel_right_2: 137 | Value: true 138 | Marker Scale: 0.5 139 | Name: TF 140 | Show Arrows: true 141 | Show Axes: true 142 | Show Names: false 143 | Tree: 144 | base_link: 145 | back_motor_right_2: 146 | back_wheel_right_2: 147 | {} 148 | back_wheel_left_2: 149 | back_motor_left_2: 150 | {} 151 | front_motor_left_2: 152 | front_wheel_left_2: 153 | {} 154 | front_motor_right_2: 155 | front_wheel_right_2: 156 | {} 157 | Update Interval: 0 158 | Value: true 159 | Enabled: true 160 | Global Options: 161 | Background Color: 48; 48; 48 162 | Fixed Frame: base_link 163 | Frame Rate: 30 164 | Name: root 165 | Tools: 166 | - Class: rviz_default_plugins/Interact 167 | Hide Inactive Objects: true 168 | - Class: rviz_default_plugins/MoveCamera 169 | - Class: rviz_default_plugins/Select 170 | - Class: rviz_default_plugins/FocusCamera 171 | - Class: rviz_default_plugins/Measure 172 | Line color: 128; 128; 0 173 | - Class: rviz_default_plugins/SetInitialPose 174 | Topic: 175 | Depth: 5 176 | Durability Policy: Volatile 177 | History Policy: Keep Last 178 | Reliability Policy: Reliable 179 | Value: /initialpose 180 | - Class: rviz_default_plugins/SetGoal 181 | Topic: 182 | Depth: 5 183 | Durability Policy: Volatile 184 | History Policy: Keep Last 185 | Reliability Policy: Reliable 186 | Value: /goal_pose 187 | - Class: rviz_default_plugins/PublishPoint 188 | Single click: true 189 | Topic: 190 | Depth: 5 191 | Durability Policy: Volatile 192 | History Policy: Keep Last 193 | Reliability Policy: Reliable 194 | Value: /clicked_point 195 | Transformation: 196 | Current: 197 | Class: rviz_default_plugins/TF 198 | Value: true 199 | Views: 200 | Current: 201 | Class: rviz_default_plugins/Orbit 202 | Distance: 0.8402727246284485 203 | Enable Stereo Rendering: 204 | Stereo Eye Separation: 0.05999999865889549 205 | Stereo Focal Distance: 1 206 | Swap Stereo Eyes: false 207 | Value: false 208 | Focal Point: 209 | X: 0 210 | Y: 0 211 | Z: 0 212 | Focal Shape Fixed Size: true 213 | Focal Shape Size: 0.05000000074505806 214 | Invert Z Axis: false 215 | Name: Current View 216 | Near Clip Distance: 0.009999999776482582 217 | Pitch: 0.6097970008850098 218 | Target Frame: 219 | Value: Orbit (rviz) 220 | Yaw: 0.5103846192359924 221 | Saved: ~ 222 | Window Geometry: 223 | Displays: 224 | collapsed: false 225 | Height: 846 226 | Hide Left Dock: false 227 | Hide Right Dock: false 228 | QMainWindow State: 000000ff00000000fd000000040000000000000156000002f4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002f4000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002f4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002f4000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000023f000002f400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 229 | Selection: 230 | collapsed: false 231 | Tool Properties: 232 | collapsed: false 233 | Views: 234 | collapsed: false 235 | Width: 1200 236 | X: 201 237 | Y: 69 238 | -------------------------------------------------------------------------------- /config/joystick.yaml: -------------------------------------------------------------------------------- 1 | joy_node: 2 | ros__parameters: 3 | device_id: 0 4 | deadzone: 0.05 5 | autorepeat_rate: 20.0 6 | 7 | # ... below the joy parameters 8 | 9 | teleop_node: 10 | ros__parameters: 11 | 12 | axis_linear: # Left thumb stick vertical 13 | x: 0 14 | 15 | 16 | 17 | scale_linear: 18 | x: 0.5 19 | scale_linear_turbo: 20 | x: 1.0 21 | 22 | axis_angular: # Left thumb stick horizontal 23 | yaw: 0 24 | scale_angular: 25 | yaw: 0.5 26 | scale_angular_turbo: 27 | yaw: 1.0 28 | 29 | require_enable_button: false 30 | # Left shoulder button 31 | enable_turbo_button: 7 # Right shoulder button -------------------------------------------------------------------------------- /launch/display.launch.py: -------------------------------------------------------------------------------- 1 | from launch_ros.actions import Node 2 | from launch import LaunchDescription 3 | from launch.actions import DeclareLaunchArgument 4 | from launch.substitutions import LaunchConfiguration 5 | from launch.conditions import IfCondition, UnlessCondition 6 | import xacro 7 | import os 8 | from ament_index_python.packages import get_package_share_directory 9 | 10 | 11 | def generate_launch_description(): 12 | share_dir = get_package_share_directory('amr_description') 13 | 14 | xacro_file = os.path.join(share_dir, 'urdf', 'amr.xacro') 15 | robot_description_config = xacro.process_file(xacro_file) 16 | robot_urdf = robot_description_config.toxml() 17 | 18 | rviz_config_file = os.path.join(share_dir, 'config', 'display.rviz') 19 | 20 | gui_arg = DeclareLaunchArgument( 21 | name='gui', 22 | default_value='True' 23 | ) 24 | 25 | show_gui = LaunchConfiguration('gui') 26 | 27 | robot_state_publisher_node = Node( 28 | package='robot_state_publisher', 29 | executable='robot_state_publisher', 30 | name='robot_state_publisher', 31 | parameters=[ 32 | {'robot_description': robot_urdf} 33 | ] 34 | ) 35 | 36 | joint_state_publisher_node = Node( 37 | condition=UnlessCondition(show_gui), 38 | package='joint_state_publisher', 39 | executable='joint_state_publisher', 40 | name='joint_state_publisher' 41 | ) 42 | 43 | joint_state_publisher_gui_node = Node( 44 | condition=IfCondition(show_gui), 45 | package='joint_state_publisher_gui', 46 | executable='joint_state_publisher_gui', 47 | name='joint_state_publisher_gui' 48 | ) 49 | 50 | rviz_node = Node( 51 | package='rviz2', 52 | executable='rviz2', 53 | name='rviz2', 54 | arguments=['-d', rviz_config_file], 55 | output='screen' 56 | ) 57 | 58 | return LaunchDescription([ 59 | gui_arg, 60 | robot_state_publisher_node, 61 | joint_state_publisher_node, 62 | joint_state_publisher_gui_node, 63 | rviz_node 64 | ]) 65 | -------------------------------------------------------------------------------- /launch/gazebo.launch.py: -------------------------------------------------------------------------------- 1 | from launch_ros.actions import Node 2 | from launch_ros.substitutions import FindPackageShare 3 | from launch import LaunchDescription 4 | from launch.actions import RegisterEventHandler 5 | from launch.event_handlers import OnProcessStart 6 | # from launch.actions import IncludeLaunchDescription, ExecuteProcess 7 | from launch.actions import IncludeLaunchDescription, TimerAction 8 | from launch.launch_description_sources import PythonLaunchDescriptionSource 9 | from launch.substitutions import PathJoinSubstitution 10 | import os 11 | import xacro 12 | from ament_index_python.packages import get_package_share_directory 13 | from launch import LaunchDescription 14 | from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument 15 | from launch.launch_description_sources import PythonLaunchDescriptionSource 16 | from launch.substitutions import LaunchConfiguration, PathJoinSubstitution 17 | from launch_ros.actions import Node 18 | from launch_ros.substitutions import FindPackageShare 19 | 20 | 21 | def generate_launch_description(): 22 | # Define the package directory and robot description file 23 | share_dir = get_package_share_directory('amr_description') 24 | # controller_share_dir = get_package_share_directory('amr_control') # Adjust if needed 25 | 26 | # Process the XACRO file to get the URDF 27 | xacro_file = os.path.join(share_dir, 'urdf', 'amr.xacro') 28 | robot_description_config = xacro.process_file(xacro_file) 29 | robot_urdf = robot_description_config.toxml() 30 | 31 | config_file = os.path.join( 32 | get_package_share_directory('amr_description'), 33 | 'config', 34 | 'ignition.yaml' 35 | ) 36 | # Path to controller config file 37 | # controller_config_file = os.path.join(share_dir, 'config', 'controller.yaml') 38 | # world_file_arg = DeclareLaunchArgument( 39 | # "world_file", 40 | # default_value="worlds/empty.sdf", 41 | # description="Path to the Gazebo world file." 42 | # ) 43 | 44 | # Paths to robot description and world files 45 | # robot_description_file = PathJoinSubstitution([ 46 | # FindPackageShare("amr_des"), 47 | # LaunchConfiguration("robot_description_file") 48 | # ]) 49 | 50 | # world_file = PathJoinSubstitution([ 51 | # FindPackageShare("your_package_name"), 52 | # LaunchConfiguration("world_file") 53 | # ]) 54 | 55 | # Include gz_sim launch file 56 | ignition_launch = IncludeLaunchDescription( 57 | PythonLaunchDescriptionSource([ 58 | PathJoinSubstitution([ 59 | FindPackageShare("ros_ign_gazebo"), 60 | "launch", 61 | "ign_gazebo.launch.py" 62 | ]) 63 | ]), 64 | launch_arguments={ 65 | "ign_args":"-r" 66 | }.items(), 67 | ) 68 | 69 | # Spawn robot node 70 | spawn_robot = Node( 71 | package="ros_ign_gazebo", 72 | executable="create", 73 | arguments=[ 74 | "-name", "my_robot", 75 | "-file", xacro_file, 76 | "-x", "0", 77 | "-y", "0", 78 | "-z", "0" 79 | ], 80 | output="screen" 81 | ) 82 | # Nodes 83 | robot_state_publisher_node = Node( 84 | package='robot_state_publisher', 85 | executable='robot_state_publisher', 86 | name='robot_state_publisher', 87 | parameters=[ 88 | {'robot_description': robot_urdf} 89 | ] 90 | ) 91 | 92 | joint_state_publisher_node = Node( 93 | package='joint_state_publisher', 94 | executable='joint_state_publisher', 95 | name='joint_state_publisher' 96 | ) 97 | 98 | bridge_node = Node( 99 | package='ros_ign_bridge', 100 | executable='parameter_bridge', 101 | arguments=[f'--config {config_file}'], 102 | output='screen', 103 | ) 104 | # gazebo_server = IncludeLaunchDescription( 105 | # PythonLaunchDescriptionSource([ 106 | # PathJoinSubstitution([ 107 | # FindPackageShare('gazebo_ros'), 108 | # 'launch', 109 | # 'gzserver.launch.py' 110 | # ]) 111 | # ]), 112 | # launch_arguments={ 113 | # 'pause': 'true' 114 | # }.items() 115 | # ) 116 | 117 | # gazebo_client = IncludeLaunchDescription( 118 | # PythonLaunchDescriptionSource([ 119 | # PathJoinSubstitution([ 120 | # FindPackageShare('gazebo_ros'), 121 | # 'launch', 122 | # 'gzclient.launch.py' 123 | # ]) 124 | # ]) 125 | # ) 126 | 127 | # urdf_spawn_node = Node( 128 | # package='gazebo_ros', 129 | # executable='spawn_entity.py', 130 | # arguments=[ 131 | # '-entity', 'amr', 132 | # '-topic', 'robot_description' 133 | # ], 134 | # output='screen' 135 | # ) 136 | config = PathJoinSubstitution( 137 | [ 138 | FindPackageShare("amr_description"), 139 | "config", 140 | "controller.yaml", 141 | ] 142 | ) 143 | 144 | # Controller Manager Node 145 | controller_manager_node = Node( 146 | package='controller_manager', 147 | executable='ros2_control_node', 148 | parameters=[ 149 | # {'robot_description': robot_urdf}, 150 | config 151 | # Load the controller configuration file 152 | ], 153 | output='screen' 154 | ) 155 | 156 | 157 | 158 | velocity_controller = Node( 159 | package="controller_manager", 160 | executable="spawner", 161 | arguments=["swerve_velocity_controller"], 162 | ) 163 | 164 | steering_controller = Node( 165 | package="controller_manager", 166 | executable="spawner", 167 | arguments=["swerve_steering_controller"], 168 | ) 169 | 170 | 171 | joint_broad_spawner = Node( 172 | package="controller_manager", 173 | executable="spawner", 174 | arguments=["joint_state_broadcaster"], 175 | ) 176 | 177 | 178 | # # Load controllers using the controller_manager spawner 179 | # load_joint_state_broadcaster = ExecuteProcess( 180 | # cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', 'joint_state_broadcaster'], 181 | # output='screen' 182 | # ) 183 | 184 | # load_steering_controllers = [ 185 | # ExecuteProcess( 186 | # cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', controller], 187 | # output='screen' 188 | # ) 189 | # for controller in [ 190 | # 'front_left_steer_controller', 191 | # 'front_right_steer_controller', 192 | # 'rear_left_steer_controller', 193 | # 'rear_right_steer_controller' 194 | # ] 195 | # ] 196 | 197 | # load_drive_controllers = [ 198 | # ExecuteProcess( 199 | # cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', controller], 200 | # output='screen' 201 | # ) 202 | # for controller in [ 203 | # 'front_left_drive_controller', 204 | # 'front_right_drive_controller', 205 | # 'rear_left_drive_controller', 206 | # 'rear_right_drive_controller' 207 | # ] 208 | # ] 209 | 210 | # Return the complete launch description 211 | return LaunchDescription([ 212 | robot_state_publisher_node, 213 | joint_state_publisher_node, 214 | # gazebo_server, 215 | # gazebo_client, 216 | # urdf_spawn_node, 217 | controller_manager_node, 218 | velocity_controller, 219 | steering_controller, 220 | joint_broad_spawner, 221 | ignition_launch, 222 | spawn_robot, 223 | bridge_node, 224 | # joint_broad_spawner # Add the controller manager 225 | # load_joint_state_broadcaster, 226 | # *load_steering_controllers, 227 | # *load_drive_controllers 228 | ]) 229 | -------------------------------------------------------------------------------- /launch/joy.launch.py: -------------------------------------------------------------------------------- 1 | from launch import LaunchDescription 2 | from launch_ros.actions import Node 3 | from launch_ros.substitutions import FindPackageShare 4 | from launch.substitutions import PathJoinSubstitution 5 | import os 6 | from ament_index_python.packages import get_package_share_directory 7 | 8 | def generate_launch_description(): 9 | 10 | config = PathJoinSubstitution( 11 | [ 12 | FindPackageShare("amr_description"), 13 | "config", 14 | "joystick.yaml", 15 | ] 16 | ) 17 | 18 | joy_node = Node( 19 | package='joy', 20 | executable='joy_node', 21 | parameters=[config], 22 | ) 23 | 24 | teleop_node = Node( 25 | package='teleop_twist_joy', 26 | executable='teleop_node', 27 | name = 'teleop_node', 28 | parameters=[ 29 | {'enable_button': -1}, 30 | {'require_enable_button': False}, 31 | {'axis_linear.x': 1}, 32 | {'axis_linear.y': 0}, 33 | {'axis_linear.z': 7}, 34 | {'axis_angular.pitch': 7}, 35 | {'axis_angular.roll': 7}, 36 | {'axis_angular.yaw': 3}, 37 | {'scale_linear.x': 4.0}, 38 | {'scale_linear.y': -4.0}, 39 | {'scale_linear.z': 1.0}, 40 | {'scale_angular.pitch': 1.0}, 41 | {'scale_angular.roll': 1.0}, 42 | {'scale_angular.yaw': 1.0} 43 | ], 44 | ) 45 | #... 46 | 47 | # And add to launch description at the bottom 48 | 49 | return LaunchDescription([ 50 | joy_node, 51 | teleop_node 52 | ]) 53 | -------------------------------------------------------------------------------- /meshes/assembly_1_1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yugmil11/Swerve_robot/1c1b4756dfeac756c2d0788b96679bf669dc6243/meshes/assembly_1_1.stl -------------------------------------------------------------------------------- /meshes/assembly_2_1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yugmil11/Swerve_robot/1c1b4756dfeac756c2d0788b96679bf669dc6243/meshes/assembly_2_1.stl -------------------------------------------------------------------------------- /meshes/assembly_3_1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yugmil11/Swerve_robot/1c1b4756dfeac756c2d0788b96679bf669dc6243/meshes/assembly_3_1.stl -------------------------------------------------------------------------------- /meshes/assembly_4_1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yugmil11/Swerve_robot/1c1b4756dfeac756c2d0788b96679bf669dc6243/meshes/assembly_4_1.stl -------------------------------------------------------------------------------- /meshes/base_link.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yugmil11/Swerve_robot/1c1b4756dfeac756c2d0788b96679bf669dc6243/meshes/base_link.stl -------------------------------------------------------------------------------- /meshes/lidar_1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yugmil11/Swerve_robot/1c1b4756dfeac756c2d0788b96679bf669dc6243/meshes/lidar_1.stl -------------------------------------------------------------------------------- /meshes/wheel_1_1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yugmil11/Swerve_robot/1c1b4756dfeac756c2d0788b96679bf669dc6243/meshes/wheel_1_1.stl -------------------------------------------------------------------------------- /meshes/wheel_2_1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yugmil11/Swerve_robot/1c1b4756dfeac756c2d0788b96679bf669dc6243/meshes/wheel_2_1.stl -------------------------------------------------------------------------------- /meshes/wheel_3_1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yugmil11/Swerve_robot/1c1b4756dfeac756c2d0788b96679bf669dc6243/meshes/wheel_3_1.stl -------------------------------------------------------------------------------- /meshes/wheel_4_1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yugmil11/Swerve_robot/1c1b4756dfeac756c2d0788b96679bf669dc6243/meshes/wheel_4_1.stl -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | amr_description 5 | 0.0.0 6 | The amr_description package 7 | author 8 | TODO: License declaration 9 | 10 | xacro 11 | rviz2 12 | robot_state_publisher 13 | joint_state_publisher 14 | joint_state_publisher_gui 15 | 16 | ament_copyright 17 | ament_flake8 18 | ament_pep257 19 | python3-pytest 20 | 21 | 22 | ament_python 23 | 24 | 25 | -------------------------------------------------------------------------------- /resource/amr_description: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Yugmil11/Swerve_robot/1c1b4756dfeac756c2d0788b96679bf669dc6243/resource/amr_description -------------------------------------------------------------------------------- /setup.cfg: -------------------------------------------------------------------------------- 1 | [develop] 2 | script-dir=$base/lib/amr_description 3 | [install] 4 | install-scripts=$base/lib/amr_description 5 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import setup 2 | import os 3 | from glob import glob 4 | 5 | package_name = 'amr_description' 6 | 7 | setup( 8 | name=package_name, 9 | version='0.0.0', 10 | packages=[package_name], 11 | data_files=[ 12 | ('share/ament_index/resource_index/packages', 13 | ['resource/' + package_name]), 14 | ('share/' + package_name, ['package.xml']), 15 | (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')), 16 | (os.path.join('share', package_name, 'urdf'), glob('urdf/*')), 17 | (os.path.join('share', package_name, 'meshes'), glob('meshes/*')), 18 | (os.path.join('share', package_name, 'config'), glob(os.path.join('config/*', '*.yaml'))) 19 | ], 20 | install_requires=['setuptools'], 21 | zip_safe=True, 22 | maintainer='author', 23 | maintainer_email='todo@todo.com', 24 | description='The ' + package_name + ' package', 25 | license='TODO: License declaration', 26 | tests_require=['pytest'], 27 | entry_points={ 28 | 'console_scripts': [ 29 | 'controller = amr_description.controller:main', 30 | ], 31 | }, 32 | ) 33 | -------------------------------------------------------------------------------- /test/test_copyright.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_copyright.main import main 16 | import pytest 17 | 18 | 19 | @pytest.mark.copyright 20 | @pytest.mark.linter 21 | def test_copyright(): 22 | rc = main(argv=['.', 'test']) 23 | assert rc == 0, 'Found errors' 24 | -------------------------------------------------------------------------------- /test/test_flake8.py: -------------------------------------------------------------------------------- 1 | # Copyright 2017 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_flake8.main import main_with_errors 16 | import pytest 17 | 18 | 19 | @pytest.mark.flake8 20 | @pytest.mark.linter 21 | def test_flake8(): 22 | rc, errors = main_with_errors(argv=[]) 23 | assert rc == 0, \ 24 | 'Found %d code style errors / warnings:\n' % len(errors) + \ 25 | '\n'.join(errors) 26 | -------------------------------------------------------------------------------- /test/test_pep257.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_pep257.main import main 16 | import pytest 17 | 18 | 19 | @pytest.mark.linter 20 | @pytest.mark.pep257 21 | def test_pep257(): 22 | rc = main(argv=['.', 'test']) 23 | assert rc == 0, 'Found code style errors / warnings' 24 | -------------------------------------------------------------------------------- /urdf/amr.gazebo: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | mock_components/GenericSystem 9 | 10 | 11 | 12 | -100 13 | 100 14 | 15 | 16 | 17 | 0.0 18 | 19 | 20 | 21 | 22 | -100 23 | 100 24 | 25 | 26 | 27 | 0.0 28 | 29 | 30 | 31 | 32 | -100 33 | 100 34 | 35 | 36 | 37 | 0.0 38 | 39 | 40 | 41 | 42 | -100 43 | 100 44 | 45 | 46 | 47 | 0.0 48 | 49 | 50 | 51 | 52 | 53 | -2.5 54 | 2.5 55 | 56 | 57 | 0.0 58 | 59 | 60 | 61 | 62 | 63 | -2.5 64 | 2.5 65 | 66 | 67 | 0.0 68 | 69 | 70 | 71 | 72 | 73 | -2.5 74 | 2.5 75 | 76 | 77 | 0.0 78 | 79 | 80 | 81 | 82 | 83 | -2.5 84 | 2.5 85 | 86 | 87 | 0.0 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | $(find amr_description)/config/controller.yaml 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | -------------------------------------------------------------------------------- /urdf/amr.trans: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | transmission_interface/SimpleTransmission 6 | 7 | hardware_interface/EffortJointInterface 8 | 9 | 10 | hardware_interface/EffortJointInterface 11 | 1 12 | 13 | 14 | 15 | 16 | transmission_interface/SimpleTransmission 17 | 18 | hardware_interface/EffortJointInterface 19 | 20 | 21 | hardware_interface/EffortJointInterface 22 | 1 23 | 24 | 25 | 26 | 27 | transmission_interface/SimpleTransmission 28 | 29 | hardware_interface/EffortJointInterface 30 | 31 | 32 | hardware_interface/EffortJointInterface 33 | 1 34 | 35 | 36 | 37 | 38 | transmission_interface/SimpleTransmission 39 | 40 | hardware_interface/EffortJointInterface 41 | 42 | 43 | hardware_interface/EffortJointInterface 44 | 1 45 | 46 | 47 | 48 | 49 | transmission_interface/SimpleTransmission 50 | 51 | hardware_interface/EffortJointInterface 52 | 53 | 54 | hardware_interface/EffortJointInterface 55 | 1 56 | 57 | 58 | 59 | 60 | transmission_interface/SimpleTransmission 61 | 62 | hardware_interface/EffortJointInterface 63 | 64 | 65 | hardware_interface/EffortJointInterface 66 | 1 67 | 68 | 69 | 70 | 71 | transmission_interface/SimpleTransmission 72 | 73 | hardware_interface/EffortJointInterface 74 | 75 | 76 | hardware_interface/EffortJointInterface 77 | 1 78 | 79 | 80 | 81 | 82 | transmission_interface/SimpleTransmission 83 | 84 | hardware_interface/EffortJointInterface 85 | 86 | 87 | hardware_interface/EffortJointInterface 88 | 1 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | -------------------------------------------------------------------------------- /urdf/amr.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | 209 | 210 | 211 | 212 | 213 | 214 | 215 | 216 | 217 | 218 | 219 | 220 | 221 | 222 | 223 | 224 | 225 | 226 | 246 | 247 | 248 | 249 | 250 | 251 | 252 | 253 | 254 | 255 | 256 | 257 | 258 | 259 | 260 | 261 | 262 | 263 | 264 | 265 | 266 | 267 | 268 | 269 | 270 | 271 | 272 | 273 | 274 | 275 | 276 | 277 | 278 | 279 | 280 | 281 | 282 | 283 | 284 | 285 | 286 | 287 | 288 | 289 | 290 | 291 | 292 | 293 | 294 | 295 | 296 | 297 | 298 | 299 | 300 | 301 | 302 | 303 | 309 | 310 | 311 | 312 | 313 | 314 | 315 | 316 | 317 | 318 | 319 | 320 | 321 | 322 | 323 | 324 | 325 | 326 | 327 | 328 | 329 | 330 | 331 | 332 | 333 | 334 | 335 | 336 | 337 | 338 | 339 | 340 | 341 | 342 | 343 | 344 | 345 | 346 | 347 | 348 | 349 | 350 | 351 | 352 | 353 | 354 | 355 | 356 | 357 | 358 | 359 | 360 | 361 | 362 | 363 | 364 | 365 | 366 | 367 | 368 | 10.0 369 | true 370 | true 371 | /scan 372 | 373 | 374 | 375 | 180 376 | 1.0 377 | 0 378 | 6.283 379 | 380 | 381 | 1 382 | 1.0 383 | 0 384 | 0 385 | 386 | 387 | 388 | 0.1 389 | 10.0 390 | 391 | 392 | gaussian 393 | 0.0 394 | 0.01 395 | 396 | 397 | 398 | ogre2 399 | 400 | 401 | 402 | 403 | 404 | 405 | 406 | 407 | ign_ros2_control/IgnitionSystem 408 | 409 | 410 | 411 | -100 412 | 100 413 | 414 | 415 | 416 | 0.0 417 | 418 | 419 | 420 | 421 | -100 422 | 100 423 | 424 | 425 | 426 | 0.0 427 | 428 | 429 | 430 | 431 | -100 432 | 100 433 | 434 | 435 | 436 | 0.0 437 | 438 | 439 | 440 | 441 | -100 442 | 100 443 | 444 | 445 | 446 | 0.0 447 | 448 | 449 | 450 | 451 | 452 | -2.5 453 | 2.5 454 | 455 | 456 | 0.0 457 | 458 | 459 | 460 | 461 | 462 | -2.5 463 | 2.5 464 | 465 | 466 | 0.0 467 | 468 | 469 | 470 | 471 | 472 | -2.5 473 | 2.5 474 | 475 | 476 | 0.0 477 | 478 | 479 | 480 | 481 | 482 | -2.5 483 | 2.5 484 | 485 | 486 | 0.0 487 | 488 | 489 | 490 | 491 | 492 | 493 | 494 | /home/yugmil/ros2_ws/src/amr_description/config/controller.yaml 495 | 496 | 497 | 498 | 499 | 500 | true 501 | 502 | true 503 | 25 504 | true 505 | /imu 506 | 507 | 508 | 0 0 0 0 0 0 509 | 510 | 511 | 512 | 513 | 514 | 515 | 518 | true 519 | true 520 | false 521 | false 522 | true 523 | true 524 | true 525 | true 526 | -1 527 | 528 | 529 | 530 | 531 | -------------------------------------------------------------------------------- /urdf/macros.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 36 | 37 | 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /urdf/materials.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | --------------------------------------------------------------------------------