feat: Assignment 5

.fish/activate.fish

@@ -1,5 +1,5 @@
 # Self-contained environment: ros2-lectures
-# Exported on: Thu Dec  4 09:45:42 AM CET 2025
+# Exported on: Sat Dec 13 05:03:27 PM CET 2025
 # Original environment from: /home/wessel/.config/fish/environments/configs/ros2-lectures
 
 # ROS2 development environment (requires distrobox)
@@ -63,10 +63,6 @@ alias cbt="colcon build --packages-select"
 alias ct="colcon test"
 alias ctr="colcon test-result"
 
-# For communication between nodes on other devices
-export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
-export ROS_DOMAIN_ID=5
-
 echo (set_color green)"Activated ROS2 environment: ros2-lectures"(set_color normal)
 
 # Custom deactivation function

src/pose_calculator/CMakeLists.txt

@@ -0,0 +1,36 @@
+cmake_minimum_required(VERSION 3.8)
+project(pose_calculator)
+
+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(rclcpp REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+
+# set dependencies
+set(dependencies
+rclcpp
+tf2_ros
+geometry_msgs
+sensor_msgs
+)
+
+# add executables for scripts
+add_executable(ik_node src/InverseKinematicNode.cpp)
+ament_target_dependencies(ik_node ${dependencies})
+
+# install the scripts
+install(TARGETS
+ik_node
+DESTINATION lib/${PROJECT_NAME})
+install(
+DIRECTORY launch
+DESTINATION share/${PROJECT_NAME}
+)
+
+ament_package()

src/pose_calculator/launch/inverse_kinematic.launch.xml

@@ -0,0 +1,20 @@
+<launch>
+    <!-- Launch arguments for target position -->
+    <arg name="target_x" default="0.29" description="Target X position"/>
+    <arg name="target_y" default="0.29" description="Target Y position"/>
+
+    <let name="urdf_path" value="$(find-pkg-share robot_description)/urdf/skyentific_robot.urdf.xacro"/>
+    <let name="rviz_config_path" value="$(find-pkg-share robot_description)/rviz/display.rviz"/>
+
+    <node pkg="robot_state_publisher" exec="robot_state_publisher">
+        <param name="robot_description" value="$(command 'xacro $(var urdf_path)')" />
+    </node>
+
+    <node pkg="pose_calculator" exec="ik_node">
+        <param name="target_x" value="$(var target_x)"/>
+        <param name="target_y" value="$(var target_y)"/>
+    </node>
+
+    <node pkg="rviz2" exec="rviz2" output="screen" args="-d $(var rviz_config_path)" />
+    <node pkg="rqt_reconfigure" exec="rqt_reconfigure" output="screen" />
+</launch>

src/pose_calculator/package.xml

@@ -1,16 +1,17 @@
 <?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>soup_relay_pkg</name>
+  <name>pose_calculator</name>
   <version>0.0.0</version>
   <description>TODO: Package description</description>
   <maintainer email="contact@wessel.gg">wessel</maintainer>
   <license>TODO: License declaration</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
-
     <depend>rclcpp</depend>
-  <depend>std_msgs</depend>
+    <depend>tf2_ros</depend>
+    <depend>geometry_msgs</depend>
+    <depend>sensor_msgs</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

src/pose_calculator/src/InverseKinematicNode.cpp

@@ -0,0 +1,193 @@
+#include <cmath>
+
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/joint_state.hpp>
+
+class InverseKinematicNode : public rclcpp::Node {
+public:
+    InverseKinematicNode() : Node("skyentific_pose_calculator_node") {
+        auto target_x_desc = rcl_interfaces::msg::ParameterDescriptor();
+        target_x_desc.description = "Target X position";
+        target_x_desc.read_only = false;
+        target_x_desc.floating_point_range.resize(1);
+        target_x_desc.floating_point_range[0].from_value = -0.5;
+        target_x_desc.floating_point_range[0].to_value = 0.5;
+        target_x_desc.floating_point_range[0].step = 0.01;
+
+        auto target_y_desc = rcl_interfaces::msg::ParameterDescriptor();
+        target_y_desc.description = "Target Y position";
+        target_y_desc.read_only = false;
+        target_y_desc.floating_point_range.resize(1);
+        target_y_desc.floating_point_range[0].from_value = -0.5;
+        target_y_desc.floating_point_range[0].to_value = 0.5;
+        target_y_desc.floating_point_range[0].step = 0.01;
+
+        target_x_ = this->declare_parameter<double>("target_x", 0.29, target_x_desc);
+        target_y_ = this->declare_parameter<double>("target_y", 0.29, target_y_desc);
+
+        link1_ = this->declare_parameter<double>("L1", 0.225);
+        link2_ = this->declare_parameter<double>("L2", 0.383);
+
+        // Set up parameter callback for dynamic reconfigure
+        param_callback_handle_ = this->add_on_set_parameters_callback(
+            std::bind(&InverseKinematicNode::callback_parameters, this, std::placeholders::_1)
+        );
+
+        if (!calculate_2dpose(target_x_, target_y_)) {
+            RCLCPP_ERROR(this->get_logger(), "failed calculating pose (x=%f, y=%f)", target_x_, target_y_);
+        } else {
+            RCLCPP_INFO(this->get_logger(),
+                "target position (x=%f, y=%f) [j1=%f, j2=%f, j3=%f, j4=%f, j5=%f, j6=%f]",
+                target_x_, target_y_, joint_1_, joint_2_, joint_3_, joint_4_, joint_5_, joint_6_
+            );
+        }
+
+        joint_state_publisher_ = this->create_publisher<sensor_msgs::msg::JointState>("joint_states", 10);
+
+        timer_ = this->create_wall_timer(
+            std::chrono::milliseconds(100),
+            std::bind(&InverseKinematicNode::publish_joint_state, this));
+    }
+
+private:
+    rcl_interfaces::msg::SetParametersResult callback_parameters(
+        const std::vector<rclcpp::Parameter> &parameters
+    ) {
+
+        rcl_interfaces::msg::SetParametersResult result;
+        result.successful = true;
+
+        bool recalculate = false;
+        double new_x = target_x_;
+        double new_y = target_y_;
+
+        for (const auto &param : parameters) {
+            if (param.get_name() == "target_x") {
+                new_x = param.as_double();
+                recalculate = true;
+                RCLCPP_INFO(this->get_logger(), "changed 'target_x' -> %.4f", new_x);
+            } else if (param.get_name() == "target_y") {
+                new_y = param.as_double();
+                recalculate = true;
+                RCLCPP_INFO(this->get_logger(), "changed 'target_y' -> %.4f", new_y);
+            }
+        }
+
+        if (recalculate) {
+            if (calculate_2dpose(new_x, new_y)) {
+                target_x_ = new_x;
+                target_y_ = new_y;
+                result.successful = true;
+                result.reason = "pose calculation successful";
+            } else {
+                result.successful = false;
+                result.reason = "target position unreachable";
+                RCLCPP_WARN(this->get_logger(), "failed to calculate pose, keeping previous values");
+            }
+        }
+
+        return result;
+    }
+
+private:
+    OnSetParametersCallbackHandle::SharedPtr param_callback_handle_;
+    rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr joint_state_publisher_;
+    rclcpp::TimerBase::SharedPtr timer_;
+
+    double target_x_, target_y_;
+    double link1_, link2_;
+    double joint_1_, joint_2_, joint_3_, joint_4_, joint_5_, joint_6_;
+
+    bool calculate_2dpose(double x, double y) {
+        RCLCPP_INFO(this->get_logger(), "0. x=%.4f, y=%.4f, l1=%.4f, l2=%.4f", x, y, link1_, link2_);
+
+        double r = std::sqrt(x * x + y * y);
+        RCLCPP_INFO(this->get_logger(), "1. r=%.6f", r);
+
+        // joint 1 = base rotation
+        joint_1_ = 0.0;
+        // joint_1_ = std::atan2(y, x);
+
+        // Check if target is reachable
+        double max_reach = link1_ + link2_;
+        double min_reach = std::abs(link1_ - link2_);
+        RCLCPP_INFO(this->get_logger(), "3. reach_min=%.4f, reach_max=%.4f", min_reach, max_reach);
+
+        if (r > max_reach || r < min_reach) {
+            RCLCPP_ERROR(this->get_logger(), "e. target out of reach, aborting");
+            return false;
+        }
+
+        // Joint 2: cos(Joint2) = (r^2 - Link1^2 - Link2^2) / (2 * Link1 * Link2)
+        double r_squared = r * r;
+        double link1_squared = link1_ * link1_;
+        double link2_squared = link2_ * link2_;
+
+        double cos_joint2 = (r_squared - link1_squared - link2_squared) / (2.0 * link1_ * link2_);
+
+        RCLCPP_INFO(this->get_logger(),
+            "4. cos_joint2=(%.6f - %.6f - %.6f) / (2*%.3f*%.3f) = %.6f",
+            r_squared, link1_squared, link2_squared, link1_, link2_, cos_joint2
+        );
+
+        joint_2_ = std::acos(cos_joint2);
+        RCLCPP_INFO(this->get_logger(), "4. joint_2=%.6f rad", joint_2_);
+
+        // Joint 3: Joint3 = atan2(y, x) - atan2(Link2*sin(Joint2), Link1 + Link2*cos(Joint2))
+        double atan2_yx = std::atan2(y, x);
+        double link2_sin_j2 = link2_ * std::sin(joint_2_);
+        double link1_plus_link2_cos_joint2 = link1_ + link2_ * std::cos(joint_2_);
+        double atan2_term = std::atan2(link2_sin_j2, link1_plus_link2_cos_joint2);
+
+        RCLCPP_INFO(this->get_logger(), "5. atan2(%.2f, %.2f) = %.6f", y, x, atan2_yx);
+        RCLCPP_INFO(this->get_logger(), "5. atan2(%.3f*sin(%.6f), %.3f + %.3f*cos(%.6f))",
+            link2_, joint_2_, link1_, link2_, joint_2_
+        );
+        RCLCPP_INFO(this->get_logger(), "5. atan2(%.6f, %.6f) = %.6f",
+            link2_sin_j2, link1_plus_link2_cos_joint2, atan2_term
+        );
+
+        joint_3_ = atan2_yx - atan2_term;
+        RCLCPP_INFO(this->get_logger(), "5. joint_3=%.6f - %.6f=%.6f rad", atan2_yx, atan2_term, joint_3_);
+
+        // joints 4-6 related to gripper
+        joint_4_ = 0.0;
+        joint_5_ = 0.0;
+        joint_6_ = 0.0;
+
+        return true;
+    }
+
+    void publish_joint_state() {
+        auto joint_state = sensor_msgs::msg::JointState();
+        joint_state.header.stamp = this->get_clock()->now();
+
+        joint_state.name = {
+            "joint_1",
+            "joint_2",
+            "joint_3",
+            "joint_4",
+            "joint_5",
+            "joint_6"
+        };
+
+        joint_state.position = {
+            joint_1_,
+            joint_2_,
+            joint_3_,
+            joint_4_,
+            joint_5_,
+            joint_6_
+        };
+
+        joint_state_publisher_->publish(joint_state);
+    }
+};
+
+int main(int argc, char** argv) {
+    rclcpp::init(argc, argv);
+    auto node = std::make_shared<InverseKinematicNode>();
+    rclcpp::spin(node);
+    rclcpp::shutdown();
+    return 0;
+}

src/robot_description/CMakeLists.txt

@@ -0,0 +1,17 @@
+cmake_minimum_required(VERSION 3.8)
+project(robot_description)
+
+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)
+
+# Install directories
+install(
+    DIRECTORY launch meshes urdf rviz
+    DESTINATION share/${PROJECT_NAME}
+)
+
+ament_package()

