-
Notifications
You must be signed in to change notification settings - Fork 17
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
- Loading branch information
1 parent
515daaf
commit 15c7fe6
Showing
42 changed files
with
2,152 additions
and
62 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
|
|
@@ -83,4 +83,4 @@ def generate_launch_description(): | |
| robot_state_publisher_node_ebot, | ||
| spawn_ebot, | ||
| static_transform | ||
| ]) | ||
| ]) | ||
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,61 @@ | ||
| cmake_minimum_required(VERSION 3.8) | ||
| project(ebot_docking) | ||
|
|
||
| # Default to C99 | ||
| if(NOT CMAKE_C_STANDARD) | ||
| set(CMAKE_C_STANDARD 99) | ||
| endif() | ||
|
|
||
| # Default to C++14 | ||
| if(NOT CMAKE_CXX_STANDARD) | ||
| set(CMAKE_CXX_STANDARD 14) | ||
| endif() | ||
|
|
||
| if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") | ||
| add_compile_options(-Wall -Wextra -Wpedantic) | ||
| endif() | ||
|
|
||
| # find dependencies | ||
| find_package(ament_cmake REQUIRED) | ||
| find_package(rosidl_default_generators REQUIRED) | ||
| find_package(rclpy REQUIRED) | ||
| # uncomment the following section in order to fill in | ||
| # further dependencies manually. | ||
| # find_package(<dependency> REQUIRED) | ||
|
|
||
| rosidl_generate_interfaces(${PROJECT_NAME} | ||
| "srv/DockSw.srv" | ||
| #DEPENDENCIES geometry_msgs # Add packages that above messages depend on, in this case geometry_msgs for Sphere.msg | ||
| ) | ||
|
|
||
| if(BUILD_TESTING) | ||
| find_package(ament_lint_auto REQUIRED) | ||
| # the following line skips the linter which checks for copyrights | ||
| # comment the line when a copyright and license is added to all source files | ||
| set(ament_cmake_copyright_FOUND TRUE) | ||
| # the following line skips cpplint (only works in a git repo) | ||
| # comment the line when this package is in a git repo and when | ||
| # a copyright and license is added to all source files | ||
| set(ament_cmake_cpplint_FOUND TRUE) | ||
| ament_lint_auto_find_test_dependencies() | ||
| endif() | ||
|
|
||
| install( | ||
| DIRECTORY | ||
| srv | ||
| scripts | ||
| DESTINATION | ||
| share/${PROJECT_NAME}/ | ||
| ) | ||
|
|
||
| install( | ||
| PROGRAMS | ||
| DESTINATION lib/${PROJECT_NAME} | ||
| ) | ||
|
|
||
| install(PROGRAMS | ||
| scripts/ebot_docking_boilerplate.py | ||
| DESTINATION lib/${PROJECT_NAME} | ||
| ) | ||
|
|
||
| ament_package() |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,24 @@ | ||
| <?xml version="1.0"?> | ||
| <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?> | ||
| <package format="3"> | ||
| <name>ebot_docking</name> | ||
| <version>0.0.0</version> | ||
| <description>TODO: Package description</description> | ||
| <maintainer email="[email protected]">ravi</maintainer> | ||
| <license>TODO: License declaration</license> | ||
|
|
||
| <buildtool_depend>ament_cmake</buildtool_depend> | ||
|
|
||
| <test_depend>ament_lint_auto</test_depend> | ||
| <test_depend>ament_lint_common</test_depend> | ||
|
|
||
| <build_depend>rosidl_default_generators</build_depend> | ||
|
|
||
| <exec_depend>rosidl_default_runtime</exec_depend> | ||
|
|
||
| <member_of_group>rosidl_interface_packages</member_of_group> | ||
|
|
||
| <export> | ||
| <build_type>ament_cmake</build_type> | ||
| </export> | ||
| </package> |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,136 @@ | ||
| #!/usr/bin/env python3 | ||
|
|
||
| ## Overview | ||
|
|
||
| # ### | ||
| # This ROS2 script is designed to control a robot's docking behavior with a rack. | ||
| # It utilizes odometry data, ultrasonic sensor readings, and provides docking control through a custom service. | ||
| # The script handles both linear and angular motion to achieve docking alignment and execution. | ||
| # ### | ||
|
|
||
| # Import necessary ROS2 packages and message types | ||
| import rclpy | ||
| from rclpy.node import Node | ||
| from nav_msgs.msg import Odometry | ||
| from geometry_msgs.msg import Twist | ||
| from sensor_msgs.msg import Range | ||
| from rclpy.callback_groups import ReentrantCallbackGroup | ||
| from rclpy.executors import MultiThreadedExecutor | ||
| from tf_transformations import euler_from_quaternion | ||
| from ebot_docking.srv import DockSw # Import custom service message | ||
| import math, statistics | ||
|
|
||
| # Define a class for your ROS2 node | ||
| class MyRobotDockingController(Node): | ||
|
|
||
| def __init__(self): | ||
| # Initialize the ROS2 node with a unique name | ||
| super().__init__('my_robot_docking_controller') | ||
|
|
||
| # Create a callback group for managing callbacks | ||
| self.callback_group = ReentrantCallbackGroup() | ||
|
|
||
| # Subscribe to odometry data for robot pose information | ||
| self.odom_sub = self.create_subscription(Odometry, 'odom', self.odometry_callback, 10) | ||
|
|
||
| # Subscribe to ultrasonic sensor data for distance measurements | ||
| self.ultrasonic_rl_sub = self.create_subscription(Range, '/ultrasonic_rl/scan', self.ultrasonic_rl_callback, 10) | ||
| # Add another one here | ||
|
|
||
|
|
||
| # Create a ROS2 service for controlling docking behavior, can add another custom service message | ||
| self.dock_control_srv = self.create_service(DockSw, 'dock_control', self.dock_control_callback, callback_group=self.callback_group) | ||
|
|
||
| # Create a publisher for sending velocity commands to the robot | ||
| # | ||
|
|
||
| # Initialize all flags and parameters here | ||
| self.is_docking = False | ||
| # | ||
| # | ||
| # | ||
| # | ||
| # | ||
| # | ||
|
|
||
| # Initialize a timer for the main control loop | ||
| self.controller_timer = self.create_timer(0.1, self.controller_loop) | ||
|
|
||
| # Callback function for odometry data | ||
| def odometry_callback(self, msg): | ||
| # Extract and update robot pose information from odometry message | ||
| self.robot_pose[0] = msg.pose.pose.position.x | ||
| self.robot_pose[1] = msg.pose.pose.position.y | ||
| quaternion_array = msg.pose.pose.orientation | ||
| orientation_list = [quaternion_array.x, quaternion_array.y, quaternion_array.z, quaternion_array.w] | ||
| _, _, yaw = euler_from_quaternion(orientation_list) | ||
| self.robot_pose[2] = yaw | ||
|
|
||
| # Callback function for the left ultrasonic sensor | ||
| def ultrasonic_rl_callback(self, msg): | ||
| self.usrleft_value = msg.range | ||
|
|
||
| # Callback function for the right ultrasonic sensor | ||
| # | ||
| # | ||
|
|
||
| # Utility function to normalize angles within the range of -π to π (OPTIONAL) | ||
| def normalize_angle(self, angle): | ||
|
|
||
| pass | ||
|
|
||
| # Main control loop for managing docking behavior | ||
|
|
||
| def controller_loop(self): | ||
|
|
||
| # The controller loop manages the robot's linear and angular motion | ||
| # control to achieve docking alignment and execution | ||
| if self.is_docking: | ||
| # ... | ||
| # Implement control logic here for linear and angular motion | ||
| # For example P-controller is enough, what is P-controller go check it out ! | ||
| # ... | ||
| pass | ||
|
|
||
| # Callback function for the DockControl service | ||
| def dock_control_callback(self, request, response): | ||
| # Extract desired docking parameters from the service request | ||
| # | ||
| # | ||
|
|
||
| # Reset flags and start the docking process | ||
| # | ||
| # | ||
|
|
||
| # Log a message indicating that docking has started | ||
| self.get_logger().info("Docking started!") | ||
|
|
||
| # Create a rate object to control the loop frequency | ||
| rate = self.create_rate(2, self.get_clock()) | ||
|
|
||
| # Wait until the robot is aligned for docking | ||
| while not self.dock_aligned: | ||
| self.get_logger().info("Waiting for alignment...") | ||
| rate.sleep() | ||
|
|
||
| # Set the service response indicating success | ||
| response.success = True | ||
| response.message = "Docking control initiated" | ||
| return response | ||
|
|
||
| # Main function to initialize the ROS2 node and spin the executor | ||
| def main(args=None): | ||
| rclpy.init(args=args) | ||
|
|
||
| my_robot_docking_controller = MyRobotDockingController() | ||
|
|
||
| executor = MultiThreadedExecutor() | ||
| executor.add_node(my_robot_docking_controller) | ||
|
|
||
| executor.spin() | ||
|
|
||
| my_robot_docking_controller.destroy_node() | ||
| rclpy.shutdown() | ||
|
|
||
| if __name__ == '__main__': | ||
| main() |
Oops, something went wrong.