added ros-gz-sim and ros-gz-bridge to packages

flake.nix

@@ -18,6 +18,12 @@
         pkgs = import nixpkgs {
           inherit system;
           overlays = [ nix-ros-overlay.overlays.default ];
+          config = {
+            # Allow the insecure freeimage package
+            permittedInsecurePackages = [
+              "freeimage-3.18.0-unstable-2024-04-18"
+            ];
+          };
         };
       in
       {
@@ -35,7 +41,7 @@
               ]
             ))
             (
-              with rosPackages.humble;
+              with rosPackages.jazzy;
               buildEnv {
                 paths = [
                   ros-core
@@ -64,12 +70,13 @@
                   moveit-ros-move-group
                   moveit-servo
                   moveit-simple-controller-manager
-                  topic-based-ros2-control
                   pilz-industrial-motion-planner
                   pick-ik
                   ompl
                   chomp-motion-planner
                   joy
+                  ros-gz-sim
+                  ros-gz-bridge
                   # ros2-controllers nixpkg does not build :(
                 ];
               }
@@ -79,6 +86,10 @@
             # Display stuff
             export DISPLAY=''${DISPLAY:-:0}
             export QT_X11_NO_MITSHM=1
+	    if [[ ! $DIRENV_IN_ENVRC ]]; then
+              eval "$(${pkgs.python3Packages.argcomplete}/bin/register-python-argcomplete ros2)"
+              eval "$(${pkgs.python3Packages.argcomplete}/bin/register-python-argcomplete colcon)"
+            fi
           '';
         };
       }

src/moveit_servo/CMakeLists.txt

@@ -0,0 +1,26 @@
+cmake_minimum_required(VERSION 3.8)
+project(moveit_servo)
+
+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)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+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/moveit_servo/config/kinematics.yaml

@@ -0,0 +1,4 @@
+panda_arm:
+  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
+  kinematics_solver_search_resolution: 0.005
+  kinematics_solver_timeout: 0.05

src/moveit_servo/launch/servo.launch.py

@@ -0,0 +1,25 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from moveit_configs_utils import MoveItConfigsBuilder
+
+def generate_launch_description():
+    moveit_config = (
+        MoveItConfigsBuilder("astra_descriptions")
+        .robot_description(file_path="config/robot.urdf.xacro")
+        .to_moveit_configs()
+    )
+
+    servo_node = Node(
+        package="moveit_servo",
+        executable="servo_node",
+        name="servo_node",
+        parameters=[
+            "config/servo_parameters.yaml",
+            moveit_config.robot_description,
+            moveit_config.robot_description_semantic,
+            moveit_config.robot_description_kinematics,
+        ],
+        output="screen",
+    )
+
+    return LaunchDescription([servo_node])

src/moveit_servo/package.xml

@@ -0,0 +1,18 @@
+<?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>moveit_servo</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="aaron.m.davis@comcast.net">aaron</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/servo_arm_twist_pkg/CMakeLists.txt