src/robot_description/launch/display.launch.xml

@@ -0,0 +1,26 @@
+<launch>
+    <let name="urdf_path"
+        value="$(find-pkg-share robot_description)/urdf/skyentific_robot.urdf.xacro"
+    />
+
+    <let name="rviz_config_path"
+        value="$(find-pkg-share robot_description)/rviz/display.rviz"
+    />
+
+    <node pkg="robot_state_publisher"
+        exec="robot_state_publisher"
+    >
+        <param name="robot_description"
+        value="$(command 'xacro $(var urdf_path)')"/>
+    </node>
+
+    <node pkg="joint_state_publisher_gui"
+        exec="joint_state_publisher_gui"
+    />
+
+    <node pkg="rviz2"
+        exec="rviz2"
+        output="screen"
+        args="-d $(var rviz_config_path)"
+    />
+</launch>

src/robot_description/meshes/readme.md

@@ -0,0 +1 @@
+meshes to be used with lesson 1

src/robot_description/package.xml

@@ -0,0 +1,15 @@
+<?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>robot_description</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="wessel@go2it.eu">wessel</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/robot_description/rviz/display.rviz

@@ -0,0 +1,239 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 85
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /RobotModel1/Description Topic1
+      Splitter Ratio: 0.5
+    Tree Height: 517
+  - Class: rviz_common/Selection
+    Name: Selection
+  - Class: rviz_common/Tool Properties
+    Expanded:
+      - /2D Goal Pose1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz_common/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz_common/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz_default_plugins/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Class: rviz_default_plugins/RobotModel
+      Collision Enabled: false
+      Description File: ""
+      Description Source: Topic
+      Description Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /robot_description
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        arm_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        elbow_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        forearm_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        hand_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        shoulder_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        tool_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        wrist_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Mass Properties:
+        Inertia: false
+        Mass: false
+      Name: RobotModel
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+    - Class: rviz_default_plugins/TF
+      Enabled: true
+      Filter (blacklist): ""
+      Filter (whitelist): ""
+      Frame Timeout: 15
+      Frames:
+        All Enabled: true
+        arm_link:
+          Value: true
+        base_link:
+          Value: true
+        elbow_link:
+          Value: true
+        forearm_link:
+          Value: true
+        hand_link:
+          Value: true
+        shoulder_link:
+          Value: true
+        tool_link:
+          Value: true
+        wrist_link:
+          Value: true
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: false
+      Tree:
+        base_link:
+          shoulder_link:
+            arm_link:
+              elbow_link:
+                forearm_link:
+                  wrist_link:
+                    hand_link:
+                      tool_link:
+                        {}
+      Update Interval: 0
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: base_link
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz_default_plugins/Interact
+      Hide Inactive Objects: true
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+    - Class: rviz_default_plugins/FocusCamera
+    - Class: rviz_default_plugins/Measure
+      Line color: 128; 128; 0
+    - Class: rviz_default_plugins/SetInitialPose
+      Covariance x: 0.25
+      Covariance y: 0.25
+      Covariance yaw: 0.06853891909122467
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /initialpose
+    - Class: rviz_default_plugins/SetGoal
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /goal_pose
+    - Class: rviz_default_plugins/PublishPoint
+      Single click: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /clicked_point
+  Transformation:
+    Current:
+      Class: rviz_default_plugins/TF
+  Value: true
+  Views:
+    Current:
+      Class: rviz_default_plugins/Orbit
+      Distance: 2.5
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0.0
+        Y: 0.0
+        Z: 0.5
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.4
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 0.785
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 854
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd000000040000000000000160000002a3fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006f00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000048000002a3000000f300fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002a3fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000048000002a3000000c500fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004bd00000042fc0100000002fb0000000800540069006d00650100000000000004bd000002dc00fffffffb0000000800540069006d0065010000000000000450000000000000000000000356000002a300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1213
+  X: 400
+  Y: 84

