一般我们启动机器人模型是通过以下的.launch文件启动，如下所示：
<launch>
<param name="robot_description" command="$(find xacro)/xacro --inorder '$(find robot_joint)/urdf/tf_robot.xacro'" />
<arg name="gui" default="true" />
<param name="use_gui" value="$(arg gui)"/>
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
<node name="rviz" pkg="rviz" type="rviz" args = "-d $(find robot_joint)/config/config.rviz" required="true" />
</launch>其中机器人模型（.urdf或者.xacro文件）是通过上述launch文件的第一条命令如下所示，这条命令读取文件中描述的机器人模型
<param name="robot_description" command="$(find xacro)/xacro --inorder '$(find robot_joint)/urdf/tf_robot.xacro'" />机器人模型通过上述命令给robot_description赋值，可以看到其前面是param， 这个是指定ros 参数服务器中的参数值，而打开rviz之后rviz就是直接从参数服务器中读取机器人描述文件， 然后进行显示，use_gui也是如此对显示产生的影响.。另外，还发起了两个发布者节点分别发布joint_states和robot_state，这也就是会由joint_states话题的原因，赋的值为：’$ (find xacro)/xacro --inorder’ 在输入参数’为’$ (arg model)’ 情况下的的运行结果。
至于其中启动的两个节点，joint_state_publisher 和 robot_state_publisher，可以查看ROS Answer上相关的解释。joint_state_publisher从ROS参数服务器中读取robot_description参数, 找到所有non-fixed joint，发布他们的 JointState 消息到 /joint_states 话题，而robot_state_publisher从 /joint_states 话题中获取机器人joint角度作为输入，使用机器人的运动学树模型计算出机器人link的3D姿态，然后将其发布到话题 /tf 和 /tf_static 。
为了验证joint_state_publisher 和 robot_state_publisher他们之间的关系，我们自己编写了 /joint_states 的话题发布程序，用于发布机器人模型的non-fixed joint节点，然后robot_state_publisher从 /joint_states 话题中获取机器人joint角度作为输入，在将其发布到话题 /tf 和 /tf_static 。我们可以通过启动rviz观察机器人的关节运动控制是否符合我们自己编写的/joint_states运动控制信息。 其发布/joint_states节点的控制程序如下：
#include <string>
#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include <tf/transform_broadcaster.h>
int main(int argc, char** argv) {
ros::init(argc, argv, "state_publisher");
ros::NodeHandle n;
//发布joint_states节点
ros::Publisher joint_pub = n.advertise<sensor_msgs::JointState>("joint_states", 1);
tf::TransformBroadcaster broadcaster;
ros::Rate loop_rate(30);
const double degree = M_PI/180;
// 机器人模型的关节运动变量
double inc= 0.005, base_arm_inc= 0.005, arm1_armbase_inc= 0.005, arm2_arm1_inc= 0.005;
double angle= 0 ,base_arm = 0, arm1_armbase = 0, arm2_arm1 = 0;
// 发布odom坐标到机器人坐标的TF变换
geometry_msgs::TransformStamped odom_trans;
sensor_msgs::JointState joint_states;
odom_trans.header.frame_id = "odom"; //odom名称
odom_trans.child_frame_id = "base_footprint"; //机器人地盘
while (ros::ok()) {
//update joint_state
joint_states.header.stamp = ros::Time::now();
joint_states.name.resize(7); //定义7个运动节点
joint_states.position.resize(7);
joint_states.name[0] ="base_to_arm_base"; //与机器人地盘相连的旋转转盘
joint_states.position[0] = base_arm;
joint_states.name[1] ="arm1_to_arm_base"; //与机器人旋转底座相连的第一节机械臂
joint_states.position[1] = arm1_armbase;
joint_states.name[2] ="arm2_to_arm1"; //与第一节机械臂相连的第二节机械臂
joint_states.position[2] = arm2_arm1;
joint_states.name[3] ="joint_wheel_1"; //以下为四个轮子
joint_states.position[3] = 0;
joint_states.name[4] ="joint_wheel_2";
joint_states.position[4] = 0;
joint_states.name[5] ="joint_wheel_3";
joint_states.position[5] = 0;
joint_states.name[6] ="joint_wheel_4";
joint_states.position[6] = 0;
// update transform
// (moving in a circle with radius)
odom_trans.header.stamp = ros::Time::now();
odom_trans.transform.translation.x = cos(angle);
odom_trans.transform.translation.y = sin(angle);
odom_trans.transform.translation.z = 0.0;
odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(angle);
//send the joint state and transform
joint_pub.publish(joint_states); //发布joint_states消息
broadcaster.sendTransform(odom_trans); //发布tf变换
//以下为更新机器人可动节点的状态
arm2_arm1 += arm2_arm1_inc;
if (arm2_arm1<-1.5 || arm2_arm1>1.5) arm2_arm1_inc *= -1;
arm1_armbase += arm1_armbase_inc;
if (arm1_armbase>1.2 || arm1_armbase<-1.0) arm1_armbase_inc *= -1;
base_arm += base_arm_inc;
if (base_arm>1. || base_arm<-1.0) base_arm_inc *= -1;
angle += degree/4;
// This will adjust as needed per iteration
loop_rate.sleep();
}
return 0;
}
机器人的模型文件如下所示：
<?xml version="1.0"?>
<robot name="mrobot" xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:include filename="$(find robot_joint)/urdf/robot_body.xacro"/>
<robot_body/>
</robot>robot_body.xacro文件如下所示：
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:property name="length_wheel" value="0.12" />
<xacro:property name="radius_wheel" value="0.18" />
<xacro:macro name="default_inertial" params="mass">
<inertial>
<mass value="${mass}"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0" />
</inertial>
</xacro:macro>
<xacro:macro name="wheel" params="parent number lengthwheel radiuswheel locationX locationY locationZ">
<joint name="joint_wheel_${number}" type="continuous">
<axis xyz="0 0 1" />
<anchor xyz="0 0 0" />
<limit effort="100" velocity="100" />
<origin xyz="${locationX} ${locationY} ${locationZ}" rpy="1.5707 0 0" />
<parent link="${parent}"/>
<child link="wheel_${number}"/>
</joint>
<link name="wheel_${number}">
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<cylinder length="${lengthwheel}" radius="${radiuswheel}"/>
</geometry>
<material name="white1">
<color rgba="0.7 1 1 0.9"/>
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<cylinder length="${lengthwheel}" radius="${radiuswheel}"/>
</geometry>
</collision>
<xacro:default_inertial mass="2"/>
</link>
</xacro:macro>
<xacro:macro name="robot_body">
<link name="base_footprint">
<visual>
<geometry>
<cylinder length="0.001" radius="0.1"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
<origin rpy="0 0 0" xyz="0 0 0"/>
</visual>
</link>
<joint name="base_footprint_joint" type="fixed">
<origin xyz="0 0 0" rpy="0 0 0"/>
<parent link="base_footprint"/>
<child link="base_link"/>
</joint>
<link name="base_link">
<visual>
<geometry>
<box size="0.8 1.2 0.3"/>
</geometry>
<origin rpy="0 0 1.57079" xyz="0 0 0.15"/>
<material name="yellow">
<color rgba="1 0.4 0 1"/>
</material>
</visual>
<collision>
<geometry>
<box size="0.8 1.2 0.1"/>
</geometry>
</collision>
<xacro:default_inertial mass="10"/>
</link>
<link name="arm_base">
<visual>
<geometry>
<box size="0.3 0.3 0.2"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0"/>
<material name="color1">
<color rgba="0.3 0.5 0.6 1"/>
</material>
</visual>
<collision>
<geometry>
<box size="0.1 0.1 0.1"/>
</geometry>
</collision>
<xacro:default_inertial mass="1"/>
</link>
<joint name="base_to_arm_base" type="continuous">
<parent link="base_link"/>
<child link="arm_base"/>
<axis xyz="0 0 1"/>
<origin xyz="0 0 0.3"/>
</joint>
<link name="arm1">
<visual>
<geometry>
<box size="0.05 0.05 0.5"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0.2"/>
<material name="yellow">
<color rgba="1 0.4 0 1"/>
</material>
</visual>
<collision>
<geometry>
<box size="0.05 0.05 0.5"/>
</geometry>
</collision>
<xacro:default_inertial mass="1"/>
</link>
<joint name="arm1_to_arm_base" type="revolute">
<parent link="arm_base"/>
<child link="arm1"/>
<axis xyz="1 0 0"/>
<origin xyz="0 0 0.15"/>
<limit effort="1000.0" lower="-1.0" upper="1.0" velocity="0.5"/>
</joint>
<link name="arm2">
<visual>
<geometry>
<box size="0.05 0.05 0.5"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0.25"/>
<material name="color2">
<color rgba="0.8 0.8 0.5 1"/>
</material>
</visual>
<collision>
<geometry>
<box size="0.05 .05 0.5"/>
</geometry>
</collision>
<xacro:default_inertial mass="1"/>
</link>
<joint name="arm2_to_arm1" type="revolute">
<parent link="arm1"/>
<child link="arm2"/>
<axis xyz="0 1 0"/>
<origin xyz="0.0 0 0.45"/>
<limit effort="1000.0" lower="-2.5" upper="2.5" velocity="0.5"/>
</joint>
<wheel parent="base_link" number="1" lengthwheel="${length_wheel}" radiuswheel="${radius_wheel}" locationX="0.4" locationY="0.48" locationZ="0"/>
<wheel parent="base_link" number="2" lengthwheel="${length_wheel}" radiuswheel="${radius_wheel}" locationX="-0.4" locationY="0.48" locationZ="0"/>
<wheel parent="base_link" number="3" lengthwheel="${length_wheel}" radiuswheel="${radius_wheel}" locationX="0.4" locationY="-0.48" locationZ="0"/>
<wheel parent="base_link" number="4" lengthwheel="${length_wheel}" radiuswheel="${radius_wheel}" locationX="-0.4" locationY="-0.48" locationZ="0"/>
</xacro:macro>
</robot>
launch启动文件：
<launch>
<param name="robot_description" command="$(find xacro)/xacro --inorder '$(find robot_joint)/urdf/tf_robot.xacro'" />
<arg name="gui" default="true" />
<param name="use_gui" value="$(arg gui)"/>
<node name="state_publisher" pkg="robot_joint" type="state_publisher" />
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
<node name="rviz" pkg="rviz" type="rviz" args = "-d $(find robot_joint)/config/config.rviz" required="true" />
</launch>
把上述的文件放在工作空间中编译，然后启动文件命令：
roslaunch robot_joint state_xacro.launch这时rviz显示如下的模型：
其机器人模型的各个可动关节的运动受state_publisher.cpp程序发布的/joint_states节点信息控制。这个例子我们验证了joint_state_publisher 和 robot_state_publisher他们之间的关系，我们通过编写/joint_states 的话题发布程序，用于发布机器人模型的non-fixed joint节点，然后robot_state_publisher从 /joint_states 话题中获取机器人joint角度作为输入，在将其发布到话题 /tf 和 /tf_static 。我们可以通过启动rviz观察机器人的关节运动控制是否符合我们自己编写的/joint_states运动控制信息。
在终端中输入如下命令，可以查看机器人模型的TF框架树
rosrun rqt_tf_tree rqt_tf_tree
输入以下命令，可以查看/joint_states主题的发布者与接收者
rostopic info /joint_states 
输入以下命令，可以查看/joint_states携带的信息
rostopic echo /joint_states 
其中name为控制的关节名称，position为需要设置的关节位置，velocity为需要设置的关节速度，effort为需要设置的最大功率，速度一定时决定机械手停止的最大握力。
其完整功能包的链接：
https://download.csdn.net/download/qq_34911636/13977006
扫一扫
8838
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