@@ -1,80 +0,0 @@
-cmake_minimum_required(VERSION 3.22)
-project(servo_arm_twist_pkg)
-
-# C++ Libraries #################################################
-
-# Core C++ library for calculations and collision checking.
-# Provides interface used by the component node.
-set(SERVO_LIB_NAME servo_arm_twist_lib)
-
-# Pose Tracking
-set(POSE_TRACKING pose_tracking)
-
-# Component Nodes (Shared libraries) ############################
-set(SERVO_COMPONENT_NODE servo_node)
-set(SERVO_CONTROLLER_INPUT servo_controller_input)
-
-# Executable Nodes ##############################################
-set(SERVO_NODE_MAIN_NAME servo_node_main)
-set(POSE_TRACKING_DEMO_NAME servo_pose_tracking_demo)
-set(FAKE_SERVO_CMDS_NAME fake_command_publisher)
-
-#################################################################
-
-# Common cmake code applied to all moveit packages
-find_package(moveit_common REQUIRED)
-moveit_package()
-
-set(THIS_PACKAGE_INCLUDE_DEPENDS
-  control_msgs
-  control_toolbox
-  geometry_msgs
-  moveit_core
-  moveit_msgs
-  moveit_ros_planning
-  pluginlib
-  rclcpp
-  rclcpp_components
-  sensor_msgs
-  std_msgs
-  std_srvs
-  tf2_eigen
-  trajectory_msgs
-)
-
-find_package(ament_cmake REQUIRED)
-find_package(eigen3_cmake_module REQUIRED)
-find_package(Eigen3 REQUIRED)
-foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
-  find_package(${Dependency} REQUIRED)
-endforeach()
-
-#####################
-## Component Nodes ##
-#####################
-
-# Add executable for using a controller
-add_library(${SERVO_CONTROLLER_INPUT} SHARED src/joystick_twist.cpp)
-ament_target_dependencies(${SERVO_CONTROLLER_INPUT} ${THIS_PACKAGE_INCLUDE_DEPENDS})
-rclcpp_components_register_nodes(${SERVO_CONTROLLER_INPUT} "servo_arm_twist_pkg::JoyToServoPub")
-
-#############
-## Install ##
-#############
-
-# Install Libraries
-install(
-  TARGETS
-    ${SERVO_CONTROLLER_INPUT}
-  EXPORT export_${PROJECT_NAME}
-  LIBRARY DESTINATION lib
-  ARCHIVE DESTINATION lib
-  RUNTIME DESTINATION bin
-  INCLUDES DESTINATION include
-)
-
-# Install Binaries
-ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
-ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
-
-ament_package()

src/servo_arm_twist_pkg/README.md

