commit 3f32c8a405d59b25253ff5f690aa4844c516d1ff Author: Marcin M Date: Mon Jun 1 22:24:35 2026 +0200 INIT: working robo-arm diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..1345657 --- /dev/null +++ b/.gitignore @@ -0,0 +1,3 @@ +build/ +install/ +log/ diff --git a/src/arm_simulation/CMakeLists.txt b/src/arm_simulation/CMakeLists.txt new file mode 100644 index 0000000..8de6721 --- /dev/null +++ b/src/arm_simulation/CMakeLists.txt @@ -0,0 +1,37 @@ +cmake_minimum_required(VERSION 3.8) +project(arm_simulation) + +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(rclpy REQUIRED) +find_package(std_msgs REQUIRED) +find_package(trajectory_msgs REQUIRED) + +install( + DIRECTORY launch urdf config scripts + DESTINATION share/${PROJECT_NAME} +) + +# Install Python scripts directly to the lib folder so 'ros2 run' can find them +install(PROGRAMS + scripts/move_arm.py + DESTINATION lib/${PROJECT_NAME} +) + +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() + +ament_package() diff --git a/src/arm_simulation/config/ros2_controllers.yaml b/src/arm_simulation/config/ros2_controllers.yaml new file mode 100644 index 0000000..c40f608 --- /dev/null +++ b/src/arm_simulation/config/ros2_controllers.yaml @@ -0,0 +1,24 @@ +controller_manager: + ros__parameters: + update_rate: 100 # Hz + use_sim_time: true + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + arm_controller: + type: joint_trajectory_controller/JointTrajectoryController + +arm_controller: + ros__parameters: + joints: + - joint_1 + - joint_2 + - joint_3 + - joint_4 + command_interfaces: + - position + state_interfaces: + - position + - velocity + use_sim_time: true \ No newline at end of file diff --git a/src/arm_simulation/launch/sim.launch.py b/src/arm_simulation/launch/sim.launch.py new file mode 100644 index 0000000..632f81b --- /dev/null +++ b/src/arm_simulation/launch/sim.launch.py @@ -0,0 +1,107 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription, RegisterEventHandler +from launch.event_handlers import OnProcessExit +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node +import xacro + + +def generate_launch_description(): + # 1. Define package name and paths + package_name = "arm_simulation" + pkg_share = get_package_share_directory(package_name) + + # Path to your XACRO file + xacro_file = os.path.join(pkg_share, "urdf", "arm.urdf.xacro") + + # Process XACRO to raw URDF string + robot_description_config = xacro.process_file(xacro_file) + robot_description = {"robot_description": robot_description_config.toxml()} + + # 2. Robot State Publisher Node + # This broadcasts the robot's coordinate frames and transforms (TF) + node_robot_state_publisher = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="screen", + parameters=[robot_description], + ) + + # 3. Gazebo Harmonic Launch + # Includes the official modern Gazebo launch script. + # '-r' tells Gazebo to unpause the physics engine automatically on startup. + gazebo = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join( + get_package_share_directory("ros_gz_sim"), + "launch", + "gz_sim.launch.py", + ) + ), + launch_arguments={"gz_args": "-r empty.sdf"}.items(), + ) + + # 4. Spawn Robot Node (Modern Gazebo Creator) + # Replaces the old 'spawn_entity.py'. It grabs the URDF via the /robot_description topic. + spawn_entity = Node( + package="ros_gz_sim", + executable="create", + arguments=["-topic", "robot_description", "-name", "simple_arm"], + output="screen", + ) + + # 5. Joint State Broadcaster Spawner + joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=[ + "joint_state_broadcaster", + "--controller-manager", + "/controller_manager", + ], + ) + + # 6. Arm Controller Spawner + # Loads the trajectory controller managing your arm's joint configuration. + arm_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=[ + "arm_controller", + "--controller-manager", + "/controller_manager", + ], + ) + + # --- Event Handlers (Timing Control) --- + # We must delay activating controllers until the Gazebo simulator is fully running + # and has registered the 'ros2_control' plugin manager inside the robot entity. + + # Delay loading joint_state_broadcaster until AFTER the robot is physically spawned + delay_joint_state_broadcaster = RegisterEventHandler( + event_handler=OnProcessExit( + target_action=spawn_entity, + on_exit=[joint_state_broadcaster_spawner], + ) + ) + + # Delay loading arm_controller until AFTER the joint_state_broadcaster is fully active + delay_arm_controller = RegisterEventHandler( + event_handler=OnProcessExit( + target_action=joint_state_broadcaster_spawner, + on_exit=[arm_controller_spawner], + ) + ) + + # Return the launch description with all processes grouped + return LaunchDescription( + [ + node_robot_state_publisher, + gazebo, + spawn_entity, + delay_joint_state_broadcaster, + delay_arm_controller, + ] + ) \ No newline at end of file diff --git a/src/arm_simulation/package.xml b/src/arm_simulation/package.xml new file mode 100644 index 0000000..06013c4 --- /dev/null +++ b/src/arm_simulation/package.xml @@ -0,0 +1,24 @@ + + + + arm_simulation + 0.0.0 + TODO: Package description + marcin + TODO: License declaration + + ament_cmake + + rclpy + std_msgs + trajectory_msgs + ros_gz_sim + gz_ros2_control + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/arm_simulation/scripts/move_arm.py b/src/arm_simulation/scripts/move_arm.py new file mode 100755 index 0000000..af0d6c3 --- /dev/null +++ b/src/arm_simulation/scripts/move_arm.py @@ -0,0 +1,59 @@ +#!/usr/bin/env python3 +import rclpy +from rclpy.node import Node +from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint +from builtin_interfaces.msg import Duration + +class ArmTrajectoryPublisher(Node): + + def __init__(self): + super().__init__('arm_trajectory_publisher') + + # 1. Create a publisher targeting the trajectory controller topic + self.publisher_ = self.create_publisher( + JointTrajectory, + '/arm_controller/joint_trajectory', + 10 + ) + + # 2. Wait 2 seconds to make sure connections are secure, then execute + self.timer = self.create_timer(2.0, self.move_robot_arm) + self.get_logger().info('Arm controller node initialized. Waiting to send command...') + + def move_robot_arm(self): + # Cancel the timer so this command only sends once + self.timer.cancel() + + # 3. Initialize the trajectory message structure + msg = JointTrajectory() + + # These names MUST match the joint names defined in your URDF and YAML files exactly + msg.joint_names = ['joint_1', 'joint_2', 'joint_3', 'joint_4'] + + # 4. Create a waypoint goal + point = JointTrajectoryPoint() + + # Target positions in Radians (joint_1 = 90 degrees, joint_2 = 45 degrees) + # Example position: joint_1=0.0, joint_2=0.5, joint_3=0.5, joint_4=-0.5 + point.positions = [0.0, 0.5, 0.5, -0.5] + + # Tell the physics engine to take exactly 4.0 seconds to execute this transition smoothly + point.time_from_start = Duration(sec=4, nanosec=0) + + # Append the waypoint to the trajectory list + msg.points.append(point) + + # 5. Publish the message to Gazebo + self.get_logger().info('Sending smooth trajectory command to joint_1 and joint_2!') + self.publisher_.publish(msg) + + +def main(args=None): + rclpy.init(args=args) + node = ArmTrajectoryPublisher() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/src/arm_simulation/urdf/arm.urdf.xacro b/src/arm_simulation/urdf/arm.urdf.xacro new file mode 100644 index 0000000..f29f4fd --- /dev/null +++ b/src/arm_simulation/urdf/arm.urdf.xacro @@ -0,0 +1,174 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + gz_ros2_control/GazeboSimSystem + + + + -3.143.14 + 0.0 + + + + + -1.571.57 + 0.0 + + + + + -2.02.0 + 0.0 + + + + + -1.571.57 + 0.0 + + + + + + + $(find arm_simulation)/config/ros2_controllers.yaml + + + + 0.1 0.1 0.1 1.00.1 0.1 0.1 1.0 + 0.0 0.0 0.8 1.00.0 0.0 0.8 1.0 + 0.5 0.5 0.5 1.00.5 0.5 0.5 1.0 + 0.0 0.0 0.8 1.00.0 0.0 0.8 1.0 + 0.8 0.0 0.0 1.00.8 0.0 0.0 1.0 + + \ No newline at end of file