[learning] robot move

README.md

this is from a beginner: trying to visualize a designated robot model perform move under moveit control. Some core code files are stored here for study reference.

Note: Rviz need to add PlanningScene and Trajectory in moveit_ros_visualization, and Marker Array in rviz_default_plugins

more detail from: moveit tutuorial

CMakeLists.txt

cmake_minimum_required(VERSION 3.8)
project(moverobot)

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(moveit_ros_planning_interface REQUIRED)
find_package(moveit_visual_tools REQUIRED)

add_executable(moverobot src/moverobot.cpp)
target_include_directories(moverobot PUBLIC
  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
  $<INSTALL_INTERFACE:include>)
target_compile_features(moverobot PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17
ament_target_dependencies(
  moverobot
  "rclcpp"
  "moveit_ros_planning_interface"
  "moveit_visual_tools"  # 可视化
)

install(TARGETS moverobot
  DESTINATION lib/${PROJECT_NAME})

# 这里把 launch 和 config 拷贝到安装目录下，程序运行时才可以找到
install(DIRECTORY
  launch config
  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()

moverobot.cpp

#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit_visual_tools/moveit_visual_tools.h>

#include <memory>
#include <rclcpp/rclcpp.hpp>
#include <thread>

int main(int argc, char *argv[]) {
  // Initialize ROS and create the Node
  rclcpp::init(argc, argv);
  auto const node = std::make_shared<rclcpp::Node>(
      "moverobot",
      rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(
          true));

  // Create a ROS logger
  auto const logger = rclcpp::get_logger("moverobot");

  // 创建一个线程
  // We spin up a SingleThreadedExecutor for the current state monitor
  // to get information about the robot's state.
  // https://docs.ros2.org/crystal/api/rclcpp/classrclcpp_1_1executors_1_1SingleThreadedExecutor.html
  rclcpp::executors::SingleThreadedExecutor executor;
  executor.add_node(node); // 添加 node 执行其回调函数（可以多个）
  auto spinner = std::thread([&executor]() { executor.spin(); });

  // Create the MoveIt MoveGroup Interface
  using moveit::planning_interface::MoveGroupInterface;
  auto move_group_interface =
      MoveGroupInterface(node, "elite_arm"); // planning group

  // Construct and initialize MoveItVisualTools
  // Parameters:
  // - node
  // - base frame (as common base for all visualization markers)
  //   NOTE: the frame name is link name (usually be /world)
  // - the rostopic to publish markers
  //   NOTE: static const std::string RVIZ_MARKER_TOPIC = "/rviz_visual_tools";
  // - robot model
  auto moveit_visual_tools = moveit_visual_tools::MoveItVisualTools{
      node, "world", rviz_visual_tools::RVIZ_MARKER_TOPIC,
      move_group_interface
          .getRobotModel()}; // 初始化
                             // moveit_visual_tools（花括号{}进行列表初始化）
  moveit_visual_tools.deleteAllMarkers();  // clear markers in RViz
  moveit_visual_tools.loadRemoteControl(); // Remote control is a simple plugin
                                           // that lets us have a button in RViz
                                           // to interact with our program.

  // Create closures for updating the text in rviz
  // 创建匿名函数快速进行相关操作 (绘制函数)
  auto const draw_title = [&moveit_visual_tools](auto text) {
    auto const text_pose = [] {
      auto msg = Eigen::Isometry3d::Identity();
      msg.translation().z() = 1.0; // Place text 1m above the base link
      return msg;
    }();
    moveit_visual_tools.publishText(text_pose, text, rviz_visual_tools::WHITE,
                                    rviz_visual_tools::XLARGE);
  }; // post a given text 1m above the base link
  auto const prompt = [&moveit_visual_tools](auto text) {
    moveit_visual_tools.prompt(text);
  }; // pause and wait for user condirmation to continue
  auto const draw_trajectory_tool_path =
      [&moveit_visual_tools,
       jmg = move_group_interface.getRobotModel()->getJointModelGroup(
           "elite_arm")](auto const trajectory) {
        moveit_visual_tools.publishTrajectoryLine(trajectory, jmg);
      }; // draw a trajectory line

  // 正式操作
  // 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.setPoseTarget(target_pose);

  // Create a plan to that target pose
  prompt("Press 'next' in the RvizVisualToolsGui window to plan");
  draw_title("Planning");
  moveit_visual_tools.trigger();
  auto const [success, plan] = [&move_group_interface] {
    moveit::planning_interface::MoveGroupInterface::Plan msg;
    auto const ok = static_cast<bool>(move_group_interface.plan(msg));
    return std::make_pair(ok, msg);
  }();
  // NOTE：尽管目标已经达到，但是重复 plan 会生成不一样的轨迹，不知道为什么

  // Execute the plan
  if (success) {
    draw_trajectory_tool_path(
        plan.trajectory_); // ❗版本不同，可能是 plan.trajectory
    moveit_visual_tools.trigger(); // 不知为何我使用时没有显示轨迹
    prompt("Press 'next' in the RvizVisualToolsGui window to execute");
    draw_title("Executing");
    moveit_visual_tools.trigger();
    move_group_interface.execute(plan);
  } else {
    draw_title("Planning Failed!");
    moveit_visual_tools.trigger();
    RCLCPP_ERROR(logger, "Planning failed!");
  }

  // Shutdown ROS
  rclcpp::shutdown(); // This will cause the spin function in the thread to
                      // return
  spinner.join();     // Join the thread before exiting
                      // 阻塞当前线程直到 spinner 线程返回
  return 0;
}

run_launch.py

import os
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, OpaqueFunction
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch.conditions import IfCondition, UnlessCondition
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
from launch.actions import ExecuteProcess
from ament_index_python.packages import get_package_share_directory
from moveit_configs_utils import MoveItConfigsBuilder


def generate_launch_description():
    moveit_config = (
        # load from package `elite_moveit_config`
        MoveItConfigsBuilder("ec66", package_name="elite_moveit_config")
        # Load robot description. 这项可以省去，会自动查找
        .robot_description(file_path="config/ec66.urdf.xacro")
        # Load trajectory execution and moveit controller managers' parameters 这项可以省去，会自动查找
        .trajectory_execution(
            file_path="config/moveit_controllers.yaml"
        ).planning_scene_monitor(  # modify default settings
            publish_robot_description=True, publish_robot_description_semantic=True
        )
        # # Load planning pipelines parameters from `config/xxx_planning.yaml` 这项可以省去，会自动查找
        # .planning_pipelines(
        #     pipelines=["ompl", "chomp", "pilz_industrial_motion_planner"]
        # )
        # Get a `MoveItConfigs` instance with all parameters loaded.
        .to_moveit_configs()
    )  # brackets can me omitted actually

    rviz_config_path = os.path.join(
        get_package_share_directory("moverobot"), "config", "moveit.rviz"
    )  # or you can use: str(moveit_config.package_path / "config/moveit.rviz")
    rviz_config = DeclareLaunchArgument(
        "rviz_config",
        default_value=rviz_config_path,
        description="RViz configuration file",
    )
    declared_arguments = [rviz_config]

    # Start the actual move_group node/action server
    run_move_group_node = Node(
        package="moveit_ros_move_group",
        executable="move_group",
        output="screen",
        parameters=[moveit_config.to_dict()],
    )

    # RViz
    rviz_node = Node(
        package="rviz2",
        executable="rviz2",
        name="rviz2",
        output="log",
        arguments=["-d", LaunchConfiguration("rviz_config")],
        parameters=[
            moveit_config.robot_description,
            moveit_config.robot_description_semantic,
            moveit_config.robot_description_kinematics,
            moveit_config.planning_pipelines,
            moveit_config.joint_limits,
        ],
    )

    # Static TF
    static_tf = Node(
        package="tf2_ros",
        executable="static_transform_publisher",
        name="static_transform_publisher",
        output="log",
        arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "world", "base_link"],
    )

    # Publish TF
    robot_state_publisher = Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        name="robot_state_publisher",
        respawn=True,  # dont know what means
        output="both",
        parameters=[moveit_config.robot_description],
    )

    ## ros2_control related nodes (ros2_control using FakeSystem as hardware)
    ros2_controllers_path = str(
        moveit_config.package_path / "config/ros2_controllers.yaml"
    )  # ros2_controllers.yaml 中定义了后面启动的控制器
    ros2_control_node = Node(
        package="controller_manager",
        executable="ros2_control_node",
        parameters=[moveit_config.robot_description, ros2_controllers_path],
        output="both",
    )

    # publish joint states
    joint_state_broadcaster_spawner = Node(
        package="controller_manager",
        executable="spawner",
        arguments=[
            "joint_state_broadcaster",  # 启动名为 joint_state_broadcaster 的控制器
            "--controller-manager-timeout",
            "300",
            "--controller-manager",  # -c 指定 controller manager
            "/controller_manager",  # controller manager 的名称为 /controller_manager
        ],
    )
    # load controllers
    arm_controller_spawner = Node(
        package="controller_manager",
        executable="spawner",
        arguments=[
            "elite_arm_controller",  # 启动名为 elite_arm_controller 的控制器
            "-c",  # -c 指定 controller manager
            "/controller_manager",  # 控制器在 /controller_manager 下被管理
        ],
    )

    nodes_to_start = [
        rviz_node,
        static_tf,
        robot_state_publisher,
        run_move_group_node,
        ros2_control_node,
        joint_state_broadcaster_spawner,
        arm_controller_spawner,
    ]

    return LaunchDescription(declared_arguments + nodes_to_start)

😄😄😄😄😄😄😄😄😄😄