mirror of
https://github.com/SHC-ASTRA/rover-ros2.git
synced 2026-02-11 09:20:40 +00:00
added ros-gz-sim and ros-gz-bridge to packages
This commit is contained in:
@@ -18,6 +18,12 @@
|
|||||||
pkgs = import nixpkgs {
|
pkgs = import nixpkgs {
|
||||||
inherit system;
|
inherit system;
|
||||||
overlays = [ nix-ros-overlay.overlays.default ];
|
overlays = [ nix-ros-overlay.overlays.default ];
|
||||||
|
config = {
|
||||||
|
# Allow the insecure freeimage package
|
||||||
|
permittedInsecurePackages = [
|
||||||
|
"freeimage-3.18.0-unstable-2024-04-18"
|
||||||
|
];
|
||||||
|
};
|
||||||
};
|
};
|
||||||
in
|
in
|
||||||
{
|
{
|
||||||
@@ -69,6 +75,8 @@
|
|||||||
ompl
|
ompl
|
||||||
chomp-motion-planner
|
chomp-motion-planner
|
||||||
joy
|
joy
|
||||||
|
ros-gz-sim
|
||||||
|
ros-gz-bridge
|
||||||
# ros2-controllers nixpkg does not build :(
|
# ros2-controllers nixpkg does not build :(
|
||||||
];
|
];
|
||||||
}
|
}
|
||||||
|
|||||||
26
src/moveit_servo/CMakeLists.txt
Normal file
26
src/moveit_servo/CMakeLists.txt
Normal file
@@ -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()
|
||||||
4
src/moveit_servo/config/kinematics.yaml
Normal file
4
src/moveit_servo/config/kinematics.yaml
Normal file
@@ -0,0 +1,4 @@
|
|||||||
|
panda_arm:
|
||||||
|
kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
|
||||||
|
kinematics_solver_search_resolution: 0.005
|
||||||
|
kinematics_solver_timeout: 0.05
|
||||||
25
src/moveit_servo/launch/servo.launch.py
Normal file
25
src/moveit_servo/launch/servo.launch.py
Normal file
@@ -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])
|
||||||
18
src/moveit_servo/package.xml
Normal file
18
src/moveit_servo/package.xml
Normal file
@@ -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>
|
||||||
@@ -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()
|
|
||||||
@@ -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.
|
|
||||||
@@ -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>
|
|
||||||
@@ -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)
|
|
||||||
Reference in New Issue
Block a user