【附带源码】机械臂MoveIt2极简教程(五)、第二个demo - rviz可视化

系列文章目录

【附带源码】机械臂MoveIt2极简教程(一)、moveit2安装
【附带源码】机械臂MoveIt2极简教程(二)、move_group交互
【附带源码】机械臂MoveIt2极简教程(三)、URDF/SRDF介绍
【附带源码】机械臂MoveIt2极简教程(四)、第一个入门demo
【附带源码】机械臂MoveIt2极简教程(五)、第二个demo - rviz可视化



本节实现的效果就是在rviz中添加文字和机械臂运动轨迹画图

机械臂MoveIt2极简教程(五)、rviz可视化

在这里插入图片描述

本节demo不另外创建package包,直接在上一节的demo_moveit包中运行。做的改动包括三个

  • 新建C++代码
  • 修改launch文件的Node
  • 修改CMakeLists.txt

1. 新建C++代码

注意代码中两个地方的修改,一个是MoveGroupInterface传入的机械臂名称叫panda_arm;另一个是MoveItVisualTools中传入的tf link名称叫panda_link0。如果这两个地方没有和机械臂本体对应上,那么rviz中无法显示你需要的文字和路径图标。

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

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    auto node = rclcpp::Node::make_shared("visualize_in_rviz", rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true));

    rclcpp::executors::SingleThreadedExecutor executors;
    executors.add_node(node);
    std::thread([&executors]()
                { executors.spin(); })
        .detach();
    auto const logger = rclcpp::get_logger("visualize_in_rviz");
    RCLCPP_INFO(logger, "start ");

    auto move_group = moveit::planning_interface::MoveGroupInterface(node, "panda_arm");
    auto moveit_visual_tools = moveit_visual_tools::MoveItVisualTools(node, "panda_link0", rviz_visual_tools::RVIZ_MARKER_TOPIC, move_group.getRobotModel());
    moveit_visual_tools.deleteAllMarkers();
    moveit_visual_tools.loadRemoteControl();

    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);
    };

    auto const prompt = [&moveit_visual_tools](auto text)
    {
        moveit_visual_tools.prompt(text);
    };
    
    auto const draw_trajectory_tool_path =
        [&moveit_visual_tools,
         jmg = move_group.getRobotModel()->getJointModelGroup(
             "panda_arm")](auto const trajectory)
    {
        moveit_visual_tools.publishTrajectoryLine(trajectory, jmg);
    };

    // 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.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]
    {
        moveit::planning_interface::MoveGroupInterface::Plan msg;
        auto const ok = static_cast<bool>(move_group.plan(msg));
        return std::make_pair(ok, msg);
    }();

    // Execute the plan
    if (success)
    {
        draw_trajectory_tool_path(plan.trajectory_);
        moveit_visual_tools.trigger();
        prompt("Press 'Next' in the RvizVisualToolsGui window to execute");
        draw_title("Executing");
        moveit_visual_tools.trigger();
        move_group.execute(plan);
    }
    else
    {
        draw_title("Planning Failed!");
        moveit_visual_tools.trigger();
        RCLCPP_ERROR(logger, "Planning failed!");
    }

    rclcpp::shutdown();

    return 0;
}

2. 修改launch文件

沿用上一节的launch文件demo_moveit.launch.py

import os
import yaml
from launch import LaunchDescription
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
import xacro

def load_file(package_name, file_path):
    package_path = get_package_share_directory(package_name)
    absolute_file_path = os.path.join(package_path, file_path)

    try:
        with open(absolute_file_path, "r") as file:
            return file.read()
    except EnvironmentError:  # parent of IOError, OSError *and* WindowsError where available
        return None


def load_yaml(package_name, file_path):
    package_path = get_package_share_directory(package_name)
    absolute_file_path = os.path.join(package_path, file_path)

    try:
        with open(absolute_file_path, "r") as file:
            return yaml.safe_load(file)
    except EnvironmentError:  # parent of IOError, OSError *and* WindowsError where available
        return None


def generate_launch_description():
    robot_description_config = xacro.process_file(
        os.path.join(
            get_package_share_directory("moveit_resources_panda_moveit_config"),
            "config",
            "panda.urdf.xacro",
        )
    )
    robot_description = {"robot_description": robot_description_config.toxml()}

    robot_description_semantic_config = load_file(
        "moveit_resources_panda_moveit_config", "config/panda.srdf"
    )
    robot_description_semantic = {
        "robot_description_semantic": robot_description_semantic_config
    }

    kinematics_yaml = load_yaml(
        "moveit_resources_panda_moveit_config", "config/kinematics.yaml"
    )

    # 第四节
    # move_group_demo = Node(
    #     package="demo_moveit",
    #     executable="demo_moveit",
    #     name="demo_moveit",
    #     output="screen",
    #     parameters=[robot_description, 
    #                 robot_description_semantic, 
    #                 kinematics_yaml,
    #                 ],
    # )

    # 第五节
    move_group_demo = Node(
        package="demo_moveit",
        executable="visualize_in_rviz",
        name="visualize_in_rviz",
        output="screen",
        parameters=[robot_description, 
                    robot_description_semantic, 
                    kinematics_yaml,
                    ],
    )


    return LaunchDescription([move_group_demo])

3. 修改CMakeLists.txt

依旧沿用上一节的CMakeLists.txt,只是添加了新的节点

