Merge pull request #366 from JeremyZoss/2021.10/day2_updates

day2 updates for clarity and consistency

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

@@ -39,12 +39,9 @@ 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
+# Mark executables for installation
 install(TARGETS vision_node myworkcell_node
-    ARCHIVE DESTINATION lib/${PROJECT_NAME}
-    LIBRARY DESTINATION lib/${PROJECT_NAME}
-    RUNTIME DESTINATION lib/${PROJECT_NAME}
-)
+    DESTINATION lib/${PROJECT_NAME})
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)

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

@@ -3,7 +3,7 @@
 <package format="3">
   <name>myworkcell_core</name>
   <version>0.0.0</version>
-  <description>TODO: Package description</description>
+  <description>core scan-n-plan application node</description>
   <maintainer email="me@rosindustrial.com">ROS-Industrial</maintainer>
   <license>TODO: License declaration</license>
   <member_of_group>rosidl_interface_packages</member_of_group>

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

@@ -34,7 +34,7 @@ class ScanNPlan : public rclcpp::Node
       return;
     }
 
-    RCLCPP_INFO(this->get_logger(), "Part Localized:  x: %f, y: %f, z: %f",
+    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);
@@ -53,8 +53,7 @@ int main(int argc, char **argv)
   // Create the ScanNPlan node
   auto app = std::make_shared<ScanNPlan>();
 
-  std::string base_frame;
-  app->get_parameter("base_frame", base_frame);
+  std::string base_frame = app->get_parameter("base_frame").as_string();
 
   //Wait for the vision node to receive data
   rclcpp::sleep_for(std::chrono::seconds(2));

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

@@ -1,3 +1,6 @@
+/**
+**  Simple ROS Node
+**/
 #include <rclcpp/rclcpp.hpp>
 #include <fake_ar_publisher/msg/ar_marker.hpp>
 #include <myworkcell_core/srv/localize_part.hpp>
@@ -29,7 +32,7 @@ class Localizer : public rclcpp::Node
   }
 
   void localizePart(myworkcell_core::srv::LocalizePart::Request::SharedPtr req,
-                  myworkcell_core::srv::LocalizePart::Response::SharedPtr res)
+                    myworkcell_core::srv::LocalizePart::Response::SharedPtr res)
   {
     // Read last message
     fake_ar_publisher::msg::ARMarker::SharedPtr p = last_msg_;

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

@@ -1,4 +1,7 @@
+#request
 string base_frame
 ---
+#response
 geometry_msgs/Pose pose
 bool success
+

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

@@ -19,7 +19,7 @@ endif()
 find_package(ament_cmake REQUIRED)
 find_package(myworkcell_core REQUIRED)
 
-install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}/)
+install(DIRECTORY launch urdf DESTINATION share/${PROJECT_NAME}/)
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)

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

@@ -39,12 +39,9 @@ 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
+# Mark executables for installation
 install(TARGETS vision_node myworkcell_node
-    ARCHIVE DESTINATION lib/${PROJECT_NAME}
-    LIBRARY DESTINATION lib/${PROJECT_NAME}
-    RUNTIME DESTINATION lib/${PROJECT_NAME}
-)
+    DESTINATION lib/${PROJECT_NAME})
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)

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

@@ -3,7 +3,7 @@
 <package format="3">
   <name>myworkcell_core</name>
   <version>0.0.0</version>
-  <description>TODO: Package description</description>
+  <description>core scan-n-plan application node</description>
   <maintainer email="me@rosindustrial.com">ROS-Industrial</maintainer>
   <license>TODO: License declaration</license>
   <member_of_group>rosidl_interface_packages</member_of_group>

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

@@ -15,9 +15,16 @@ class ScanNPlan : public rclcpp::Node
   {
     RCLCPP_INFO(get_logger(), "Attempting to localize part");
 
+    // Wait for service to be available
+    if (!vision_client_->wait_for_service(std::chrono::seconds(5))) {
+      RCLCPP_ERROR(get_logger(), "Unable to find localize_part service. Start vision_node first.");
+      return;
+    }
+
     // Create a request for the LocalizePart service call
     auto request = std::make_shared<myworkcell_core::srv::LocalizePart::Request>();
     request->base_frame = base_frame;
+    RCLCPP_INFO_STREAM(get_logger(), "Requesting pose in base frame: " << base_frame);
 
     auto future = vision_client_->async_send_request(request);
 
@@ -34,7 +41,7 @@ class ScanNPlan : public rclcpp::Node
       return;
     }
 
