diff --git a/src/servo_arm_twist_pkg/CMakeLists.txt b/src/servo_arm_twist_pkg/CMakeLists.txt deleted file mode 100644 index 8719d44..0000000 --- a/src/servo_arm_twist_pkg/CMakeLists.txt +++ /dev/null @@ -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() diff --git a/src/servo_arm_twist_pkg/README.md b/src/servo_arm_twist_pkg/README.md deleted file mode 100644 index 727ae50..0000000 --- a/src/servo_arm_twist_pkg/README.md +++ /dev/null @@ -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. diff --git a/src/servo_arm_twist_pkg/package.xml b/src/servo_arm_twist_pkg/package.xml deleted file mode 100644 index c2d1601..0000000 --- a/src/servo_arm_twist_pkg/package.xml +++ /dev/null @@ -1,58 +0,0 @@ - - - - servo_arm_twist_pkg - 2.5.9 - Provides real-time manipulator Cartesian and joint servoing. - Blake Anderson - Andy Zelenak - Tyler Weaver - Henning Kayser - - BSD 3-Clause - - https://ros-planning.github.io/moveit_tutorials - - Brian O'Neil - Andy Zelenak - Blake Anderson - Alexander Rössler - Tyler Weaver - Adam Pettinger - - ament_cmake - moveit_common - - control_msgs - control_toolbox - geometry_msgs - moveit_msgs - moveit_core - moveit_ros_planning_interface - pluginlib - sensor_msgs - std_msgs - std_srvs - tf2_eigen - trajectory_msgs - - gripper_controllers - joint_state_broadcaster - joint_trajectory_controller - joy - robot_state_publisher - tf2_ros - moveit_configs_utils - launch_param_builder - - ament_cmake_gtest - ament_lint_auto - ament_lint_common - controller_manager - ros_testing - - - ament_cmake - - - diff --git a/src/servo_arm_twist_pkg/src/joystick_twist.cpp b/src/servo_arm_twist_pkg/src/joystick_twist.cpp deleted file mode 100644 index e098851..0000000 --- a/src/servo_arm_twist_pkg/src/joystick_twist.cpp +++ /dev/null @@ -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 -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -// 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_DEFAULTS = { { LEFT_TRIGGER, 1.0 }, { RIGHT_TRIGGER, 1.0 } }; -std::map 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& axes, const std::vector& buttons, - std::unique_ptr& twist, - std::unique_ptr& joint) -{ - // Give joint jogging priority because it is only buttons - // If any joint jog command is requested, we are only publishing joint commands - if (0) - { - // 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& 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( - JOY_TOPIC, rclcpp::SystemDefaultsQoS(), - [this](const sensor_msgs::msg::Joy::ConstSharedPtr& msg) { return joyCB(msg); }); - - twist_pub_ = this->create_publisher(TWIST_TOPIC, rclcpp::SystemDefaultsQoS()); - joint_pub_ = this->create_publisher(JOINT_TOPIC, rclcpp::SystemDefaultsQoS()); - // collision_pub_ = - // this->create_publisher("/planning_scene", rclcpp::SystemDefaultsQoS()); - - // Create a service client to start the ServoNode - servo_start_client_ = this->create_client("/servo_node/start_servo"); - servo_start_client_->wait_for_service(std::chrono::seconds(1)); - servo_start_client_->async_send_request(std::make_shared()); - - // // 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(); - // 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(); - auto joint_msg = std::make_unique(); - - // 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::SharedPtr joy_sub_; - rclcpp::Publisher::SharedPtr twist_pub_; - rclcpp::Publisher::SharedPtr joint_pub_; - rclcpp::Publisher::SharedPtr collision_pub_; - rclcpp::Client::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(servo_arm_twist_pkg::JoyToServoPub)