src/robot_description/rviz/readme.md

@@ -0,0 +1 @@
+rviz2 config file

src/robot_description/urdf/skyentific_robot.urdf.xacro

@@ -0,0 +1,263 @@
+<?xml version="1.0"?>
+<robot
+    name="skyentific_robot"
+    xmlns:xacro="http://www.ros.org/wiki/xacro"
+>
+    <!-- Include ros2_control Plugins -->
+    <xacro:include
+        filename="$(find robot_description)/urdf/skyentific_robot_ros2_control.xacro"
+    />
+    <!-- Useful XACRO Variables (Properties) -->
+    <xacro:property name="PI" value="3.14159265359" />
+    <material name="grey">
+        <color rgba="0.5 0.5 0.5 1.0"/>
+    </material>
+    <material name="red">
+        <color rgba="0.5 0.0 0.0 1.0"/>
+    </material>
+
+    <link name="base_link">
+        <visual>
+            <origin xyz="0 0 0" rpy="0 0 0"/>
+            <geometry>
+                <mesh
+                    filename="file://$(find robot_description)/meshes/base_z_ob.stl"
+                    scale="0.001 0.001 0.001"
+                />
+            </geometry>
+            <material name="grey" />
+        </visual>
+        <collision> <!-- cyclinder equivalent -->
+            <origin xyz="0 0 0.075" rpy="0 0 0"/>
+            <geometry>
+                <cylinder radius="0.1" length="0.15"/>
+            </geometry>
+        </collision>
+    </link>
+
+    <link name="link_1">
+        <visual>
+            <origin xyz="0 0 0" rpy="0 0 0"/>
+            <geometry>
+                <mesh
+                    filename="file://$(find robot_description)/meshes/link_1.stl"
+                    scale="0.001 0.001 0.001"
+                />
+            </geometry>
+            <material name="grey" />
+        </visual>
+        <collision>
+            <origin xyz="0 0 0.150" rpy="1.57 0 0"/>
+            <geometry>
+                <cylinder radius="0.095" length="0.1"/>
+            </geometry>
+        </collision>
+    </link>
+
+    <joint name="joint_1" type="revolute">
+        <parent link="base_link"/>
+        <child link="link_1"/>
+        <origin xyz="0 0 0.0965" rpy="0 0 0"/>
+        <axis xyz="0 0 1"/>
+        <limit
+            effort="1000.0"
+            velocity="100.0"
+            lower="-3.14"
+            upper="3.14"
+        />
+    </joint>
+
+    <link name="link_2">
+        <visual>
+            <origin xyz="0 0 0" rpy="0 0 0"/>
+            <geometry>
+                <mesh
+                    filename="file://$(find robot_description)/meshes/link_2.stl"
+                    scale="0.001 0.001 0.001"
+                />
+            </geometry>
+            <material name="grey"/>
+        </visual>
+        <collision>
+            <origin xyz="0.225 0 0" rpy="0 0 0"/>
+            <geometry>
+                <cylinder radius="0.07" length="0.1"/>
+            </geometry>
+        </collision>
+    </link>
+
+    <joint name="joint_2" type="revolute">
+        <parent link="link_1"/>
+        <child link="link_2"/>
+        <origin xyz="0 0 0.150" rpy="0 -1.57 1.57"/>
+        <axis xyz="0 0 1"/>
+        <limit
+            effort="1000.0"
+            velocity="100.0"
+            lower="-2.51"
+            upper="2.51"
+        />
+    </joint>
+
+    <link name="link_3">
+        <visual>
+            <origin xyz="0 0 0" rpy="0 0 0"/>
+            <geometry>
+                <mesh
+                    filename="file://$(find robot_description)/meshes/link_3.stl"
+                    scale="0.001 0.001 0.001"
+                />
+            </geometry>
+            <material name="grey" />
+        </visual>
+        <collision>
+            <origin xyz="0.2 0 0" rpy="0 0 0"/>
+            <geometry>
+                <cylinder radius="0.055" length="0.1"/>
+            </geometry>
+        </collision>
+    </link>
+
+    <joint name="joint_3" type="revolute">
+        <parent link="link_2"/>
+        <child link="link_3"/>
+        <origin xyz="0.225 0 0" rpy="0 0 0"/>
+        <axis xyz="0 0 1"/>
+        <limit
+            effort="1000.0"
+            velocity="100.0"
+            lower="-2.51"
+            upper="2.51"
+        />
+    </joint>
+
+    <link name="link_4">
+        <visual>
+            <origin xyz="0 0 0" rpy="0 0 0"/>
+            <geometry>
+                <mesh
+                    filename="file://$(find robot_description)/meshes/link_4.stl"
+                    scale="0.001 0.001 0.001"
+                />
+            </geometry>
+            <material name="grey" />
+        </visual>
+        <collision>
+            <origin xyz="0.1 0 0" rpy="0 1.57 0"/>
+            <geometry>
+                <cylinder radius="0.055" length="0.06"/>
+            </geometry>
+        </collision>
+    </link>
+
+    <joint name="joint_4" type="revolute">
+        <parent link="link_3"/>
+        <child link="link_4"/>
+        <origin xyz="0.2 0 0" rpy="0 0 0"/>
+        <axis xyz="0 0 1"/>
+        <limit
+            effort="1000.0"
+            velocity="100.0"
+            lower="-2.51"
+            upper="2.51"
+        />
+    </joint>
+
+    <link name="link_5">
+        <visual>
+            <origin xyz="0 0 0" rpy="0 0 0"/>
+            <geometry>
+                <mesh
+                    filename="file://$(find robot_description)/meshes/link_5_gripper.stl"
+                    scale="0.001 0.001 0.001"
+                />
+            </geometry>
+            <material name="grey" />
+        </visual>
+        <collision>
+            <origin xyz="0 0 0" rpy="1.57 0 0"/>
+            <geometry>
+                <cylinder radius="0.055" length="0.01"/>
+            </geometry>
+        </collision>
+    </link>
+
+    <joint name="joint_5" type="revolute">
+        <parent link="link_4"/>
+        <child link="link_5"/>
+        <origin xyz="0.0915 0 0" rpy="0 1.57 0"/>
+        <axis xyz="0 0 1"/>
+        <limit
+            effort="1000.0"
+            velocity="100.0"
+            lower="-2.51"
+            upper="2.51"
+        />
+    </joint>
+
+    <link name="gripper_right">
+        <visual>
+            <origin xyz="0 0 0" rpy="0 0 0"/>
+            <geometry>
+                <mesh
+                    filename="file://$(find robot_description)/meshes/rightjaw.stl"
+                    scale="0.001 0.001 0.001"
+                />
+            </geometry>
+            <material name="red"/>
+        </visual>
+        <collision>
+            <origin xyz="0.01 0.025 0.045" rpy="0 0 0"/>
+            <geometry>
+                <cylinder radius="0.0125" length="0.04"/>
+            </geometry>
+        </collision>
+    </link>
+
+    <joint name="joint_6" type="prismatic">
+        <parent link="link_5"/>
+        <child link="gripper_right"/>
+        <origin xyz="-0.01 0 0.1015" rpy="0 0 0"/>
+        <axis xyz="0 1 0"/>
+        <limit
+            effort="1000.0"
+            velocity="100.0"
+            lower="-0.018"
+            upper="0.02"
+        />
+    </joint>
+
+    <link name="gripper_left">
+        <visual>
+            <origin xyz="0 0 0" rpy="0 0 0"/>
+            <geometry>
+                <mesh
+                    filename="file://$(find robot_description)/meshes/leftjaw.stl"
+                    scale="0.001 0.001 0.001"
+                />
+            </geometry>
+            <material name="red"/>
+        </visual>
+        <collision>
+            <origin xyz="0.01 -0.025 0.045" rpy="0 0 0"/>
+            <geometry>
+                <cylinder radius="0.0125" length="0.04"/>
+            </geometry>
+        </collision>
+    </link>
+
+    <joint name="joint_7" type="prismatic">
+        <parent link="link_5"/>
+        <child link="gripper_left"/>
+        <origin xyz="-0.01 0 0.1015" rpy="0 0 0"/>
+        <axis xyz="0 1 0"/>
+        <mimic joint="joint_6" multiplier="-1"/>
+        <limit
+            effort="1000.0"
+            velocity="100.0"
+            lower="-0.018"
+            upper="0.02"
+        />
+    </joint>
+
+</robot>