-    RCLCPP_INFO(this->get_logger(), "Part Localized:  x: %f, y: %f, z: %f",
+    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);
@@ -53,8 +60,7 @@ int main(int argc, char **argv)
   // Create the ScanNPlan node
   auto app = std::make_shared<ScanNPlan>();
 
-  std::string base_frame;
-  app->get_parameter("base_frame", base_frame);
+  std::string base_frame = app->get_parameter("base_frame").as_string();
 
   //Wait for the vision node to receive data
   rclcpp::sleep_for(std::chrono::seconds(2));

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

@@ -1,3 +1,6 @@
+/**
+**  Simple ROS Node
+**/
 #include <rclcpp/rclcpp.hpp>
 #include <fake_ar_publisher/msg/ar_marker.hpp>
 #include <myworkcell_core/srv/localize_part.hpp>
@@ -29,7 +32,7 @@ class Localizer : public rclcpp::Node
   }
 
   void localizePart(myworkcell_core::srv::LocalizePart::Request::SharedPtr req,
-                  myworkcell_core::srv::LocalizePart::Response::SharedPtr res)
+                    myworkcell_core::srv::LocalizePart::Response::SharedPtr res)
   {
     // Read last message
     fake_ar_publisher::msg::ARMarker::SharedPtr p = last_msg_;

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

@@ -1,4 +1,7 @@
+#request
 string base_frame
 ---
+#response
 geometry_msgs/Pose pose
 bool success
+

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

@@ -41,12 +41,9 @@ 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
+# Mark executables for installation
 install(TARGETS vision_node myworkcell_node
-    ARCHIVE DESTINATION lib/${PROJECT_NAME}
-    LIBRARY DESTINATION lib/${PROJECT_NAME}
-    RUNTIME DESTINATION lib/${PROJECT_NAME}
-)
+    DESTINATION lib/${PROJECT_NAME})
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)

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

@@ -3,7 +3,7 @@
 <package format="3">
   <name>myworkcell_core</name>
   <version>0.0.0</version>
-  <description>TODO: Package description</description>
+  <description>core scan-n-plan application node</description>
   <maintainer email="me@rosindustrial.com">ROS-Industrial</maintainer>
   <license>TODO: License declaration</license>
   <member_of_group>rosidl_interface_packages</member_of_group>

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

@@ -15,9 +15,16 @@ class ScanNPlan : public rclcpp::Node
   {
     RCLCPP_INFO(get_logger(), "Attempting to localize part");
 
+    // Wait for service to be available
+    if (!vision_client_->wait_for_service(std::chrono::seconds(5))) {
+      RCLCPP_ERROR(get_logger(), "Unable to find localize_part service. Start vision_node first.");
+      return;
+    }
+
     // Create a request for the LocalizePart service call
     auto request = std::make_shared<myworkcell_core::srv::LocalizePart::Request>();
     request->base_frame = base_frame;
+    RCLCPP_INFO_STREAM(get_logger(), "Requesting pose in base frame: " << base_frame);
 
     auto future = vision_client_->async_send_request(request);
 
@@ -34,7 +41,7 @@ class ScanNPlan : public rclcpp::Node
       return;
     }
 
-    RCLCPP_INFO(this->get_logger(), "Part Localized:  x: %f, y: %f, z: %f",
+    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);
@@ -53,8 +60,7 @@ int main(int argc, char **argv)
   // Create the ScanNPlan node
   auto app = std::make_shared<ScanNPlan>();
 
-  std::string base_frame;
-  app->get_parameter("base_frame", base_frame);
+  std::string base_frame = app->get_parameter("base_frame").as_string();
 
   //Wait for the vision node to receive data
   rclcpp::sleep_for(std::chrono::seconds(2));

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

@@ -1,3 +1,6 @@
+/**
+**  Simple ROS Node
+**/
 #include <rclcpp/rclcpp.hpp>
 #include <tf2_ros/buffer.h>
 #include <tf2_ros/transform_listener.h>
