视频链接:
ROS2 Moveit2 手把手教你 Panda 机械臂关节速度控制!(附代码)
代码仓库:GitHub - LitchiCheng/ros2_package
前期讲到了笛卡尔速度,也就是末端的速度控制,今天介绍下关节速度如何玩!
新建package,命名moveit_joint_velocity
ros2 pkg create --build-type ament_cmake moveit_joint_velocity --dependencies rclcpp control_msgs geometry_msgs moveit_servo moveit_core moveit_msgs planning_scene_monitor tf2_ros
CMakeLists.txt文件
cmake_minimum_required(VERSION 3.8)
project(moveit_joint_velocity)
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)
find_package(rclcpp REQUIRED)
find_package(control_msgs REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(moveit_servo REQUIRED)
find_package(moveit_core REQUIRED)
find_package(moveit_msgs REQUIRED)
find_package(tf2_ros 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()
add_executable(joint_velocity_node src/joint_velocity.cpp)
ament_target_dependencies(joint_velocity_node
rclcpp
control_msgs
geometry_msgs
moveit_servo
moveit_core
moveit_msgs
tf2_ros
)
install(TARGETS
joint_velocity_node
DESTINATION lib/${PROJECT_NAME}
)
ament_package()
src中新建joint_velocity_node.cpp
#include <rclcpp/rclcpp.hpp>
#include <moveit_servo/servo_parameters.h>
#include <moveit_servo/servo.h>
#include <moveit/planning_scene_monitor/planning_scene_monitor.h>
using namespace std::chrono_literals;
static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_joint_velocity.joint_velocity_node");
rclcpp::Node::SharedPtr node_;
rclcpp::Publisher<control_msgs::msg::JointJog>::SharedPtr joint_cmd_pub_;
std::string joint_name = panda_joint1;
double velocity = 0.0;
void publishCommands()
{
auto msg = std::make_unique<control_msgs::msg::JointJog>();
msg->header.stamp = node_->now();
msg->joint_names.push_back(joint_name);
msg->velocities.push_back(velocity);
joint_cmd_pub_->publish(std::move(msg));
}
int main(int argc, char** argv)
{
rclcpp::init(argc, argv);
rclcpp::NodeOptions node_options;
node_options.use_intra_process_comms(false);
node_ = std::make_shared<rclcpp::Node>("joint_velocity_node", node_options);
node_->declare_parameter("velocity", velocity);
node_->declare_parameter("joint_name", joint_name);
node_->get_parameter("velocity", velocity);
node_->get_parameter("joint_name", joint_name);
auto tf_buffer = std::make_shared<tf2_ros::Buffer>(node_->get_clock());
auto planning_scene_monitor = std::make_shared<planning_scene_monitor::PlanningSceneMonitor>(
node_, "robot_description", tf_buffer, "planning_scene_monitor");
if (planning_scene_monitor->getPlanningScene())
{
planning_scene_monitor->startStateMonitor("/joint_states");
planning_scene_monitor->setPlanningScenePublishingFrequency(25);
planning_scene_monitor->startPublishingPlanningScene(planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE,
"/moveit_servo/publish_planning_scene");
planning_scene_monitor->startSceneMonitor();
planning_scene_monitor->providePlanningSceneService();
}
else
{
RCLCPP_ERROR(LOGGER, "Planning scene not configured");
return EXIT_FAILURE;
}
joint_cmd_pub_ = node_->create_publisher<control_msgs::msg::JointJog>("joint_velocity_node/delta_joint_cmds", 10);
auto servo_parameters = moveit_servo::ServoParameters::makeServoParameters(node_);
if (!servo_parameters)
{
RCLCPP_FATAL(LOGGER, "Failed to load the servo parameters");
return EXIT_FAILURE;
}
auto servo = std::make_unique<moveit_servo::Servo>(node_, servo_parameters, planning_scene_monitor);
servo->start();
rclcpp::TimerBase::SharedPtr timer = node_->create_wall_timer(50ms, publishCommands);
auto executor = std::make_unique<rclcpp::executors::MultiThreadedExecutor>();
executor->add_node(node_);
executor->spin();
rclcpp::shutdown();
return 0;
}
编译运行,先运行rviz场景,再运行moveit_joint_node,指定joint_name和velocity
colcon build --packages-select moveit_joint_velocity
source install/setup.bash
ros2 launch moveit2_tutorials demo.launch.py rviz_config:=panda_moveit_config_demo_empty.rviz
ros2 run moveit_joint_velocity joint_velocity_node --ros-args -p joint_name:="panda_joint1" -p velocity:=0.3
不动的原因可能是到达关节限位