请问：如何让urdf模型通过终端完成关节运动

H.

版本 ros-foxy

比如该码垛机械臂通过ros2实现关节控制

该机械臂是三自由度的机械臂,分别是base_link，Link_B,Link_S
注：Jiont_S_D 是根据Link_B,Link_S 的角度改变发生改变

目前我能够实现从SW模型倒入到rviz里面，通过自带界面完成关节运动

2022-11-07 17-38-24屏幕截图.png
2022-11-07 17-29-49屏幕截图.png

小鱼

@847718956 这个其实不难哈，你如果了解joint_state_publisher_gui这个界面是如何控制机械臂的，一定就知道怎么做了。

参考动手学ROS2文档：https://fishros.com/d2lros2/#/humble/chapt8/get_started/4.通过JointStates控制RVIZ2关节

具体方案就是不要joint_states_publisher，而是手动写个代码发布joint_states话题数据即可。

参考代码：

#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
# 1.导入消息类型JointState
from sensor_msgs.msg import JointState

import threading
import time

class RotateWheelNode(Node):
    def __init__(self,name):
        super().__init__(name)
        self.get_logger().info(f"node {name} init..")
        # 创建并初始化发布者成员属性pub_joint_states_
        self.joint_states_publisher_ = self.create_publisher(JointState,"joint_states", 10) 
        # 初始化数据
        self._init_joint_states()
        self.pub_rate = self.create_rate(30)
        self.thread_ = threading.Thread(target=self._thread_pub)
        self.thread_.start()

    
    def _init_joint_states(self):
        # 初始左右轮子的速度
        self.joint_speeds = [0.0,0.0]
        self.joint_states = JointState()
        self.joint_states.header.stamp = self.get_clock().now().to_msg()
        self.joint_states.header.frame_id = ""
        # 关节名称
        self.joint_states.name = ['left_wheel_joint','right_wheel_joint']
        # 关节的位置
        self.joint_states.position = [0.0,0.0]
        # 关节速度
        self.joint_states.velocity = self.joint_speeds
        # 力 
        self.joint_states.effort = []

    def update_speed(self,speeds):
        self.joint_speeds = speeds

    def _thread_pub(self):
        last_update_time = time.time()
        while rclpy.ok():
            delta_time =  time.time()-last_update_time
            last_update_time = time.time()
            # 更新位置
            self.joint_states.position[0]  += delta_time*self.joint_states.velocity[0]
            self.joint_states.position[1]  += delta_time*self.joint_states.velocity[1]
            # 更新速度
            self.joint_states.velocity = self.joint_speeds
            # 更新 header
            self.joint_states.header.stamp = self.get_clock().now().to_msg()
            # 发布关节数据
            self.joint_states_publisher_.publish(self.joint_states)
            self.pub_rate.sleep()

def main(args=None):
    rclpy.init(args=args) # 初始化rclpy
    node = RotateWheelNode("rotate_fishbot_wheel")  # 新建一个节点
    node.update_speed([15.0,-15.0])
    rclpy.spin(node) # 保持节点运行，检测是否收到退出指令（Ctrl+C）
    rclpy.shutdown() # 关闭rclpy

H.

@小鱼我使用aqt-grap生成的关系如下图，多了/robot_description
\

相比动手学Ros2文档内的关系图，我认为该模型不仅一些固定的坐标轴发生旋转，而且其他的位置的坐标轴也发生改变了

H.

@小鱼我使用aqt-grap生成的关系如下图，多了/robot_description
问题:需要做一个客户端吗，请求是不是和发布joint_states话题数据相同，在接收到的/robot_decription的数据该怎么做处理

当前进度我可以完成发布joint_states话题，但rviz打不开

2022-11-08 11-31-55屏幕截图.png

采用参考文档的launch文件,Fixed Frame下拉菜单不能点
2022-11-08 11-12-29屏幕截图.png

参考文档的launch文件

import os
from launch import LaunchDescription
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare


def generate_launch_description():
    package_name = 'rviz_example'
    urdf_name = "rviz_example.urdf"

    ld = LaunchDescription()
    pkg_share = FindPackageShare(package=package_name).find(package_name) 
    urdf_model_path = os.path.join(pkg_share, f'urdf/{urdf_name}')

    robot_state_publisher_node = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        arguments=[urdf_model_path]
        )

    joint_state_publisher_node = Node(
        package='joint_state_publisher_gui',
        executable='joint_state_publisher_gui',
        name='joint_state_publisher_gui',
        arguments=[urdf_model_path]
        )

    rviz2_node = Node(
        package='rviz2',
        executable='rviz2',
        name='rviz2',
        output='screen',
        )

    ld.add_action(robot_state_publisher_node)
    #ld.add_action(joint_state_publisher_node)
    ld.add_action(rviz2_node)

    return ld

