diff --git a/src/astra_arm_moveit_config/config/astra_arm_simulated_config.yaml b/src/astra_arm_moveit_config/config/astra_arm_simulated_config.yaml new file mode 100644 index 0000000..6a71af2 --- /dev/null +++ b/src/astra_arm_moveit_config/config/astra_arm_simulated_config.yaml @@ -0,0 +1,71 @@ +############################################### +# Modify all parameters related to servoing here +############################################### +use_gazebo: false # Whether the robot is started in a Gazebo simulation environment + +## Properties of incoming commands +command_in_type: "unitless" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s +scale: + # Scale parameters are only used if command_in_type=="unitless" + linear: 0.2 # Max linear velocity. Unit is [m/s]. Only used for Cartesian commands. + rotational: 0.4 # Max angular velocity. Unit is [rad/s]. Only used for Cartesian commands. + # Max joint angular/linear velocity. Only used for joint commands on joint_command_in_topic. + joint: 0.5 + +# Optionally override Servo's internal velocity scaling when near singularity or collision (0.0 = use internal velocity scaling) +# override_velocity_scaling_factor = 0.0 # valid range [0.0:1.0] + +## Properties of outgoing commands +publish_period: 0.034 # 1/Nominal publish rate [seconds] +low_latency_mode: false # Set this to true to publish as soon as an incoming Twist command is received (publish_period is ignored) + +# What type of topic does your robot driver expect? +# Currently supported are std_msgs/Float64MultiArray or trajectory_msgs/JointTrajectory +command_out_type: trajectory_msgs/JointTrajectory + +# What to publish? Can save some bandwidth as most robots only require positions or velocities +publish_joint_positions: true +publish_joint_velocities: true +publish_joint_accelerations: false + +## Plugins for smoothing outgoing commands +smoothing_filter_plugin_name: "online_signal_smoothing::ButterworthFilterPlugin" + +# If is_primary_planning_scene_monitor is set to true, the Servo server's PlanningScene advertises the /get_planning_scene service, +# which other nodes can use as a source for information about the planning environment. +# NOTE: If a different node in your system is responsible for the "primary" planning scene instance (e.g. the MoveGroup node), +# then is_primary_planning_scene_monitor needs to be set to false. +is_primary_planning_scene_monitor: true + +## MoveIt properties +move_group_name: astra_arm # Often 'manipulator' or 'arm' +planning_frame: base_link # The MoveIt planning frame. Often 'base_link' or 'world' + +## Other frames +ee_frame_name: End_Effector # The name of the end effector link, used to return the EE pose +robot_link_command_frame: base_link # commands must be given in the frame of a robot link. Usually either the base or end effector + +## Stopping behaviour +incoming_command_timeout: 0.1 # Stop servoing if X seconds elapse without a new command +# If 0, republish commands forever even if the robot is stationary. Otherwise, specify num. to publish. +# Important because ROS may drop some messages and we need the robot to halt reliably. +num_outgoing_halt_msgs_to_publish: 4 + +## Configure handling of singularities and joint limits +lower_singularity_threshold: 17.0 # Start decelerating when the condition number hits this (close to singularity) +hard_stop_singularity_threshold: 30.0 # Stop when the condition number hits this +joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving quickly, make this larger. +leaving_singularity_threshold_multiplier: 2.0 # Multiply the hard stop limit by this when leaving singularity (see https://github.com/ros-planning/moveit2/pull/620) + +## Topic names +cartesian_command_in_topic: ~/delta_twist_cmds # Topic for incoming Cartesian twist commands +joint_command_in_topic: ~/delta_joint_cmds # Topic for incoming joint angle commands +joint_topic: /joint_states +status_topic: ~/status # Publish status to this topic +command_out_topic: /astra_arm_controller/joint_trajectory # Publish outgoing commands here + +## Collision checking for the entire robot body +check_collisions: true # Check collisions? +collision_check_rate: 10.0 # [Hz] Collision-checking can easily bog down a CPU if done too often. +self_collision_proximity_threshold: 0.01 # Start decelerating when a self-collision is this far [m] +scene_collision_proximity_threshold: 0.02 # Start decelerating when a scene collision is this far [m] diff --git a/src/astra_arm_moveit_config/config/initial_positions.yaml b/src/astra_arm_moveit_config/config/initial_positions.yaml index d7d7a92..8380ed4 100644 --- a/src/astra_arm_moveit_config/config/initial_positions.yaml +++ b/src/astra_arm_moveit_config/config/initial_positions.yaml @@ -3,8 +3,8 @@ initial_positions: Axis_0_Joint: 0 Axis_1_Joint: 0 - Axis_2_Joint: 0 - Axis_3_Joint: 0 + Axis_2_Joint: -1.6 + Axis_3_Joint: -1.6 Gripper_Slider_Left: 0 Wrist-EF_Roll_Joint: 0 Wrist_Differential_Joint: 0 \ No newline at end of file diff --git a/src/astra_arm_moveit_config/config/ros2_controllers.yaml b/src/astra_arm_moveit_config/config/ros2_controllers.yaml index a043c8d..f9fc1b0 100644 --- a/src/astra_arm_moveit_config/config/ros2_controllers.yaml +++ b/src/astra_arm_moveit_config/config/ros2_controllers.yaml @@ -29,7 +29,7 @@ astra_arm_controller: state_interfaces: - position - velocity - allow_nonzero_velocity_at_trajectory_end: true + allow_nonzero_velocity_at_trajectory_end: false hand_controller: ros__parameters: joint: Gripper_Slider_Left \ No newline at end of file diff --git a/src/astra_arm_moveit_config/launch/demo.launch.py b/src/astra_arm_moveit_config/launch/demo.launch.py index 9ee060b..79ce0e6 100644 --- a/src/astra_arm_moveit_config/launch/demo.launch.py +++ b/src/astra_arm_moveit_config/launch/demo.launch.py @@ -22,6 +22,11 @@ from moveit_configs_utils.launch_utils import ( DeclareBooleanLaunchArg, ) +from launch_param_builder import ParameterBuilder +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + + def generate_launch_description(): moveit_config = MoveItConfigsBuilder("ASTRA_Arm", package_name="astra_arm_moveit_config").to_moveit_configs() # return generate_demo_launch(moveit_config) @@ -106,6 +111,68 @@ def generate_launch_description(): ) ) + ## SERVO + + # This sets the update rate and planning group name for the acceleration limiting filter. + acceleration_filter_update_period = {"update_period": 0.01} + planning_group_name = {"planning_group_name": "astra_arm"} + + # Get parameters for the Servo node + servo_params = { + "moveit_servo": ParameterBuilder("astra_arm_moveit_config") + .yaml("config/astra_arm_simulated_config.yaml") + .to_dict() + } + ld.add_action( + ComposableNodeContainer( + name="moveit_servo_demo_container", + namespace="/", + package="rclcpp_components", + executable="component_container_mt", + composable_node_descriptions=[ + # Example of launching Servo as a node component + # Assuming ROS2 intraprocess communications works well, this is a more efficient way. + # ComposableNode( + # package="moveit_servo", + # plugin="moveit_servo::ServoServer", + # name="servo_server", + # parameters=[ + # servo_params, + # moveit_config.robot_description, + # moveit_config.robot_description_semantic, + # ], + # ), + ComposableNode( + package="servo_arm_twist_pkg", + plugin="servo_arm_twist_pkg::JoyToServoPub", + name="controller_to_servo_twist_node", + ), + ComposableNode( + package="joy", + plugin="joy::Joy", + name="joy_node", + ), + ], + output="screen", + ) + ) + # Launch a standalone Servo node. + # As opposed to a node component, this may be necessary (for example) if Servo is running on a different PC + ld.add_action( + Node( + package="moveit_servo", + executable="servo_node_main", + parameters=[ + servo_params, + moveit_config.robot_description, + moveit_config.robot_description_semantic, + moveit_config.robot_description_kinematics, + ], + output="screen", + ) + ) + + ld.add_action( IncludeLaunchDescription( PythonLaunchDescriptionSource( diff --git a/src/servo_arm_twist_pkg/CMakeLists.txt b/src/servo_arm_twist_pkg/CMakeLists.txt new file mode 100644 index 0000000..8494a48 --- /dev/null +++ b/src/servo_arm_twist_pkg/CMakeLists.txt @@ -0,0 +1,219 @@ +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) +# C++ library containing the parameters initialization +# - This is intended to use with the component node when you +# - need to read the parameters loaded into servo in a separate +# - node. +# set(SERVO_PARAM_LIB_NAME ${SERVO_LIB_NAME}_parameters) + +# 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() + +include_directories( + include +) + +################### +## C++ Libraries ## +################### + +# # This library provides a way of loading parameters for servo +# add_library(${SERVO_PARAM_LIB_NAME} SHARED +# src/servo_parameters.cpp +# src/parameter_descriptor_builder.cpp +# ) +# set_target_properties(${SERVO_PARAM_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +# ament_target_dependencies(${SERVO_PARAM_LIB_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +# # This library provides a C++ interface for sending realtime twist or joint commands to a robot +# add_library(${SERVO_LIB_NAME} SHARED +# # These files are used to produce differential motion +# src/collision_check.cpp +# src/enforce_limits.cpp +# src/servo.cpp +# src/servo_calcs.cpp +# src/utilities.cpp +# ) +# set_target_properties(${SERVO_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +# ament_target_dependencies(${SERVO_LIB_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS}) +# target_link_libraries(${SERVO_LIB_NAME} ${SERVO_PARAM_LIB_NAME}) + +# add_library(${POSE_TRACKING} SHARED src/pose_tracking.cpp) +# ament_target_dependencies(${POSE_TRACKING} ${THIS_PACKAGE_INCLUDE_DEPENDS}) +# target_link_libraries(${POSE_TRACKING} ${SERVO_LIB_NAME}) + +##################### +## Component Nodes ## +##################### + +# # Add and export library to run as a ROS node component, and receive commands via topics +# add_library(${SERVO_COMPONENT_NODE} SHARED src/servo_node.cpp) +# ament_target_dependencies(${SERVO_COMPONENT_NODE} ${THIS_PACKAGE_INCLUDE_DEPENDS}) +# target_link_libraries(${SERVO_COMPONENT_NODE} ${SERVO_LIB_NAME}) +# rclcpp_components_register_nodes(${SERVO_COMPONENT_NODE} "servo_arm_twist_pkg::ServoNode") + +# 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") + +###################### +## Executable Nodes ## +###################### + +# # An executable node for the servo server +# add_executable(${SERVO_NODE_MAIN_NAME} src/servo_node_main.cpp) +# target_link_libraries(${SERVO_NODE_MAIN_NAME} ${SERVO_COMPONENT_NODE}) +# ament_target_dependencies(${SERVO_NODE_MAIN_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +# # An example of pose tracking +# add_executable(${POSE_TRACKING_DEMO_NAME} src/cpp_interface_demo/pose_tracking_demo.cpp) +# target_link_libraries(${POSE_TRACKING_DEMO_NAME} ${POSE_TRACKING}) +# ament_target_dependencies(${POSE_TRACKING_DEMO_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +# # Add executable to publish fake servo commands for testing/demo purposes +# add_executable(${FAKE_SERVO_CMDS_NAME} test/publish_fake_jog_commands.cpp) +# ament_target_dependencies(${FAKE_SERVO_CMDS_NAME} +# rclcpp +# geometry_msgs +# std_srvs +# ) + +############# +## Install ## +############# + +# Install Libraries +install( + TARGETS + # ${SERVO_LIB_NAME} + # ${SERVO_LIB_NAME}_parameters + # ${POSE_TRACKING} + # ${SERVO_COMPONENT_NODE} + ${SERVO_CONTROLLER_INPUT} + EXPORT export_${PROJECT_NAME} + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include +) + +# # Install Binaries +# install( +# TARGETS +# ${SERVO_NODE_MAIN_NAME} +# ${CPP_DEMO_NAME} +# ${POSE_TRACKING_DEMO_NAME} +# ${FAKE_SERVO_CMDS_NAME} +# ARCHIVE DESTINATION lib +# LIBRARY DESTINATION lib +# RUNTIME DESTINATION lib/${PROJECT_NAME} +# ) + +# Install include, launch, config directories +install(DIRECTORY include/ DESTINATION include) +install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) + +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) + +############# +## TESTING ## +############# + +# if(BUILD_TESTING) +# find_package(ament_lint_auto REQUIRED) +# find_package(ament_cmake_gtest REQUIRED) +# find_package(ros_testing REQUIRED) +# find_package(Boost REQUIRED COMPONENTS filesystem) + +# # These don't pass yet, disable them for now +# set(ament_cmake_copyright_FOUND TRUE) +# set(ament_cmake_cpplint_FOUND TRUE) +# set(ament_cmake_flake8_FOUND TRUE) +# set(ament_cmake_uncrustify_FOUND TRUE) + +# # Run all lint tests in package.xml except those listed above +# ament_lint_auto_find_test_dependencies() + +# # Servo integration launch test +# ament_add_gtest_executable(test_servo_integration +# test/test_servo_interface.cpp +# test/servo_launch_test_common.hpp +# ) +# target_link_libraries(test_servo_integration ${SERVO_PARAM_LIB_NAME}) +# ament_target_dependencies(test_servo_integration ${THIS_PACKAGE_INCLUDE_DEPENDS}) +# add_ros_test(test/launch/test_servo_integration.test.py TIMEOUT 120 ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") + +# # Servo collision checking integration test +# ament_add_gtest_executable(test_servo_collision +# test/test_servo_collision.cpp +# test/servo_launch_test_common.hpp +# ) +# target_link_libraries(test_servo_collision ${SERVO_PARAM_LIB_NAME}) +# ament_target_dependencies(test_servo_collision ${THIS_PACKAGE_INCLUDE_DEPENDS}) +# add_ros_test(test/launch/test_servo_collision.test.py TIMEOUT 120 ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") + +# # pose_tracking +# ament_add_gtest_executable(test_servo_pose_tracking +# test/pose_tracking_test.cpp +# ) +# ament_target_dependencies(test_servo_pose_tracking ${THIS_PACKAGE_INCLUDE_DEPENDS}) +# target_link_libraries(test_servo_pose_tracking ${POSE_TRACKING}) +# add_ros_test(test/launch/test_servo_pose_tracking.test.py TIMEOUT 120 ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") + +# # Unit tests +# ament_add_gtest(servo_calcs_unit_tests +# test/servo_calcs_unit_tests.cpp +# ) +# target_link_libraries(servo_calcs_unit_tests ${SERVO_LIB_NAME}) + +# endif() + +ament_package() diff --git a/src/servo_arm_twist_pkg/README.md b/src/servo_arm_twist_pkg/README.md new file mode 100644 index 0000000..727ae50 --- /dev/null +++ b/src/servo_arm_twist_pkg/README.md @@ -0,0 +1,3 @@ +# 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 new file mode 100644 index 0000000..2e9be76 --- /dev/null +++ b/src/servo_arm_twist_pkg/package.xml @@ -0,0 +1,59 @@ + + + + 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 new file mode 100644 index 0000000..5a117d0 --- /dev/null +++ b/src/servo_arm_twist_pkg/src/joystick_twist.cpp @@ -0,0 +1,271 @@ +/********************************************************************* + * 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 (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& 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)