From 1fac75a3faa9e29349d8335ee4a9e5175f61498a Mon Sep 17 00:00:00 2001 From: DzC-0i <11778119+DzC-0i@user.noreply.gitee.com> Date: Tue, 17 Dec 2024 12:13:21 +0800 Subject: [PATCH 1/2] =?UTF-8?q?Update=20v1.0.10=20=E6=96=B0=E5=A2=9Emoveit?= =?UTF-8?q?=E8=BF=90=E5=8A=A8=E6=B5=8B=E8=AF=95?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- coarm_move_cpp_test/CMakeLists.txt | 73 +++++++++++ .../launch/coarm_move_cpp_test.launch.py | 23 ++++ coarm_move_cpp_test/package.xml | 24 ++++ coarm_move_cpp_test/src/move_client_test.cpp | 101 +++++++++++++++ coarm_move_cpp_test/src/move_server.cpp | 117 +++++++++++++++++ coarm_move_cpp_test/src/move_test.cpp | 119 ++++++++++++++++++ 6 files changed, 457 insertions(+) create mode 100644 coarm_move_cpp_test/CMakeLists.txt create mode 100644 coarm_move_cpp_test/launch/coarm_move_cpp_test.launch.py create mode 100644 coarm_move_cpp_test/package.xml create mode 100644 coarm_move_cpp_test/src/move_client_test.cpp create mode 100644 coarm_move_cpp_test/src/move_server.cpp create mode 100644 coarm_move_cpp_test/src/move_test.cpp diff --git a/coarm_move_cpp_test/CMakeLists.txt b/coarm_move_cpp_test/CMakeLists.txt new file mode 100644 index 0000000..9775cd7 --- /dev/null +++ b/coarm_move_cpp_test/CMakeLists.txt @@ -0,0 +1,73 @@ +cmake_minimum_required(VERSION 3.22) +project(coarm_move_cpp_test) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(moveit_ros_planning_interface REQUIRED) +find_package(rclcpp REQUIRED) +find_package(coarm_msgs REQUIRED) + +add_executable(move_test src/move_test.cpp) +add_executable(move_server src/move_server.cpp) +add_executable(move_client_test src/move_client_test.cpp) + +target_include_directories(move_test PUBLIC + $ + $) +target_include_directories(move_server PUBLIC + $ + $) +target_include_directories(move_client_test PUBLIC + $ + $) + +target_compile_features(move_test PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 +ament_target_dependencies( + move_test + moveit_ros_planning_interface + rclcpp +) + +target_compile_features(move_server PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 +ament_target_dependencies( + move_server + rclcpp + coarm_msgs + moveit_ros_planning_interface +) +target_compile_features(move_client_test PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 +ament_target_dependencies( + move_client_test + rclcpp + coarm_msgs +) + +install(TARGETS move_test move_server move_client_test + DESTINATION lib/${PROJECT_NAME}) + +install( + DIRECTORY launch + DESTINATION share/${PROJECT_NAME} +) + +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() diff --git a/coarm_move_cpp_test/launch/coarm_move_cpp_test.launch.py b/coarm_move_cpp_test/launch/coarm_move_cpp_test.launch.py new file mode 100644 index 0000000..3f07e63 --- /dev/null +++ b/coarm_move_cpp_test/launch/coarm_move_cpp_test.launch.py @@ -0,0 +1,23 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_demo_launch + +def generate_launch_description(): + + # Set MoveIt configuration + moveit_config = MoveItConfigsBuilder("coarm", "coarm_description", "coarm_moveit_config").to_moveit_configs() + + # Define hello_moveit node + coarm_move_cpp_test_node = Node( + package="coarm_move_cpp_test", + executable="move_test", + output="screen", + parameters=[ + moveit_config.robot_description, # Load URDF + moveit_config.robot_description_semantic, # Load SRDF + moveit_config.robot_description_kinematics, # Load kinematics.yaml + ], + ) + + return LaunchDescription([coarm_move_cpp_test_node]) diff --git a/coarm_move_cpp_test/package.xml b/coarm_move_cpp_test/package.xml new file mode 100644 index 0000000..1d91d68 --- /dev/null +++ b/coarm_move_cpp_test/package.xml @@ -0,0 +1,24 @@ + + + + coarm_move_cpp_test + 0.0.0 + TODO: Package description + DzC-0i + Apache-2.0 + + ament_cmake + + moveit_ros_planning_interface + rclcpp + coarm_msgs + + moveit + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/coarm_move_cpp_test/src/move_client_test.cpp b/coarm_move_cpp_test/src/move_client_test.cpp new file mode 100644 index 0000000..0f6bfdb --- /dev/null +++ b/coarm_move_cpp_test/src/move_client_test.cpp @@ -0,0 +1,101 @@ +#include +#include + +using namespace std::chrono_literals; + +class MoveClient : public rclcpp::Node +{ +public: + // 构造函数,有一个参数为节点名称 + MoveClient(std::string name) : Node(name) + { + RCLCPP_INFO(this->get_logger(), "Node Start: %s.", name.c_str()); + move_clinet_ = + this->create_client("/coarm_moveit_server/move_server", rmw_qos_profile_services_default); + + while (!move_clinet_->wait_for_service(1s)) + { + if (!rclcpp::ok()) + { + std::cerr << "\033[31m[ERROR]\tInterrupted while waiting for the service. Exiting.\033[0m" << std::endl; + return; + } + RCLCPP_INFO(this->get_logger(), "Move server not available, waiting again..."); + } + RCLCPP_INFO(this->get_logger(), "Move server initialized"); + } + ~MoveClient() {} + + void send_request(double x, double y, double z, double rx, double ry, double rz) + { + auto request = std::make_shared(); + request->pose.x = x; + request->pose.y = y; + request->pose.z = z; + request->pose.rx = rx; + request->pose.ry = ry; + request->pose.rz = rz; + + auto future = + move_clinet_->async_send_request(request, std::bind(&MoveClient::result_callback, this, std::placeholders::_1)); + } + +private: + using MoveitMove = coarm_msgs::srv::MoveitMove; + rclcpp::Client::SharedPtr move_clinet_; + const std::string red = "\033[31m"; + const std::string green = "\033[32m"; + const std::string reset = "\033[0m"; + + void result_callback(rclcpp::Client::SharedFuture result_future) + { + auto response = result_future.get(); + if (response->success) + { + RCLCPP_INFO(this->get_logger(), "%s%s%s", green.c_str(), response->message.c_str(), reset.c_str()); + } + else + { + RCLCPP_ERROR(this->get_logger(), "%s%s%s", red.c_str(), response->message.c_str(), reset.c_str()); + } + } +}; + +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + double x = 0.28; + double y = -0.2; + double z = 0.5; + double rx = 0.0; + double ry = 1.5708; + double rz = 0.0; + if (argc != 7) + { + std::cout << "\033[31musage:" << "X Y Z RX RY RZ, use default value\033[0m" << std::endl; + } + else + { + x = std::stod(argv[1]); + y = std::stod(argv[2]); + z = std::stod(argv[3]); + rx = std::stod(argv[4]); + ry = std::stod(argv[5]); + rz = std::stod(argv[6]); + std::cout << "\033[32m" << x << "\t" + << y << "\t" + << z << "\t" + << rx << "\t" + << ry << "\t" + << rz << "\033[0m" << std::endl; + } + /*创建对应节点的共享指针对象*/ + auto node = std::make_shared("move_client"); + + node->send_request(x, y, z, rx, ry, rz); + + /* 运行节点，并检测退出信号*/ + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/coarm_move_cpp_test/src/move_server.cpp b/coarm_move_cpp_test/src/move_server.cpp new file mode 100644 index 0000000..3212b10 --- /dev/null +++ b/coarm_move_cpp_test/src/move_server.cpp @@ -0,0 +1,117 @@ +#include +#include +#include +// #include +#include +#include + +const auto &logger = rclcpp::get_logger("move_server"); + +class MoveServer : public rclcpp::Node +{ +public: + MoveServer(std::string name) : Node(name, rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)) + { + using namespace std::placeholders; + RCLCPP_INFO(logger, "%s 节点测试1", name.c_str()); // ROS2日志输出 + + move_server_ = this->create_service("/coarm_moveit_server/move_server", + std::bind(&MoveServer::handle_service_callback, this, _1, _2), + rmw_qos_profile_services_default); + // RCLCPP_INFO(logger, "MoveIt move Server is ready!"); + } + ~MoveServer() {} + + // 初始化movei_geoup + void initialize_move_group() + { + move_group_ = std::make_shared(shared_from_this(), "arm"); + // 配置规划参数 + move_group_configurePlanning(); + } + +private: + using MoveitMove = coarm_msgs::srv::MoveitMove; + rclcpp::Service::SharedPtr move_server_; + std::shared_ptr move_group_; + + void handle_service_callback(std::shared_ptr request, + std::shared_ptr response) + { + double pose_x = request->pose.x; + double pose_y = request->pose.y; + double pose_z = request->pose.z; + double pose_rx = request->pose.rx; + double pose_ry = request->pose.ry; + double pose_rz = request->pose.rz; + RCLCPP_INFO(logger, "pose:%lf %lf %lf, %lf %lf %lf", pose_x, pose_y, pose_z, pose_rx, pose_ry, pose_rz); + + geometry_msgs::msg::Pose target_pose; + target_pose.position.x = pose_x; + target_pose.position.y = pose_y; + target_pose.position.z = pose_z; + // 使用 tf2 将 RPY 转换为四元数 + tf2::Quaternion q; + q.setRPY(pose_rx, pose_ry, pose_rz); + + // 设置到 target_pose 的 orientation + target_pose.orientation = tf2::toMsg(q); + + move_group_->setStartStateToCurrentState(); + move_group_->setPoseTarget(target_pose); + // move_group_->setApproximateJointValueTarget(target_pose); + + moveit::planning_interface::MoveGroupInterface::Plan plan; + bool success = (move_group_->plan(plan) == moveit::core::MoveItErrorCode::SUCCESS); + + if (success) + { + RCLCPP_INFO(logger, "Moving to target pose..."); + move_group_->execute(plan); + response->success = true; + response->message = "Motion executed successfully."; + } + else + { + RCLCPP_ERROR(logger, "Planning failed!"); + response->success = false; + response->message = "Motion planning failed."; + } + } + + void move_group_configurePlanning() + { + // 允许在规划失败后重新规划 + move_group_->allowReplanning(true); + // 设置尝试规划轨迹的次数为10次 + move_group_->setNumPlanningAttempts(10); + // 设置规划算法的最大允许时间为5秒 + move_group_->setPlanningTime(5.0); + // // 设置路径规划算法 + // move_group_->setPlanningPipelineId("ompl"); + // move_group_->setPlannerId("RRTConnectkConfigDefault"); + + // // 设置运动到目标位置（单位米）和关节姿态的误差（单位弧度） + // move_group_->setGoalPositionTolerance(0.01); + // move_group_->setGoalJointTolerance(0.03); + // 设置最大关节速度限制的比例因子,可取值范围(0,1] + move_group_->setMaxVelocityScalingFactor(0.8); + // // 最大关节加速度 + // move_group_->setMaxAccelerationScalingFactor(0.8); + } +}; + +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + auto const node = std::make_shared("move_server"); + + node->initialize_move_group(); + + // const auto& logger = rclcpp::get_logger("move_server"); + // RCLCPP_INFO(logger, "move_server 节点测试2."); + + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/coarm_move_cpp_test/src/move_test.cpp b/coarm_move_cpp_test/src/move_test.cpp new file mode 100644 index 0000000..8647e6e --- /dev/null +++ b/coarm_move_cpp_test/src/move_test.cpp @@ -0,0 +1,119 @@ +#include + +#include +#include +#include + +int main(int argc, char *argv[]) +{ + // Initialize ROS and create the Node + rclcpp::init(argc, argv); + auto const node = std::make_shared( + "move_test", rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node); + auto spinner = std::thread([&executor]() { executor.spin(); }); + + // Create a ROS logger + const auto& logger = rclcpp::get_logger("move_test"); + RCLCPP_INFO(logger, "move_test 节点已经启动."); + + // Next step goes here + // Create the MoveIt MoveGroup Interface + using moveit::planning_interface::MoveGroupInterface; + auto move_group_interface = MoveGroupInterface(node, "arm"); + // 获取规划组的规划参考系 + std::string planning_frame = move_group_interface.getPlanningFrame(); + RCLCPP_INFO_STREAM(logger, "Planning frame: " << planning_frame); + + // 允许在第一次规划失败后重新规划 + move_group_interface.allowReplanning(true); + // 设置尝试规划轨迹的次数为10次。 + move_group_interface.setNumPlanningAttempts(10); + // 设置规划算法设置最大允许时间为5秒。 + move_group_interface.setPlanningTime(5.0); + // 设置路径规划算法 + move_group_interface.setPlannerId("RRTConnectkConfigDefault"); + + // // 设置运动到目标位置（单位米）和关节姿态的误差（单位弧度） + // move_group_interface.setGoalPositionTolerance(0.02); + // move_group_interface.setGoalJointTolerance(0.03); + + // 设置一个比例因子以选择性的降低最大关节速度限制，可取值范围(0,1]。 + move_group_interface.setMaxVelocityScalingFactor(0.8); + + std::vector release_joint = move_group_interface.getCurrentJointValues(); + + move_group_interface.setNamedTarget("home"); + move_group_interface.move(); + + // Set a target Pose + auto const target_pose = [] + { + geometry_msgs::msg::Pose msg; + msg.orientation.w = 1.0; + msg.position.x = 0.28; + msg.position.y = -0.2; + msg.position.z = 0.5; + return msg; + }(); + //将规划的起始状态设置为机械臂当前状态 + move_group_interface.setStartStateToCurrentState(); + move_group_interface.setPoseTarget(target_pose); + + moveit::planning_interface::MoveGroupInterface::Plan plan_joint; + bool success_plan = (move_group_interface.plan(plan_joint) == + moveit::core::MoveItErrorCode::SUCCESS); + + if (success_plan) + { + RCLCPP_INFO(logger, "Moving to target pose..."); + move_group_interface.execute(plan_joint); + } + else + { + RCLCPP_ERROR(logger, "Planning failed!"); + } + + // 定义目标关节位置 + std::vector target_joint = { 0, -0.685, 0.3, 1.470, 0.522, 0.011}; + //将规划的起始状态设置为机械臂当前状态 + move_group_interface.setStartStateToCurrentState(); + // 设置目标位姿 + move_group_interface.setJointValueTarget(target_joint); + + // Create a plan to that target pose + auto const [success, plan] = [&move_group_interface] + { + moveit::planning_interface::MoveGroupInterface::Plan msg; + auto const ok = static_cast(move_group_interface.plan(msg)); + return std::make_pair(ok, msg); + }(); + + // Execute the plan + if (success) + { + RCLCPP_INFO(logger, "Moving to target pose..."); + move_group_interface.execute(plan); + } + else + { + RCLCPP_ERROR(logger, "Planning failed!"); + } + + move_group_interface.setNamedTarget("home"); + move_group_interface.move(); + + //将规划的起始状态设置为机械臂当前状态 + move_group_interface.setStartStateToCurrentState(); + // 设置目标位姿 + move_group_interface.setJointValueTarget(release_joint); + + move_group_interface.move(); + + // Shutdown ROS + rclcpp::shutdown(); + spinner.join(); + return 0; +} \ No newline at end of file -- Gitee From 28346a3fdb9ea9c5c2c6351ce1579938acafc712 Mon Sep 17 00:00:00 2001 From: DzC-0i <11778119+DzC-0i@user.noreply.gitee.com> Date: Tue, 17 Dec 2024 12:15:58 +0800 Subject: [PATCH 2/2] =?UTF-8?q?Update=20v1.0.10=20=E6=96=B0=E5=A2=9Emoveit?= =?UTF-8?q?=E8=BF=90=E5=8A=A8=E6=B5=8B=E8=AF=95?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- README.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/README.md b/README.md index 0b625f8..f1c3c24 100644 --- a/README.md +++ b/README.md @@ -6,6 +6,9 @@ The coarm_ros2 repository allowing efficient control of the Coarm robots within Coarm机械臂ROS2开发包，支持Coarm机械臂在ROS2下的集成控制。 +### Update v1.0.10 +1. 新增moveit运动测试 + ### Update v1.0.9 1. 新增MoveJ接口。coarm_planner中可以通过参数控制对MoveJ接口的调用 -- Gitee