Merge pull request #338 from jdlangs/ros2-basic-day2

Ported sessions 3 and 4 to ROS2

.travis.yml

@@ -21,12 +21,12 @@ matrix:
     env:
     - CONTAINER_IMAGE=ros:melodic \
       CONTAINER_NAME=industrial_training_ci \
-      SRC_DIRS="exercises/3.0/src exercises/3.1/src exercises/3.2/src exercises/3.3/src"
+      SRC_DIRS="exercises/3.0/ros1/src exercises/3.1/ros1/src exercises/3.2/ros1/src exercises/3.3/ros1/src"
   - name: "Exercise 4"
     env:
     - CONTAINER_IMAGE=ros:melodic \
       CONTAINER_NAME=industrial_training_ci \
-      SRC_DIRS="exercises/4.0/src exercises/4.1/src"
+      SRC_DIRS="exercises/4.0/ros1/src exercises/4.1/ros1/src"
   - name: "Exercise 5"
     env:
     - CONTAINER_IMAGE=ros:melodic \

exercises/3.0/ros2/src/myworkcell_core/CMakeLists.txt

@@ -0,0 +1,60 @@
+cmake_minimum_required(VERSION 3.5)
+project(myworkcell_core)
+
+# 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(rclcpp REQUIRED)
+find_package(fake_ar_publisher REQUIRED)
+
+# Interface generation dependencies
+find_package(geometry_msgs REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "srv/LocalizePart.srv"
+  DEPENDENCIES geometry_msgs
+)
+
+# The vision_node executable
+add_executable(vision_node src/vision_node.cpp)
+ament_target_dependencies(vision_node rclcpp fake_ar_publisher)
+rosidl_target_interfaces(vision_node ${PROJECT_NAME} "rosidl_typesupport_cpp")
+
+# The myworkcell_node executable
+add_executable(myworkcell_node src/myworkcell_node.cpp)
+ament_target_dependencies(myworkcell_node rclcpp)
+rosidl_target_interfaces(myworkcell_node ${PROJECT_NAME} "rosidl_typesupport_cpp")
+
+# Mark executables and/or libraries for installation
+install(TARGETS vision_node myworkcell_node
+    ARCHIVE DESTINATION lib/${PROJECT_NAME}
+    LIBRARY DESTINATION lib/${PROJECT_NAME}
+    RUNTIME DESTINATION lib/${PROJECT_NAME}
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # uncomment the line when a copyright and license is not present in all source files
+  #set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # uncomment the line when this package is not in a git repo
+  #set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

exercises/3.0/ros2/src/myworkcell_core/package.xml

@@ -0,0 +1,26 @@
+<?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>myworkcell_core</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="me@rosindustrial.com">ROS-Industrial</maintainer>
+  <license>TODO: License declaration</license>
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <build_depend>rosidl_default_generators</build_depend>
+  <exec_depend>rosidl_default_runtime</exec_depend>
+  <depend>geometry_msgs</depend>
+
+  <depend>rclcpp</depend>
+  <depend>fake_ar_publisher</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

exercises/3.0/ros2/src/myworkcell_core/src/myworkcell_node.cpp

@@ -0,0 +1,66 @@
+#include <rclcpp/rclcpp.hpp>
+#include <myworkcell_core/srv/localize_part.hpp>
+
+class ScanNPlan : public rclcpp::Node
+{
+public:
+  ScanNPlan() : Node("scan_n_plan")
+  {
+    this->declare_parameter("base_frame", "world");
+
+    vision_client_ = this->create_client<myworkcell_core::srv::LocalizePart>("localize_part");
+  }
+
+  void start(const std::string& base_frame)
+  {
+    RCLCPP_INFO(get_logger(), "Attempting to localize part");
+
+    // Create a request for the LocalizePart service call
+    auto request = std::make_shared<myworkcell_core::srv::LocalizePart::Request>();
+    request->base_frame = base_frame;
+
+    auto future = vision_client_->async_send_request(request);
+
+    if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), future) != rclcpp::executor::FutureReturnCode::SUCCESS)
+    {
+      RCLCPP_ERROR(this->get_logger(), "Failed to receive LocalizePart service response");
+      return;
+    }
+
+    auto response = future.get();
+    if (! response->success)
+    {
+      RCLCPP_ERROR(this->get_logger(), "LocalizePart service failed");
+      return;
+    }
+
+    RCLCPP_INFO(this->get_logger(), "Part Localized:  x: %f, y: %f, z: %f",
+        response->pose.position.x,
+        response->pose.position.y,
+        response->pose.position.z);
+  }
+
+private:
+  // Planning components
+  rclcpp::Client<myworkcell_core::srv::LocalizePart>::SharedPtr vision_client_;
+};
+
+int main(int argc, char **argv)
+{
+  // This must be called before anything else ROS-related
+  rclcpp::init(argc, argv);
+
+  // Create the ScanNPlan node
+  auto app = std::make_shared<ScanNPlan>();
+
+  std::string base_frame;
+  app->get_parameter("base_frame", base_frame);
+
+  //Wait for the vision node to receive data
+  rclcpp::sleep_for(std::chrono::seconds(2));
+
+  app->start(base_frame);
+
+  rclcpp::shutdown();
+  return 0;
+}

