diff --git a/240628/src/turtle_pub/CMakeLists.txt b/240628/src/turtle_pub/CMakeLists.txt new file mode 100644 index 0000000..9d86868 --- /dev/null +++ b/240628/src/turtle_pub/CMakeLists.txt @@ -0,0 +1,46 @@ +# cmake_minimum_required(VERSION 3.5) +# project(turtle_pub) + +# # 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() + +# find_package(ament_cmake REQUIRED) +# find_package(rclpy REQUIRED) +# find_package(geometry_msgs REQUIRED) +# find_package(rosidl_default_generators REQUIRED) + +# # Generate service interfaces +# rosidl_generate_interfaces(${PROJECT_NAME} +# "srv/DrawCircle.srv" +# ) + +# ament_export_dependencies(rosidl_default_runtime) + +# ament_package() + + +cmake_minimum_required(VERSION 3.5) +project(turtle_pub) + +# Dependencies +find_package(ament_cmake REQUIRED) +find_package(rclpy REQUIRED) +find_package(std_srvs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +# Generate interfaces +rosidl_generate_interfaces(${PROJECT_NAME} + "srv/DrawCircle.srv" +) + +ament_export_dependencies(rosidl_default_runtime) + +ament_package() diff --git a/240628/src/turtle_pub/package.xml b/240628/src/turtle_pub/package.xml new file mode 100644 index 0000000..75787e6 --- /dev/null +++ b/240628/src/turtle_pub/package.xml @@ -0,0 +1,27 @@ + + turtle_pub + 0.0.0 + TODO: Package description + vivekananda + TODO: License declaration + + + rclpy + geometry_msgs + std_srvs + + + rosidl_default_generators + rosidl_default_runtime + rosidl_interface_packages + + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/240628/src/turtle_pub/resource/turtle_pub b/240628/src/turtle_pub/resource/turtle_pub new file mode 100644 index 0000000..e69de29 diff --git a/240628/src/turtle_pub/setup.cfg b/240628/src/turtle_pub/setup.cfg new file mode 100644 index 0000000..a84b69e --- /dev/null +++ b/240628/src/turtle_pub/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/turtle_pub +[install] +install_scripts=$base/lib/turtle_pub diff --git a/240628/src/turtle_pub/setup.py b/240628/src/turtle_pub/setup.py new file mode 100644 index 0000000..4495c59 --- /dev/null +++ b/240628/src/turtle_pub/setup.py @@ -0,0 +1,30 @@ +from setuptools import find_packages, setup +import os +from glob import glob + +package_name = 'turtle_pub' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + (os.path.join('share', package_name, 'srv'), glob('srv/*.srv')) + ], + install_requires=['setuptools', 'rosidl_default_generators'], + zip_safe=True, + maintainer='vivekananda', + maintainer_email='vivekananda@todo.todo', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'draw_circle_service=turtle_pub.draw_circle_service:main', + 'move_circle = turtle_pub.move_circle:main', + ], + }, +) \ No newline at end of file diff --git a/240628/src/turtle_pub/srv/DrawCircle.srv b/240628/src/turtle_pub/srv/DrawCircle.srv new file mode 100644 index 0000000..719623c --- /dev/null +++ b/240628/src/turtle_pub/srv/DrawCircle.srv @@ -0,0 +1,6 @@ +float64 x +float64 y +float64 radius +--- +bool success +string message \ No newline at end of file diff --git a/240628/src/turtle_pub/test/test_copyright.py b/240628/src/turtle_pub/test/test_copyright.py new file mode 100644 index 0000000..97a3919 --- /dev/null +++ b/240628/src/turtle_pub/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/240628/src/turtle_pub/test/test_flake8.py b/240628/src/turtle_pub/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/240628/src/turtle_pub/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/240628/src/turtle_pub/test/test_pep257.py b/240628/src/turtle_pub/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/240628/src/turtle_pub/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/240628/src/turtle_pub/turtle_pub/__init__.py b/240628/src/turtle_pub/turtle_pub/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/240628/src/turtle_pub/turtle_pub/draw_circle_service.py b/240628/src/turtle_pub/turtle_pub/draw_circle_service.py new file mode 100644 index 0000000..3a67c39 --- /dev/null +++ b/240628/src/turtle_pub/turtle_pub/draw_circle_service.py @@ -0,0 +1,102 @@ +import rclpy +from rclpy.node import Node +from geometry_msgs.msg import Twist +from turtlesim.msg import Pose +from turtlesim.srv import TeleportAbsolute +from turtle_pub.srv import DrawCircle +import math +import time + +class TurtleController(Node): + def __init__(self): + super().__init__('turtle_controller') + self.get_logger().info('Turtle Controller Node started.') + + # Create a publisher for the cmd_vel topic + self.publisher_ = self.create_publisher(Twist, '/turtle1/cmd_vel', 10) + self.pose = Pose() + + # Subscribe to the pose topic to get the turtle's current position + self.create_subscription(Pose, '/turtle1/pose', self.pose_callback, 10) + + # Create the service server for drawing the circle + self.srv = self.create_service(DrawCircle, 'draw_circle', self.draw_circle_callback) + self.teleport_client = self.create_client(TeleportAbsolute, '/turtle1/teleport_absolute') + + def pose_callback(self, msg): + self.pose = msg + + def draw_circle_callback(self, request, response): + self.get_logger().info(f'Received request to draw a circle at x={request.x}, y={request.y} with radius={request.radius}') + + # Phase 1: Teleport to the specified coordinates + self.teleport_turtle(request.x, request.y, 0.0) + + # Phase 2: Move along the positive x-axis + self.move_straight(request.radius) + + # Phase 3: Move in a circular path + self.move_in_circle(request.radius) + + response.success = True + response.message = "Cool success! The TurtleBot completed its movements." + self.get_logger().info('TurtleBot completed movements.') + return response + + def teleport_turtle(self, x, y, theta): + self.get_logger().info(f'Teleporting to x={x}, y={y}, theta={theta}') + while not self.teleport_client.wait_for_service(timeout_sec=1.0): + self.get_logger().info('Teleport service not available, waiting...') + + req = TeleportAbsolute.Request() + req.x = x + req.y = y + req.theta = theta + self.teleport_client.call_async(req) + time.sleep(1) # Give time for the teleport to complete + + def move_straight(self, distance): + self.get_logger().info(f'Moving straight for distance: {distance}') + + start_x = self.pose.x + start_y = self.pose.y + cmd_vel_msg = Twist() + cmd_vel_msg.linear.x = 1.0 # Move forward at a speed of 1.0 m/s + + while self.pose.x < (start_x + distance): + self.publisher_.publish(cmd_vel_msg) + rclpy.spin_once(self) + + # Stop the turtle + cmd_vel_msg.linear.x = 0.0 + self.publisher_.publish(cmd_vel_msg) + + def move_in_circle(self, radius): + self.get_logger().info(f'Moving in a circular path with radius: {radius}') + + cmd_vel_msg = Twist() + cmd_vel_msg.linear.x = 1.0 + cmd_vel_msg.angular.z = 1.0 / radius + + # Move for one full circle. Time = 2*pi*r / v = 2*pi / w + duration = (2 * math.pi) / cmd_vel_msg.angular.z + start_time = time.time() + + while time.time() - start_time < duration: + self.publisher_.publish(cmd_vel_msg) + rclpy.spin_once(self) + + # Stop the turtle + cmd_vel_msg.linear.x = 0.0 + cmd_vel_msg.angular.z = 0.0 + self.publisher_.publish(cmd_vel_msg) + +def main(args=None): + rclpy.init(args=args) + turtle_controller = TurtleController() + rclpy.spin(turtle_controller) + turtle_controller.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/240628/src/turtle_pub/turtle_pub/move_circle.py b/240628/src/turtle_pub/turtle_pub/move_circle.py new file mode 100644 index 0000000..41a6c95 --- /dev/null +++ b/240628/src/turtle_pub/turtle_pub/move_circle.py @@ -0,0 +1,25 @@ +import rclpy +from rclpy.node import Node +from geometry_msgs.msg import Twist + +class MoveCircle(Node): + def __init__(self): + super().__init__('move_circle') + self.pub = self.create_publisher(Twist, '/cmd_vel', 10) + timer_period = 0.1 + self.timer = self.create_timer(timer_period, self.publish_cmd) + + def publish_cmd(self): + vel = Twist() + vel.linear.x = 0.2 + vel.angular.z = 0.2 + self.pub.publish(vel) + +def main(args=None): + rclpy.init(args=args) + node = MoveCircle() + rclpy.spin(node) + rclpy.shutdown() + +if __name__ == '__main__': + main()