ROS-3-Gazebo
1.为了更好的显示机器人的三维模型，我们调用Gazebo软件。先前我们通过joint_state_publisher来计算出机器人每个关节之间的坐标系关系，但是在这里，机器人应该自己提供这些信息，否则Ga’ze’bo不会知道机器人关节之间的坐标系关系。这就需要以下两个条件：Plugins和Transmission.
roslaunch urdf_sim_tutorial gazebo.launch
- Plugin
Plugin主要功能是将Gazebo与ROS相连接，使用方法是在urdf文件中添加说明，如下所示：
/
可以在以下的代码中查看结果，虽然没什么变化：
roslaunch urdf_sim_tutorial gazebo.launch model:=urdf/09-publishjoints.urdf.xacro
虽然在URDF文件中建立了两者的关系，但是我们仍然需要代码来说明我们想要将机器人模型在Gazebo中运行，也就是要建立Controller.控制器最初加载到ROS参数空间中。 我们有一个yaml文件joints.yaml，它指定了我们的第一个控制器。该控制器在joint_state_controller package中，可直接从Gazebo将机器人关节的状态发布到ROS中。在09-joints.launch中，可以看到我们应该如何将这个yaml文件加载到r2d2_joint_state_controller命名空间中。 然后我们使用该命名空间调用controller_manager / spawner`脚本，并将其加载到Gazebo中。通过运行以下代码会运行控制器，并将消息发布在/joint_states 话题上。
roslaunch urdf_sim_tutorial 09-joints.launch
3.Transmissions
对于每个非固定的关节，我们需要指定Transmission来告诉Gazebo这个关节可以做什么，因此在urdf文件中我们需要添加如下的代码，需要注意的是，joint name要和urdf文件中的joint名称相同。
transmission_interface/SimpleTransmission
1
PositionJointInterface
我们可以通过以下代码试运行：
roslaunch urdf_sim_tutorial 09-joints.launch model:=urdf/10-firsttransmission.urdf.xacro
- Joint Control
我们想要具体的来控制Transmission，因此我们在上述的controller中添加如下代码，注意type要和hardwareInterface相匹配：
type: "position_controllers/JointPositionController"
joint: head_swivel
之后可以使用如下代码来试运行：
roslaunch urdf_sim_tutorial 10-head.launch
之后Gazebo会订阅新的话题，我们可以通过在ROS中发布新的数值来任意改变joint的位置，例如：
rostopic pub /r2d2_head_controller/command std_msgs/Float64 "data: -0.707"
- 实现Joint Controller的协同配合
为了避免对单个joint进行操作而达到组群协同的目的，我们只需要在ROS parameters 中指定一个新的controller即可，该控制器如下：
type: "position_controllers/JointGroupPositionController"
joints:
- gripper_extension
- left_gripper_joint
- right_gripper_joint
我们可以通过以下代码来试运行：
roslaunch urdf_sim_tutorial 12-gripper.launch
之后我们可以通过发布数组，来指定每个joint的位置：
rostopic pub /r2d2_gripper_controller/command std_msgs/Float64MultiArray "layout:
dim:
- label: ''
size: 3
stride: 1
data_offset: 0
data: [0, 0.5, 0.5]"
7.实现轮子类型的joint的连续转动
为了实现这一目标，首先我们需要一个控制轮子速度的controller,其次我们需要一个控制joint转动位置的controller。
7.1速度controller
transmission_interface/SimpleTransmission
1
VelocityJointInterface
7.2转动位置controller
为了避免单独给每个轮子指定controller,我们可以将所有轮子一起控制，采用DiffDriverController，代码如下：
type: "diff_drive_controller/DiffDriveController"
publish_rate: 50
left_wheel: ['left_front_wheel_joint', 'left_back_wheel_joint']
right_wheel: ['right_front_wheel_joint', 'right_back_wheel_joint']
wheel_separation: 0.44
# Odometry covariances for the encoder output of the robot. These values should
# be tuned to your robot's sample odometry data, but these values are a good place
# to start
pose_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.03]
twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.03]
# Top level frame (link) of the robot description
base_frame_id: base_link
# Velocity and acceleration limits for the robot
linear:
x:
has_velocity_limits : true
max_velocity : 0.2 # m/s
has_acceleration_limits: true
max_acceleration : 0.6 # m/s^2
angular:
z:
has_velocity_limits : true
max_velocity : 2.0 # rad/s
has_acceleration_limits: true
max_acceleration : 6.0 # rad/s^2
DiffDriverController会订阅Twist cmd_vel类型的消息，然后移动机器人，如下面的代码所示：
roslaunch urdf_sim_tutorial 13-diffdrive.launch
8.创建自己的urdf文件
8.1我们想要建立一个自己的机器人，在文件my_robot.urdf粘贴如下代码：
之后运行以下命令来检查urdf文件语法是否正确来通过编译，首先安装独立于ROS的检查报urdfdom
$ sudo apt-get install liburdfdom-tools
之后开始检查：
$ rosmake urdfdom_model # only needed if installed from source
$ check_urdf my_robot.urdf # hydro and later
8.2 用标签来指定link坐标系之间的偏移量，如下面代码所示：
之后运行命令检查有无错误：
$ check_urdf my_robot.urdf
8.3 使用标签为Joint添加旋转轴
之后运行检查，并通过在rviz和pdf文件中查看坐标系和Joint的状态
$ check_urdf my_robot.urdf
$ urdf_to_graphiz my_robot.urdf
$ evince test_robot.pdf
- 编译urdf文件
首先创立工作空间
$ cd ~/catkin_ws/src
$ catkin_create_pkg testbot_description urdf
$ cd testbot_description
之后建立所需要的urdf文件夹以及src文件夹，如果有需要，还应该建立mesh,materials,cad文件夹
mkdir urdf
mkdir src
之后将8中创建的.urdf文件拷贝到urdf文件夹下面，并将如下代码粘贴到新建的src/parser.cpp文件中：
#include
#include "ros/ros.h"
int main(int argc, char** argv){
ros::init(argc, argv, "my_parser");
if (argc != 2){
ROS_ERROR("Need a urdf file as argument");
return -1;
}
std::string urdf_file = argv[1];
urdf::Model model;
if (!model.initFile(urdf_file)){
ROS_ERROR("Failed to parse urdf file");
return -1;
}
ROS_INFO("Successfully parsed urdf file");
return 0;
}
之后再CMakeLists.txt后面粘贴如下代码：
add_executable(parser src/parser.cpp)
target_link_libraries(parser ${catkin_LIBRARIES})
将package进行编译
$ cd ~/catkin_ws
$ catkin_make
$ ./parser my_robot.urdf
# Example: ./devel/lib/testbot_description/parser ./src/testbot_description/urdf/my_robot.urdf
- 从urdf文件创建KDL Tree
首先安装KDL parser
$ rosdep install kdl_parser
之后build这个package:
$ rosmake kdl_parser
之后将KDL设置为package.xml的依赖项：
...
在文件中添加include从而能够调用KDL：
#include
10.1 从file中创建KDL
KDL::Tree my_tree;
if (!kdl_parser::treeFromFile("filename", my_tree)){
ROS_ERROR("Failed to construct kdl tree");
return false;
}
之后完成pr2.urdf文件的创立：
$ rosrun xacro xacro.py `rospack find pr2_description`/robots/pr2.urdf.xacro > pr2.urdf
10.2 从parameter server创建
KDL::Tree my_tree;
ros::NodeHandle node;
std::string robot_desc_string;
node.param("robot_description", robot_desc_string, std::string());
if (!kdl_parser::treeFromString(robot_desc_string, my_tree)){
ROS_ERROR("Failed to construct kdl tree");
return false;
}
10.3从.xml文件中创立
KDL::Tree my_tree;
TiXmlDocument xml_doc;
xml_doc.Parse(xml_string.c_str());
xml_root = xml_doc.FirstChildElement("robot");
if (!xml_root){
ROS_ERROR("Failed to get robot from xml document");
return false;
}
if (!kdl_parser::treeFromXml(xml_root, my_tree)){
ROS_ERROR("Failed to construct kdl tree");
return false;
}
10.4 从urdf model创立
KDL::Tree my_tree;
urdf::Model my_model;
if (!my_model.initXml(....)){
ROS_ERROR("Failed to parse urdf robot model");
return false;
}
if (!kdl_parser::treeFromUrdfModel(my_model, my_tree)){
ROS_ERROR("Failed to construct kdl tree");
return false;
}
11.使用robot_state_publisher来向/tf广播所使用的urdf模型的状态
首先有一个机器人模型model.xml
然后创建如下的安装包，并把代码拷贝到src/state_publisher.cpp文件中：
cd %TOP_DIR_YOUR_CATKIN_WS%/src
catkin_create_pkg r2d2 roscpp rospy tf sensor_msgs std_msgs
#include
#include
#include
#include
int main(int argc, char** argv) {
ros::init(argc, argv, "state_publisher");
ros::NodeHandle n;
ros::Publisher joint_pub = n.advertise("joint_states", 1);
tf::TransformBroadcaster broadcaster;
ros::Rate loop_rate(30);
const double degree = M_PI/180;
// robot state
double tilt = 0, tinc = degree, swivel=0, angle=0, height=0, hinc=0.005;
// message declarations
geometry_msgs::TransformStamped odom_trans;
sensor_msgs::JointState joint_state;
odom_trans.header.frame_id = "odom";
odom_trans.child_frame_id = "axis";
while (ros::ok()) {
//update joint_state
joint_state.header.stamp = ros::Time::now();
joint_state.name.resize(3);
joint_state.position.resize(3);
joint_state.name[0] ="swivel";
joint_state.position[0] = swivel;
joint_state.name[1] ="tilt";
joint_state.position[1] = tilt;
joint_state.name[2] ="periscope";
joint_state.position[2] = height;
// update transform
// (moving in a circle with radius=2)
odom_trans.header.stamp = ros::Time::now();
odom_trans.transform.translation.x = cos(angle)*2;
odom_trans.transform.translation.y = sin(angle)*2;
odom_trans.transform.translation.z = .7;
odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(angle+M_PI/2);
//send the joint state and transform
joint_pub.publish(joint_state);
broadcaster.sendTransform(odom_trans);
// Create new robot state
tilt += tinc;
if (tilt<-.5 || tilt>0) tinc *= -1;
height += hinc;
if (height>.2 || height<0) hinc *= -1;
swivel += degree;
angle += degree/4;
// This will adjust as needed per iteration
loop_rate.sleep();
}
return 0;
}
之后建立display.launch文件准备广播
之后再CMakeLists.txt文件中粘贴以下代码：
find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs tf)
include_directories(include ${catkin_INCLUDE_DIRS})
add_executable(state_publisher src/state_publisher.cpp)
target_link_libraries(state_publisher ${catkin_LIBRARIES})
$ catkin_make
$ roslaunch r2d2 display.launch
$ rosrun rviz rviz
Choose odom as your fixed frame (under Global Options). Then choose “Add Display” and add a Robot Model Display and a TF Display