@@ -1,3 +0,0 @@
-# Moveit Servo
-
-See the [Realtime Arm Servoing Tutorial](https://moveit.picknik.ai/main/doc/realtime_servo/realtime_servo_tutorial.html) for installation instructions, quick-start guide, an overview about `moveit_servo`, and to learn how to set it up on your robot.

src/servo_arm_twist_pkg/package.xml

@@ -1,58 +0,0 @@
-<?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>servo_arm_twist_pkg</name>
-  <version>2.5.9</version>
-  <description>Provides real-time manipulator Cartesian and joint servoing.</description>
-  <maintainer email="blakeanderson@utexas.edu">Blake Anderson</maintainer>
-  <maintainer email="andyz@utexas.edu">Andy Zelenak</maintainer>
-  <maintainer email="tyler@picknik.ai">Tyler Weaver</maintainer>
-  <maintainer email="henningkayser@picknik.ai">Henning Kayser</maintainer>
-
-  <license>BSD 3-Clause</license>
-
-  <url type="website">https://ros-planning.github.io/moveit_tutorials</url>
-
-  <author>Brian O'Neil</author>
-  <author email="andyz@utexas.edu">Andy Zelenak</author>
-  <author>Blake Anderson</author>
-  <author email="alex@machinekoder.com">Alexander Rössler</author>
-  <author email="tyler@picknik.ai">Tyler Weaver</author>
-  <author email="adam.pettinger@utexas.edu">Adam Pettinger</author>
-
-  <buildtool_depend>ament_cmake</buildtool_depend>
-  <depend>moveit_common</depend>
-
-  <depend>control_msgs</depend>
-  <depend>control_toolbox</depend>
-  <depend>geometry_msgs</depend>
-  <depend>moveit_msgs</depend>
-  <depend>moveit_core</depend>
-  <depend>moveit_ros_planning_interface</depend>
-  <depend>pluginlib</depend>
-  <depend>sensor_msgs</depend>
-  <depend>std_msgs</depend>
-  <depend>std_srvs</depend>
-  <depend>tf2_eigen</depend>
-  <depend>trajectory_msgs</depend>
-
-  <exec_depend>gripper_controllers</exec_depend>
-  <exec_depend>joint_state_broadcaster</exec_depend>
-  <exec_depend>joint_trajectory_controller</exec_depend>
-  <exec_depend>joy</exec_depend>
-  <exec_depend>robot_state_publisher</exec_depend>
-  <exec_depend>tf2_ros</exec_depend>
-  <exec_depend>moveit_configs_utils</exec_depend>
-  <exec_depend>launch_param_builder</exec_depend>
-
-  <test_depend>ament_cmake_gtest</test_depend>
-  <test_depend>ament_lint_auto</test_depend>
-  <test_depend>ament_lint_common</test_depend>
-  <test_depend>controller_manager</test_depend>
-  <test_depend>ros_testing</test_depend>
-
-  <export>
-    <build_type>ament_cmake</build_type>
-  </export>
-
-</package>

src/servo_arm_twist_pkg/src/joystick_twist.cpp

@@ -1,271 +0,0 @@
-/*********************************************************************
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2020, PickNik Inc.
- *  All rights reserved.
- *
- *  Redistribution and use in source and binary forms, with or without
- *  modification, are permitted provided that the following conditions
- *  are met:
- *
- *   * Redistributions of source code must retain the above copyright
- *     notice, this list of conditions and the following disclaimer.
- *   * Redistributions in binary form must reproduce the above
- *     copyright notice, this list of conditions and the following
- *     disclaimer in the documentation and/or other materials provided
- *     with the distribution.
- *   * Neither the name of PickNik Inc. nor the names of its
- *     contributors may be used to endorse or promote products derived
- *     from this software without specific prior written permission.
- *
- *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- *  POSSIBILITY OF SUCH DAMAGE.
- *********************************************************************/
-
-/*      Title     : joystick_servo_example.cpp
- *      Project   : servo_arm_twist_pkg
- *      Created   : 08/07/2020
- *      Author    : Adam Pettinger
- */
-
-#include <sensor_msgs/msg/joy.hpp>
-#include <geometry_msgs/msg/twist_stamped.hpp>
-#include <control_msgs/msg/joint_jog.hpp>
-#include <std_srvs/srv/trigger.hpp>
-#include <moveit_msgs/msg/planning_scene.hpp>
-#include <rclcpp/client.hpp>
-#include <rclcpp/experimental/buffers/intra_process_buffer.hpp>
-#include <rclcpp/node.hpp>
-#include <rclcpp/publisher.hpp>
-#include <rclcpp/qos.hpp>
-#include <rclcpp/qos_event.hpp>
-#include <rclcpp/subscription.hpp>
-#include <rclcpp/time.hpp>
-#include <rclcpp/utilities.hpp>
-#include <thread>
-
-// We'll just set up parameters here
-const std::string JOY_TOPIC = "/joy";
-const std::string TWIST_TOPIC = "/servo_node/delta_twist_cmds";
-const std::string JOINT_TOPIC = "/servo_node/delta_joint_cmds";
-const std::string EEF_FRAME_ID = "End_Effector";
-const std::string BASE_FRAME_ID = "base_link";
-
-// Enums for button names -> axis/button array index
-// For XBOX 1 controller
-enum Axis
-{
-    LEFT_STICK_X = 0,
-    LEFT_STICK_Y = 1,
-    LEFT_TRIGGER = 2,
-    RIGHT_STICK_X = 3,
-    RIGHT_STICK_Y = 4,
-    RIGHT_TRIGGER = 5,
-    D_PAD_X = 6,
-    D_PAD_Y = 7
-};
-enum Button
-{
-    A = 0,
-    B = 1,
-    X = 2,
-    Y = 3,
-    LEFT_BUMPER = 4,
-    RIGHT_BUMPER = 5,
-    CHANGE_VIEW = 6,
-    MENU = 7,
-    HOME = 8,
-    LEFT_STICK_CLICK = 9,
-    RIGHT_STICK_CLICK = 10
-};
-
-// Some axes have offsets (e.g. the default trigger position is 1.0 not 0)
-// This will map the default values for the axes
-std::map<Axis, double> AXIS_DEFAULTS = { { LEFT_TRIGGER, 1.0 }, { RIGHT_TRIGGER, 1.0 } };
-std::map<Button, double> BUTTON_DEFAULTS;
-
-// To change controls or setup a new controller, all you should to do is change the above enums and the follow 2
-// functions
-/** \brief // This converts a joystick axes and buttons array to a TwistStamped or JointJog message
- * @param axes The vector of continuous controller joystick axes
- * @param buttons The vector of discrete controller button values
- * @param twist A TwistStamped message to update in prep for publishing
- * @param joint A JointJog message to update in prep for publishing
- * @return return true if you want to publish a Twist, false if you want to publish a JointJog
- */
-bool convertJoyToCmd(const std::vector<float>& axes, const std::vector<int>& buttons,
-                     std::unique_ptr<geometry_msgs::msg::TwistStamped>& twist,
-                     std::unique_ptr<control_msgs::msg::JointJog>& joint)
-{
-    // // Give joint jogging priority because it is only buttons
-    // // If any joint jog command is requested, we are only publishing joint commands
-    // if (buttons[A] || buttons[B] || buttons[X] || buttons[Y] || axes[D_PAD_X] || axes[D_PAD_Y])
-    // {
-    //   // Map the D_PAD to the proximal joints
-    //   joint->joint_names.push_back("panda_joint1");
-    //   joint->velocities.push_back(axes[D_PAD_X]);
-    //   joint->joint_names.push_back("panda_joint2");
-    //   joint->velocities.push_back(axes[D_PAD_Y]);
-
-    //   // Map the diamond to the distal joints
-    //   joint->joint_names.push_back("panda_joint7");
-    //   joint->velocities.push_back(buttons[B] - buttons[X]);
-    //   joint->joint_names.push_back("panda_joint6");
-    //   joint->velocities.push_back(buttons[Y] - buttons[A]);
-    //   return false;
-    // }
-
-    // The bread and butter: map buttons to twist commands
-    twist->twist.linear.z = axes[RIGHT_STICK_Y];
-    twist->twist.linear.y = axes[RIGHT_STICK_X];
-
-    double lin_x_right = -0.5 * (axes[RIGHT_TRIGGER] - AXIS_DEFAULTS.at(RIGHT_TRIGGER));
-    double lin_x_left = 0.5 * (axes[LEFT_TRIGGER] - AXIS_DEFAULTS.at(LEFT_TRIGGER));
-    twist->twist.linear.x = lin_x_right + lin_x_left;
-
-    twist->twist.angular.y = axes[LEFT_STICK_Y];
-    twist->twist.angular.x = axes[LEFT_STICK_X];
-
-    double roll_positive = buttons[RIGHT_BUMPER];
-    double roll_negative = -1 * (buttons[LEFT_BUMPER]);
-    twist->twist.angular.z = roll_positive + roll_negative;
-
-    return true;
-}
-
-/** \brief // This should update the frame_to_publish_ as needed for changing command frame via controller
- * @param frame_name Set the command frame to this
- * @param buttons The vector of discrete controller button values
- */
-void updateCmdFrame(std::string& frame_name, const std::vector<int>& buttons)
-{
-    if (buttons[CHANGE_VIEW] && frame_name == EEF_FRAME_ID)
-        frame_name = BASE_FRAME_ID;
-    else if (buttons[MENU] && frame_name == BASE_FRAME_ID)
-        frame_name = EEF_FRAME_ID;
-}
-
-namespace servo_arm_twist_pkg
-{
-class JoyToServoPub : public rclcpp::Node
-{
-public:
-    JoyToServoPub(const rclcpp::NodeOptions& options)
-        : Node("joy_to_twist_publisher", options), frame_to_publish_(BASE_FRAME_ID)
-    {
-        // Setup pub/sub
-        joy_sub_ = this->create_subscription<sensor_msgs::msg::Joy>(
-            JOY_TOPIC, rclcpp::SystemDefaultsQoS(),
-            [this](const sensor_msgs::msg::Joy::ConstSharedPtr& msg) { return joyCB(msg); });
-
-        twist_pub_ = this->create_publisher<geometry_msgs::msg::TwistStamped>(TWIST_TOPIC, rclcpp::SystemDefaultsQoS());
-        joint_pub_ = this->create_publisher<control_msgs::msg::JointJog>(JOINT_TOPIC, rclcpp::SystemDefaultsQoS());
-        // collision_pub_ =
-        //     this->create_publisher<moveit_msgs::msg::PlanningScene>("/planning_scene", rclcpp::SystemDefaultsQoS());
-
-        // Create a service client to start the ServoNode
-        servo_start_client_ = this->create_client<std_srvs::srv::Trigger>("/servo_node/start_servo");
-        servo_start_client_->wait_for_service(std::chrono::seconds(1));
-        servo_start_client_->async_send_request(std::make_shared<std_srvs::srv::Trigger::Request>());
-
-        // // Load the collision scene asynchronously
-        // collision_pub_thread_ = std::thread([this]() {
-        //   rclcpp::sleep_for(std::chrono::seconds(3));
-        //   // Create collision object, in the way of servoing
-        //   moveit_msgs::msg::CollisionObject collision_object;
-        //   collision_object.header.frame_id = "panda_link0";
-        //   collision_object.id = "box";
-
-        //   shape_msgs::msg::SolidPrimitive table_1;
-        //   table_1.type = table_1.BOX;
-        //   table_1.dimensions = { 0.4, 0.6, 0.03 };
-
-        //   geometry_msgs::msg::Pose table_1_pose;
-        //   table_1_pose.position.x = 0.6;
-        //   table_1_pose.position.y = 0.0;
-        //   table_1_pose.position.z = 0.4;
-
-        //   shape_msgs::msg::SolidPrimitive table_2;
-        //   table_2.type = table_2.BOX;
-        //   table_2.dimensions = { 0.6, 0.4, 0.03 };
-
-        //   geometry_msgs::msg::Pose table_2_pose;
-        //   table_2_pose.position.x = 0.0;
-        //   table_2_pose.position.y = 0.5;
-        //   table_2_pose.position.z = 0.25;
-
-        //   collision_object.primitives.push_back(table_1);
-        //   collision_object.primitive_poses.push_back(table_1_pose);
-        //   collision_object.primitives.push_back(table_2);
-        //   collision_object.primitive_poses.push_back(table_2_pose);
-        //   collision_object.operation = collision_object.ADD;
-
-        //   moveit_msgs::msg::PlanningSceneWorld psw;
-        //   psw.collision_objects.push_back(collision_object);
-
-        //   auto ps = std::make_unique<moveit_msgs::msg::PlanningScene>();
-        //   ps->world = psw;
-        //   ps->is_diff = true;
-        //   collision_pub_->publish(std::move(ps));
-        // });
-    }
-
-    // ~JoyToServoPub() override
-    // {
-    //     if (collision_pub_thread_.joinable())
-    //         collision_pub_thread_.join();
-    // }
-
-    void joyCB(const sensor_msgs::msg::Joy::ConstSharedPtr& msg)
-    {
-        // Create the messages we might publish
-        auto twist_msg = std::make_unique<geometry_msgs::msg::TwistStamped>();
-        auto joint_msg = std::make_unique<control_msgs::msg::JointJog>();
-
-        // This call updates the frame for twist commands
-        updateCmdFrame(frame_to_publish_, msg->buttons);
-
-        // Convert the joystick message to Twist or JointJog and publish
-        if (convertJoyToCmd(msg->axes, msg->buttons, twist_msg, joint_msg))
-        {
-            // publish the TwistStamped
-            twist_msg->header.frame_id = frame_to_publish_;
-            twist_msg->header.stamp = this->now();
-            twist_pub_->publish(std::move(twist_msg));
-        }
-        // else
-        // {
-        //     // publish the JointJog
-        //     joint_msg->header.stamp = this->now();
-        //     joint_msg->header.frame_id = "panda_link3";
-        //     joint_pub_->publish(std::move(joint_msg));
-        // }
-    }
-
-private:
-    rclcpp::Subscription<sensor_msgs::msg::Joy>::SharedPtr joy_sub_;
-    rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr twist_pub_;
-    rclcpp::Publisher<control_msgs::msg::JointJog>::SharedPtr joint_pub_;
-    rclcpp::Publisher<moveit_msgs::msg::PlanningScene>::SharedPtr collision_pub_;
-    rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr servo_start_client_;
-
-    std::string frame_to_publish_;
-
-    // std::thread collision_pub_thread_;
-};  // class JoyToServoPub
-
-}  // namespace servo_arm_twist_pkg
-
-// Register the component with class_loader
-#include <rclcpp_components/register_node_macro.hpp>
-RCLCPP_COMPONENTS_REGISTER_NODE(servo_arm_twist_pkg::JoyToServoPub)