exercises/3.0/ros2/src/myworkcell_core/src/vision_node.cpp

@@ -0,0 +1,65 @@
+#include <rclcpp/rclcpp.hpp>
+#include <fake_ar_publisher/msg/ar_marker.hpp>
+#include <myworkcell_core/srv/localize_part.hpp>
+
+class Localizer : public rclcpp::Node
+{
+public:
+  Localizer() : Node("vision_node"), last_msg_{nullptr}
+  {
+    ar_sub_ = this->create_subscription<fake_ar_publisher::msg::ARMarker>(
+        "ar_pose_marker",
+        rclcpp::QoS(1),
+        std::bind(&Localizer::visionCallback, this, std::placeholders::_1));
+
+    server_ = this->create_service<myworkcell_core::srv::LocalizePart>(
+        "localize_part",
+        std::bind(&Localizer::localizePart, this, std::placeholders::_1, std::placeholders::_2));
+  }
+
+  void visionCallback(fake_ar_publisher::msg::ARMarker::SharedPtr msg)
+  {
+    last_msg_ = msg;
+    /*
+    RCLCPP_INFO(get_logger(), "Received pose: x=%f, y=%f, z=%f",
+        msg->pose.pose.position.x,
+        msg->pose.pose.position.y,
+        msg->pose.pose.position.z);
+    */
+  }
+
+  void localizePart(myworkcell_core::srv::LocalizePart::Request::SharedPtr req,
+                  myworkcell_core::srv::LocalizePart::Response::SharedPtr res)
+  {
+    // Read last message
+    fake_ar_publisher::msg::ARMarker::SharedPtr p = last_msg_;
+
+    if (! p)
+    {
+      RCLCPP_ERROR(this->get_logger(), "no data");
+      res->success = false;
+      return;
+    }
+
+    res->pose = p->pose.pose;
+    res->success = true;
+  }
+
+  rclcpp::Subscription<fake_ar_publisher::msg::ARMarker>::SharedPtr ar_sub_;
+  rclcpp::Service<myworkcell_core::srv::LocalizePart>::SharedPtr server_;
+  fake_ar_publisher::msg::ARMarker::SharedPtr last_msg_;
+};
+
+int main(int argc, char* argv[])
+{
+  // This must be called before anything else ROS-related
+  rclcpp::init(argc, argv);
+
+  // The Localizer class provides this node's ROS interfaces
+  auto node = std::make_shared<Localizer>();
+
+  RCLCPP_INFO(node->get_logger(), "Vision node starting");
+
+  // Don't exit the program.
+  rclcpp::spin(node);
+}

exercises/3.0/ros2/src/myworkcell_core/srv/LocalizePart.srv

@@ -0,0 +1,4 @@
+string base_frame
+---
+geometry_msgs/Pose pose
+bool success

exercises/3.0/ros2/src/myworkcell_support/CMakeLists.txt

@@ -0,0 +1,35 @@
+cmake_minimum_required(VERSION 3.5)
+project(myworkcell_support)
+
+# 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(myworkcell_core REQUIRED)
+
+install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}/)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # uncomment the line when a copyright and license is not present in all source files
+  #set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # uncomment the line when this package is not in a git repo
+  #set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

exercises/3.0/ros2/src/myworkcell_support/launch/workcell.launch.py

@@ -0,0 +1,25 @@
+import launch
+import launch_ros
+
+def generate_launch_description():
+    return launch.LaunchDescription([
+        launch_ros.actions.Node(
+            name='fake_ar_publisher_node',
+            package='fake_ar_publisher',
+            executable='fake_ar_publisher_node',
+            output='screen',
+        ),
+        launch_ros.actions.Node(
+            name='vision_node',
+            package='myworkcell_core',
+            executable='vision_node',
+            output='screen',
+        ),
+        launch_ros.actions.Node(
+            name='myworkcell_node',
+            package='myworkcell_core',
+            executable='myworkcell_node',
+            output='screen',
+            parameters=[{'base_frame': 'world'}],
+        )
+    ])

exercises/3.0/ros2/src/myworkcell_support/package.xml

@@ -0,0 +1,20 @@
+<?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>myworkcell_support</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="me@rosindustrial.com">ROS-Industrial</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>myworkcell_core</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

exercises/3.0/ros2/src/myworkcell_support/urdf/workcell.urdf

@@ -0,0 +1,31 @@
+<?xml version="1.0" ?>
+<robot name="myworkcell" xmlns:xacro="http://ros.org/wiki/xacro">
+  <link name="world"/>
+
+  <link name="table">
+    <visual>
+      <geometry>
+        <box size="1.0 1.0 0.05"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <box size="1.0 1.0 0.05"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <link name="camera_frame"/>
+
+  <joint name="world_to_table" type="fixed">
+    <parent link="world"/>
+    <child link="table"/>
+    <origin xyz="0 0 0.5" rpy="0 0 0"/>
+  </joint>
+
+  <joint name="world_to_camera" type="fixed">
+    <parent link="world"/>
+    <child link="camera_frame"/>
+    <origin xyz="-0.25 -0.5 1.25" rpy="0 3.14159 0"/>
+  </joint>
+</robot>