Code 7.21: Arbotix 控制器的安装
# 安装Arbotix功能包
sudo apt-get install ros-melodic-arbotix
# 安装Arbotix功 能 包 所 依 赖 的 串 口 库
pip install pyserial
Arbotix安装完成后就需要在功能包的更目录下创建config文件夹,并在其中创建robot_arbotix.yaml文件
robot_arbotix.yaml
controllers: {
base_controller: {
type: diff_controller ,
base_frame_id: base_link ,
base_width: 0.52 ,
ticks_meter: 2000 ,
Kp: 12,
Kd: 12,
Ki: 0,
Ko: 50,
accel_limit: 1.0
}
}
simple_sai_robot2.urdf
<?xml version="1.0"?>
<robot name="sai_robot">
<link name="base_link" />
<link name="robot_body_layer">
<visual >
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<geometry >
<cylinder radius="0.25" length="0.01"/>
</geometry >
<material name="yellow">
<color rgba="1 0.4 0.0 1.0"/>
</material >
</visual >
<collision >
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<geometry >
<cylinder radius="0.25" length="0.01"/>
</geometry >
</collision >
<inertial >
<mass value="0.5"/>
<inertia ixx="0.00782" ixy="0" ixz="0"
iyy="0.00782" iyz="0" izz="0.01563"/>
</inertial >
</link >
<gazebo reference ="robot_body_layer">
<material >Gazebo/Orange </material >
</gazebo >
<joint name="dummy_joint" type="fixed">
<origin xyz="0.0 0.0 0.095" rpy="0.0 0.0 0.0"/>
<parent link="base_link"/>
<child link="robot_body_layer"/>
</joint >
<link name="left_motor_link">
<visual >
<origin xyz="0.0 0 0" rpy="1.57 0.0 0.0"/>
<geometry >
<cylinder radius="0.02" length="0.08"/>
</geometry >
<material name="gray">
<color rgba="0.75 0.75 0.75 1.0"/>
</material >
</visual >
<collision>
<origin xyz="0.0 0 0" rpy="1.57 0.0 0.0"/>
<geometry >
<cylinder radius="0.02" length="0.08"/>
</geometry >
</collision >
<inertial >
<mass value="0.1"/>
<inertia ixx="0.00006" ixy="0" ixz="0"
iyy="0.00006" iyz="0" izz="0.00002"/>
</inertial >
</link >
<gazebo reference ="left_motor_link">
<material >Gazebo/Grey</material >
</gazebo >
<link name="right_motor_link">
<visual >
<origin xyz="0.0 0 0" rpy="1.57 0.0 0.0"/>
<geometry >
<cylinder radius="0.02" length="0.08"/>
</geometry >
<material name="gray" />
</visual >
<collision >
<origin xyz="0.0 0 0" rpy="1.57 0.0 0.0"/>
<geometry >
<cylinder radius="0.02" length="0.08"/>
</geometry >
</collision >
<inertial >
<mass value="0.1"/>
<inertia ixx="0.00006" ixy="0" ixz="0"
iyy="0.00006" iyz="0" izz="0.00002"/>
</inertial >
</link >
<gazebo reference ="right_motor_link">
<material >Gazebo/Grey </material >
</gazebo >
<joint name="left_motor_join" type="fixed">
<origin xyz="0.125 0.22 -0.025" rpy="0.0 0.0 0.0"/>
<parent link="robot_body_layer"/>
<child link="left_motor_link"/>
</joint >
<joint name="right_motor_join" type="fixed">
<origin xyz="0.125 -0.22 -0.025" rpy="0.0 0.0 0.0"/>
<parent link="robot_body_layer"/>
<child link="right_motor_link"/>
</joint >
<link name="left_wheel_link">
<visual >
<origin xyz="0.0 0 0" rpy="1.57 0.0 0.0"/>
<geometry >
<cylinder radius="0.07" length="0.04"/>
</geometry >
<material name="black">
<color rgba="0.0 0.0 0.0 1.0"/>
</material >
</visual >
<collision >
<origin xyz="0.0 0 0" rpy="1.57 0.0 0.0"/>
<geometry >
<cylinder radius="0.07" length="0.04"/>
</geometry >
</collision >
<inertial >
<mass value="0.1"/>
<inertia ixx="0.00014" ixy="0" ixz="0"
iyy="0.00014" iyz="0" izz="0.00025"/>
</inertial >
</link >
<gazebo reference ="left_wheel_link">
<material >Gazebo/Black</material >
</gazebo >
<link name="right_wheel_link">
<visual >
<origin xyz="0.0 0 0" rpy="1.57 0.0 0.0"/>
<geometry >
<cylinder radius="0.07" length="0.04"/>
</geometry >
<material name="black" />
</visual >
<collision >
<origin xyz="0.0 0 0" rpy="1.57 0.0 0.0"/>
<geometry >
<cylinder radius="0.07" length="0.04"/>
</geometry >
</collision >
<inertial >
<mass value="0.1"/>
<inertia ixx="0.00014" ixy="0" ixz="0"
iyy="0.00014" iyz="0" izz="0.00025"/>
</inertial >
</link >
<gazebo reference ="right_wheel_link">
<material >Gazebo/Black </material >
</gazebo >
<joint name="left_wheel_joint" type="continuous">
<origin xyz="0.0 0.06 0.0" rpy="0.0 0.0 0.0"/>
<parent link="left_motor_link"/>
<child link="left_wheel_link"/>
<axis xyz="0.0 1.0 0.0"/>
</joint >
<joint name="right_wheel_joint" type="continuous">
<origin xyz="0.0 -0.06 0.0" rpy="0.0 0.0 0.0"/>
<parent link="right_motor_link"/>
<child link="right_wheel_link"/>
<axis xyz="0.0 1.0 0.0"/>
</joint >
<link name="caster_link">
<visual >
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<geometry >
<sphere radius="0.045"/>
</geometry >
<material name="black" />
</visual >
<collision >
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<geometry >
<sphere radius="0.045"/>
</geometry >
</collision >
<inertial >
<mass value="0.1"/>
<inertia ixx="0.00008" ixy="0" ixz="0"
iyy="0.00008" iyz="0" izz="0.00008"/>
</inertial >
</link >
<gazebo reference ="caster_link">
<material >Gazebo/Black </material >
<mu1 value="0.0"/>
<mu2 value="0.0"/>
<kp value="1000000.0" />
<kd value="10.0" />
</gazebo >
<joint name="caster_joint" type="fixed">
<origin xyz=" -0.125 0.0 -0.05" rpy="0.0 0.0 0.0"/>
<parent link="robot_body_layer"/>
<child link="caster_link"/>
</joint >
<link name="imu_link">
<visual >
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<geometry >
<box size="0.1 0.1 0.01"/>
</geometry >
<material name="green">
<color rgba="0.42 0.54 0.45 1.0"/>
</material >
</visual >
<collision >
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<geometry >
<box size="0.1 0.1 0.01"/>
</geometry >
</collision >
<inertial >
<mass value="0.05"/>
<inertia ixx="0.00004" ixy="0" ixz="0"
iyy="0.00008" iyz="0" izz="0.00004"/>
</inertial >
</link >
<gazebo reference ="imu_link">
<material >Gazebo/Green </material >
</gazebo >
<joint name="imu_joint" type="fixed">
<origin xyz=" -0.125 0.0 0.01" rpy="0.0 0.0 0.0"/>
<parent link="robot_body_layer"/>
<child link="imu_link"/>
</joint >
<link name="laser_link">
<visual >
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<geometry >
<cylinder radius="0.05" length="0.05"/>
</geometry >
<material name="blue">
<color rgba="0.0 0.0 1.0 1.0"/>
</material >
</visual >
<collision >
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<geometry >
<cylinder radius="0.05" length="0.05"/>
</geometry >
</collision >
<inertial >
<mass value="0.2"/>
<inertia ixx="0.00017" ixy="0" ixz="0"
iyy="0.00017" iyz="0" izz="0.00025"/>
</inertial >
</link >
<gazebo reference ="laser_link">
<material >Gazebo/SkyBlue </material >
</gazebo >
<joint name="laser_joint" type="fixed">
<origin xyz="0.0 0.0 0.03" rpy="0.0 0.0 0.0"/>
<parent link="robot_body_layer"/>
<child link="laser_link"/>
</joint >
<link name="camera_link">
<visual >
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<geometry >
<box size="0.03 0.15 0.02"/>
</geometry >
<material name="gold">
<color rgba="0.9 0.9 0.1 1.0"/>
</material >
</visual >
<collision >
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<geometry >
<box size="0.03 0.15 0.02"/>
</geometry >
</collision >
<inertial >
<mass value="0.2"/>
<inertia ixx="0.00002" ixy="0" ixz="0"
iyy="0.00039" iyz="0" izz="0.00038"/>
</inertial >
</link >
<gazebo reference ="camera_link">
<material >Gazebo/Gold </material >
</gazebo >
<joint name="camera_joint" type="fixed">
<origin xyz="0.18 0.0 0.015" rpy="0.0 0.0 0.0"/>
<parent link="robot_body_layer"/>
<child link="camera_link"/>
</joint >
<link name="us_back_link">
<visual >
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<geometry >
<box size="0.01 0.02 0.04"/>
</geometry >
<material name="red">
<color rgba="0.71 0.54 0.75 1.0"/>
</material >
</visual >
<collision >
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<geometry >
<box size="0.01 0.02 0.04"/>
</geometry >
</collision >
<inertial >
<mass value="0.05"/>
<inertia ixx="0.000007" ixy="0" ixz="0"
iyy="0.000002" iyz="0" izz="0.000008"/>
</inertial >
</link >
<gazebo reference ="us_back_link">
<material >Gazebo/Turquoise </material >
</gazebo >
<link name="us_front_left_link">
<visual >
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<geometry >
<box size="0.01 0.02 0.04"/>
</geometry >
<material name="red" />
</visual >
<collision >
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<geometry >
<box size="0.01 0.02 0.04"/>
</geometry >
<material name="red" />
</collision >
<inertial >
<mass value="0.05"/>
<inertia ixx="0.000007" ixy="0" ixz="0"
iyy="0.000002" iyz="0" izz="0.000008"/>
</inertial >
</link >
<gazebo reference ="us_front_left_link">
<material >Gazebo/ Turquoise </material >
</gazebo >
<link name="us_front_right_link">
<visual >
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<geometry >
<box size="0.01 0.02 0.04"/>
</geometry >
<material name="red" />
</visual >
<collision >
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<geometry >
<box size="0.01 0.02 0.04"/>
</geometry >
<material name="red" />
</collision >
<inertial >
<mass value="0.05"/>
<inertia ixx="0.000007" ixy="0" ixz="0"
iyy="0.000002" iyz="0" izz="0.000008"/>
</inertial >
</link >
<gazebo reference ="us_front_right_link">
<material >Gazebo/Turquoise </material >
</gazebo >
<joint name="us_back_joint" type="fixed">
<origin xyz=" -0.2 0.0 0.025" rpy="0.0 0.0 0.0"/>
<parent link="robot_body_layer"/>
<child link="us_back_link"/>
</joint >
<joint name="us_front_left_joint" type="fixed">
<origin xyz="0.18 0.12 0.025" rpy="0.0 0.0 0.0"/>
<parent link="robot_body_layer"/>
<child link="us_front_left_link"/>
</joint >
<joint name="us_front_right_joint" type="fixed">
<origin xyz="0.18 -0.12 0.025" rpy="0.0 0.0 0.0"/>
<parent link="robot_body_layer"/>
<child link="us_front_right_link"/>
</joint >
<transmission name="base_l_wheel_trans">
<type>transmission_interface/SimpleTransmission</type >
<joint name="base_l_wheel_joint">
<hardwareInterface>VelocityJointInterface</hardwareInterface>
</joint>
<actuator name="left_motor_link">
<mechanicalReduction >1</mechanicalReduction>
<hardwareInterface>VelocityJointInterface</hardwareInterface>
</actuator>
</transmission>
<transmission name="base_r_wheel_trans">
<type>transmission_interface/SimpleTransmission</type>
<joint name="base_r_wheel_joint">
<hardwareInterface>VelocityJointInterface</hardwareInterface>
</joint>
<actuator name="right_motor_link">
<mechanicalReduction >1</mechanicalReduction >
<hardwareInterface>VelocityJointInterface</hardwareInterface>
</actuator>
</transmission>
<gazebo >
<plugin name="differential_drive_controller"
filename="libgazebo_ros_diff_drive.so">
<rosDebugLevel >Debug</rosDebugLevel >
<robotNamespace >/</robotNamespace>
<alwaysOn >true </alwaysOn >
<odometrySource >world </odometrySource >
<broadcastTF >1</broadcastTF >
<publishTf >1</publishTf >
<publishWheelTF >true </publishWheelTF >
<publishOdomTF >true </publishOdomTF >
<publishWheelJointState >true </publishWheelJointState >
<updateRate >100.0 </updateRate >
<legacyMode >true </legacyMode >
<leftJoint >base_l_wheel_joint </leftJoint >
<rightJoint >base_r_wheel_joint </rightJoint >
<wheelSeparation >0.52 </wheelSeparation >
<wheelDiameter >0.14 </wheelDiameter >
<wheelTorque >30</wheelTorque >
<wheelAcceleration >1.8 </wheelAcceleration >
<commandTopic >cmd_vel</commandTopic >
<odometryFrame >odom </odometryFrame >
<odometryTopic >odom </odometryTopic >
<robotBaseFrame>base_footprint</robotBaseFrame>
</plugin>
</gazebo >
</robot>
ddd.launch
<launch >
<arg name="model"
default="$(find begin1)/urdf/simple_sai_robot2.urdf"/>
<arg name="rvizconfig"
default="$(find begin1)/rviz/simple_model.rviz" />
<arg name="gui" default="true" />
<param name="robot_description"
command="$(find xacro)/xacro $(arg model)" />
<node if="$(arg gui)" name="joint_state_publisher_gui"
pkg="joint_state_publisher_gui"
type="joint_state_publisher_gui" />
<node unless="$(arg gui)" 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 $(arg rvizconfig)"
required="true" />
<node name="arbotix" pkg="arbotix_python"
type="arbotix_driver" output="screen">
<rosparam file="$(find begin1)/config/robot_arbotix.yaml"
command="load" />
<param name="sim" value="true" />
</node >
</launch >
启动launch
roslaunch begin1 ddd.launch
让小车做圆周运动
rostopic pub /cmd_vel geometry_msgs/Twist -r 10 -- '[0.4 , 0.0, 0.0]' '[0.0 , 0.0,-1]'
Fixed Frame 设置为odom