src/robot_description/urdf/skyentific_robot_ros2_control.xacro

@@ -0,0 +1,50 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+    <ros2_control name="Arm" type="system">
+        <hardware>
+            <plugin>mock_components/GenericSystem</plugin>
+        </hardware>
+        <joint name="joint_1">
+            <command_interface name="position"/>
+            <state_interface name="position">
+                <param name="initial_value">0.0</param>
+            </state_interface>
+        </joint>
+        <joint name="joint_2">
+            <command_interface name="position"/>
+            <state_interface name="position">
+                <param name="initial_value">0.0</param>
+            </state_interface>
+        </joint>
+        <joint name="joint_3">
+            <command_interface name="position"/>
+            <state_interface name="position">
+                <param name="initial_value">0.0</param>
+            </state_interface>
+        </joint>
+        <joint name="joint_4">
+            <command_interface name="position"/>
+            <state_interface name="position">
+                <param name="initial_value">0.0</param>
+            </state_interface>
+        </joint>
+        <joint name="joint_5">
+            <command_interface name="position"/>
+            <state_interface name="position">
+                <param name="initial_value">0.0</param>
+            </state_interface>
+        </joint>
+    </ros2_control>
+
+    <ros2_control name="Gripper" type="system">
+        <hardware>
+            <plugin>mock_components/GenericSystem</plugin>
+        </hardware>
+        <joint name="joint_6">
+            <command_interface name="position"/>
+            <state_interface name="position">
+                <param name="initial_value">0.0</param>
+            </state_interface>
+        </joint>
+    </ros2_control>
+</robot>