@@ -34,7 +37,7 @@ class Localizer : public rclcpp::Node
   }
 
   void localizePart(myworkcell_core::srv::LocalizePart::Request::SharedPtr req,
-                  myworkcell_core::srv::LocalizePart::Response::SharedPtr res)
+                    myworkcell_core::srv::LocalizePart::Response::SharedPtr res)
   {
     // Read last message
     fake_ar_publisher::msg::ARMarker::SharedPtr p = last_msg_;

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

@@ -1,4 +1,7 @@
+#request
 string base_frame
 ---
+#response
 geometry_msgs/Pose pose
 bool success
+

exercises/3.2/ros2/src/myworkcell_support/launch/urdf.launch.py

@@ -1,7 +1,8 @@
 import os
-import yaml
 import launch
 import launch_ros
+import xacro
+
 from ament_index_python import get_package_share_directory
 
 def get_package_file(package, file_path):
@@ -10,41 +11,17 @@ def get_package_file(package, file_path):
     absolute_file_path = os.path.join(package_path, file_path)
     return absolute_file_path
 
-def load_file(file_path):
-    """Load the contents of a file into a string"""
-    try:
-        with open(file_path, 'r') as file:
-            return file.read()
-    except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available
-        return None
-
-def load_yaml(file_path):
-    """Load a yaml file into a dictionary"""
-    try:
-        with open(file_path, 'r') as file:
-            return yaml.safe_load(file)
-    except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available
-        return None
-
-def run_xacro(xacro_file):
-    """Run xacro and output a file in the same directory with the same name, w/o a .xacro suffix"""
-    urdf_file, ext = os.path.splitext(xacro_file)
-    if ext != '.xacro':
-        raise RuntimeError(f'Input file to xacro must have a .xacro extension, got {xacro_file}')
-    os.system(f'xacro {xacro_file} -o {urdf_file}')
-    return urdf_file
-
 def generate_launch_description():
     xacro_file = get_package_file('myworkcell_support', 'urdf/workcell.urdf.xacro')
-    urdf_file = run_xacro(xacro_file)
+    urdf = xacro.process_file(xacro_file).toprettyxml(indent='  ')
 
     return launch.LaunchDescription([
         launch_ros.actions.Node(
             name='robot_state_publisher',
             package='robot_state_publisher',
             executable='robot_state_publisher',
             output='screen',
-            arguments=[urdf_file],
+            parameters=[{'robot_description': urdf}],
         ),
         launch_ros.actions.Node(
             name='joint_state_publisher_gui',

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

@@ -20,6 +20,6 @@ def generate_launch_description():
             package='myworkcell_core',
             executable='myworkcell_node',
             output='screen',
-            parameters=[{'base_frame': 'table'}],
+            parameters=[{'base_frame': 'world'}],
         )
     ])

exercises/3.2/ros2/src/myworkcell_support/urdf/workcell.urdf.xacro

@@ -1,7 +1,19 @@
 <?xml version="1.0" ?>
 <robot name="myworkcell" xmlns:xacro="http://ros.org/wiki/xacro">
-  <xacro:include filename="$(find ur_description)/urdf/ur5.urdf.xacro" />
-  <xacro:ur5_robot prefix="" joint_limited="true"/>
+  <xacro:include filename="$(find ur_description)/urdf/ur_macro.xacro"/>
+
+  <!-- ur arm instantiation -->
+  <xacro:arg name="ur_type" default="ur5"/>
+  <xacro:arg name="use_fake_hardware" default="true"/>
+  <xacro:arg name="fake_sensor_commands" default="true"/>
+  <xacro:ur_robot
+      prefix=""
+      joint_limits_parameters_file="$(find ur_description)/config/$(arg ur_type)/joint_limits.yaml"
+      kinematics_parameters_file="$(find ur_description)/config/$(arg ur_type)/default_kinematics.yaml"
+      physical_parameters_file="$(find ur_description)/config/$(arg ur_type)/physical_parameters.yaml"
+      visual_parameters_file="$(find ur_description)/config/$(arg ur_type)/visual_parameters.yaml"
+      use_fake_hardware="$(arg use_fake_hardware)"
+      fake_sensor_commands="$(arg fake_sensor_commands)"/>
 
   <link name="world"/>