cmake_minimum_required(VERSION 3.10.2)
project(demo_moveit)

# Common cmake code applied to all moveit packages
find_package(moveit_common REQUIRED)
moveit_package()

find_package(Eigen3 REQUIRED)
find_package(Boost REQUIRED system filesystem date_time thread)
find_package(ament_cmake REQUIRED)
find_package(control_msgs REQUIRED)
find_package(moveit_core REQUIRED)
find_package(moveit_ros_planning REQUIRED)
find_package(moveit_ros_planning_interface REQUIRED)
find_package(moveit_ros_perception REQUIRED)
find_package(moveit_servo REQUIRED)
find_package(interactive_markers REQUIRED)
find_package(rviz_visual_tools REQUIRED)
find_package(moveit_visual_tools REQUIRED)
find_package(geometric_shapes REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_action REQUIRED)
find_package(pluginlib REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(tf2_eigen REQUIRED)
find_package(tf2_geometry_msgs REQUIRED)

set(THIS_PACKAGE_INCLUDE_DIRS
  doc/interactivity/include
)

set(THIS_PACKAGE_INCLUDE_DEPENDS
  ament_cmake
  rclcpp
  rclcpp_action
  tf2_geometry_msgs
  tf2_ros
  moveit_core
  rviz_visual_tools
  moveit_visual_tools
  moveit_ros_planning_interface
  interactive_markers
  tf2_geometry_msgs
  moveit_ros_planning
  pluginlib
  Eigen3
  Boost
  control_msgs
  moveit_servo
)

include_directories(${THIS_PACKAGE_INCLUDE_DIRS})


ament_export_dependencies(
  ${THIS_PACKAGE_INCLUDE_DEPENDS}
)

ament_export_include_directories(include)


# -------------------------------------------------------------- executable
add_executable(demo_moveit
  src/demo_moveit.cpp)

target_include_directories(demo_moveit
    PUBLIC include)

ament_target_dependencies(demo_moveit
    ${THIS_PACKAGE_INCLUDE_DEPENDS} Boost)

# -------------------------------------------------------------- executable
add_executable(visualize_in_rviz
  src/visualize_in_rviz.cpp)

target_include_directories(visualize_in_rviz
    PUBLIC include)

ament_target_dependencies(visualize_in_rviz
    ${THIS_PACKAGE_INCLUDE_DEPENDS} Boost)

# -------------------------------------------------------------- install
  
install(TARGETS demo_moveit visualize_in_rviz planning_around_objects
  DESTINATION lib/${PROJECT_NAME}
)
install(DIRECTORY launch
  DESTINATION share/${PROJECT_NAME}
)


ament_package()

4. 运行

  • 进入工作空间,编译项目
cd ~/ws_moveit

colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release --packages-select demo_moveit
  • 启动panda_arm机械臂
ros2 launch moveit2_tutorials demo.launch.py
  1. 记得先把MotionPlanning关闭,因为目前用不到;
  2. 添加插件RvizVisualToolsGui
  3. 添加MarkerArray,topic选择/rviz_visual_tools

在这里插入图片描述
在这里插入图片描述

  • 启动本节程序
ros2 launch demo_moveit demo_moveit.launch.py
  • 点击RvizVisualToolsGui的Next按钮,即可看到机械臂运动动画,如文章开头的视频展示的一样。

觉得对您有帮助的,可以点个赞👍支持一下,谢谢各位!

因为淋过雨,所以想为别人撑把伞;因为踩过太多坑,所以想让喜欢机器人的同学们减少试错成本!

  • 10
    点赞
  • 11
    收藏
    觉得还不错? 一键收藏
  • 打赏
    打赏
  • 0
    评论
MoveIt是一个功能强大的机器人操作系统(ROS)插件,用于规划和控制机械臂运动。它为机械臂提供了运动规划、逆运动学解算、碰撞检测等功能,使得机械臂能够在复杂的环境中进行精确和安全的移动。MoveIt使用了先进的运动规划算法,包括RRT、OMPL等,能够快速找到机械臂的运动轨迹。 Gazebo是一个用于仿真机器人和环境的开源物理仿真器。它模拟了机器人在现实世界中的动力学和物理特性,包括重力、摩擦力等。使用Gazebo,我们可以在虚拟环境中进行机器人的开发、测试和验证。Gazebo将机器人的模型和控制器与MoveIt集成,可以实现机械臂的运动仿真和控制。 MoveIt和Gazebo的结合可以提供一个完整的机械臂开发和仿真环境。首先,我们可以使用MoveIt规划机械臂的运动轨迹,并将其发送给Gazebo进行仿真。在仿真中,我们可以观察机械臂在不同环境中的运动情况,并进行调试和优化。同时,Gazebo还可以提供机器人周围环境的传感器数据,用于机械臂的感知和决策。 此外,MoveIt还支持与真实物理机械臂的集成。我们可以将规划好的轨迹发送给真实机械臂控制器,实现机械臂的精确控制。通过在实际环境中运行和测试机械臂,我们可以验证算法的正确性和鲁棒性。 综上所述,MoveIt和Gazebo机械臂提供了一个全面而强大的解决方案,用于机械臂的规划、运动仿真和控制。它们的结合使得机械臂的开发过程更加高效和可靠,并为机器人研究和应用提供了强大的工具。
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

押波张飞

觉得有用,是我最大的动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付:¥1
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值