src/soup_relay_pkg/CMakeLists.txt

@@ -1,39 +0,0 @@
-cmake_minimum_required(VERSION 3.8)
-project(soup_relay_pkg)
-
-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(rclcpp REQUIRED)
-find_package(std_msgs REQUIRED)
-
-add_executable(soup_relay src/soup_relay.cpp)
-target_include_directories(soup_relay PUBLIC
-  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-  $<INSTALL_INTERFACE:include/${PROJECT_NAME}>)
-target_compile_features(soup_relay PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17
-ament_target_dependencies(
-  soup_relay
-  "rclcpp"
-  "std_msgs"
-)
-
-install(TARGETS soup_relay
-  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()

src/soup_relay_pkg/src/soup_relay.cpp

@@ -1,54 +0,0 @@
-#include <string>
-#include <iostream>
-#include <rclcpp/rclcpp.hpp>
-#include <std_msgs/msg/string.hpp>
-
-class SoupRelayNode : public rclcpp::Node {
-public:
-    SoupRelayNode() : Node("soup_relay_node") {
-        auto topic_publisher = this->declare_parameter<std::string>("topic_publisher", "vin7");
-        auto topic_subscriber = this->declare_parameter<std::string>("topic_subscriber", "vin6");
-
-        publisher_ = this->create_publisher<std_msgs::msg::String>(topic_publisher, 10);
-        subscriber_ = this->create_subscription<std_msgs::msg::String>(
-            topic_subscriber, 10,
-            std::bind(&SoupRelayNode::message_callback, this, std::placeholders::_1)
-        );
-
-        RCLCPP_INFO(this->get_logger(),
-            "listener='%s', publisher='%s'", topic_subscriber.c_str(), topic_publisher.c_str()
-        );
-    }
-
-private:
-    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscriber_;
-    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
-
-    void message_callback(const std_msgs::msg::String::SharedPtr msg) {
-        auto new_msg = std_msgs::msg::String();
-        new_msg.data = trim(msg->data, " ") + " makkelijk";
-
-        RCLCPP_INFO(this->get_logger(),
-            "incoming='%s', outgoing='%s'", msg->data.c_str(), new_msg.data.c_str()
-        );
-
-        publisher_->publish(new_msg);
-    }
-
-    std::string trim(const std::string& str, const std::string& whitespace = " \t") {
-        const auto strBegin = str.find_first_not_of(whitespace);
-        if (strBegin == std::string::npos) return "";
-
-        const auto strEnd = str.find_last_not_of(whitespace);
-        const auto strRange = strEnd - strBegin + 1;
-
-        return str.substr(strBegin, strRange);
-    }
-};
-
-int main(int argc, char *argv[]) {
-    rclcpp::init(argc, argv);
-    rclcpp::spin(std::make_shared<SoupRelayNode>());
-    rclcpp::shutdown();
-    return 0;
-}