我之前的launch文件

from ament_index_python.packages import get_package_share_path
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.conditions import IfCondition, UnlessCondition
from launch.substitutions import Command, LaunchConfiguration

from launch_ros.actions import Node
from launch_ros.parameter_descriptions import ParameterValue

def generate_launch_description():
    urdf_tutorial_path = get_package_share_path('rviz_example')
    default_model_path = urdf_tutorial_path / 'urdf/rviz_example.urdf'
    default_rviz_config_path = urdf_tutorial_path / 'rviz/urdf.rviz'

    gui_arg = DeclareLaunchArgument(name='gui', default_value='true', choices=['true', 'false'],
                                    description='Flag to enable joint_state_publisher_gui')
    model_arg = DeclareLaunchArgument(name='model', default_value=str(default_model_path),
                                      description='Absolute path to robot urdf file')
    rviz_arg = DeclareLaunchArgument(name='rvizconfig', default_value=str(default_rviz_config_path),
                                     description='Absolute path to rviz config file')

    robot_description = ParameterValue(Command(['xacro ', LaunchConfiguration('model')]),
                                       value_type=str)

    robot_state_publisher_node = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        parameters=[{'robot_description': robot_description}]
    )

    # Depending on gui parameter, either launch joint_state_publisher or joint_state_publisher_gui
    joint_state_publisher_gui_node = Node(
        package='joint_state_publisher_gui',
        executable='joint_state_publisher_gui',
        condition=IfCondition(LaunchConfiguration('gui'))
    )

    rviz_node = Node(
        package='rviz2',
        executable='rviz2',
        name='rviz2',
        output='screen',
        arguments=['-d', LaunchConfiguration('rvizconfig')],
    )

    return LaunchDescription([
        gui_arg,
        model_arg,
        rviz_arg,
        joint_state_publisher_gui_node,
        robot_state_publisher_node,
        rviz_node
    ])

小鱼

@847718956 在请问：如何让urdf模型通过终端完成关节运动中说：

需要做一个客户端吗，请求是不是和发布joint_states话题数据相同，在接收到的/robot_decription的数据该怎么做处理

需要写一个节点发布/joint_states信息，话题内容要包含你机械臂的关节名称，我提供的代码里的关节名称是fishbot的关节名称，你要修改成你的关节名称

@小鱼在请问：如何让urdf模型通过终端完成关节运动中说：

关节名称

    self.joint_states.name = ['left_wheel_joint','right_wheel_joint']

另外就是机械臂的关节是有上下限制的，所以不能直接用我给的程序那样子不断的递增发布数据，也要修改一下，简单改了一个版本。

你把这个文件写成一个节点，放到launch文件，替代原来的joint_state_publisher_gui就可以

#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
# 1.导入消息类型JointState
from sensor_msgs.msg import JointState

import threading
import time

class RotateWheelNode(Node):
    def __init__(self,name):
        super().__init__(name)
        self.get_logger().info(f"node {name} init..")
        # 创建并初始化发布者成员属性pub_joint_states_
        self.joint_states_publisher_ = self.create_publisher(JointState,"joint_states", 10) 
        # 初始化数据
        self._init_joint_states()
        self.pub_rate = self.create_rate(30)
        self.thread_ = threading.Thread(target=self._thread_pub)
        self.thread_.start()

    
    def _init_joint_states(self):
        # 初始左右轮子的速度
        self.joint_speeds = [0.0,0.0]
        self.joint_states = JointState()
        self.joint_states.header.stamp = self.get_clock().now().to_msg()
        self.joint_states.header.frame_id = ""
        # 关节名称
        self.joint_states.name = ['link1','link2','xxx','...]
        # 关节的位置
        self.joint_states.position = [0.0,0.0,xxx,...]
        # 力 
        self.joint_states.effort = []

    def update_position (self,position):
        self.joint_states.position= position

    def _thread_pub(self):
        while rclpy.ok():
            # 更新 header
            self.joint_states.header.stamp = self.get_clock().now().to_msg()
            # 发布关节数据
            self.joint_states_publisher_.publish(self.joint_states)
            self.pub_rate.sleep()

def main(args=None):
    rclpy.init(args=args) # 初始化rclpy
    node = RotateWheelNode("rotate_fishbot_wheel")  # 新建一个节点
    node.update_position ([0.0,0.0,xxx,...])
    rclpy.spin(node) # 保持节点运行，检测是否收到退出指令（Ctrl+C）
    rclpy.shutdown() # 关闭rclpy

H.

@小鱼已解决
joint_states.name 导入的是.urdf中的Joint的名称，不是Link的名称