一、创建ros软件包
bash
#1、创建工作空间
mkdir 2d_robot_ws
cd 2d_robot_ws
mkdir src
cd src
catkin_init_workspace
#2、编译工作空间
cd ..
catkin_make
#3、创建软件包
catkin_create_pkg 2d_robot std_msgs rospy roscpp
二、创建模型文件
1、编写urdf模型文件
在2d_robot_ws/src/2d_robot下创建launch文件夹和urdf文件夹,launch文件夹中放启动文件,urdf文件夹中放模型文件。
2d_robot_model.urdf
xml
<?xml version="1.0"?>
<robot name="my_robot">
<link name="link_base"> 基座机构
<inertial>
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value="10"/>
<inertia ixx="100" ixy="0" ixz="0" iyy="100" izz="100" iyz="0"/>
</inertial>
<visual>
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<geometry>
<box size="5.0 5.0 1.0"/>
</geometry>
<material name="123">
<color rgba="0.0 1.0 1.0 1.0"/>
</material>
</visual>
<collision>
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<geometry>
<box size="5.0 5.0 1.0"/>
</geometry>
</collision>
</link>
<link name="link_one"> 第二个机构
<inertial>
<origin xyz="0.0 0.0 5.0" rpy="0.0 0.0 0.0"/>
<mass value="5"/>
<inertia ixx="100" ixy="0" ixz="0" iyy="100" izz="100" iyz="0"/>
</inertial>
<visual>
<origin xyz="0.0 0.0 5.0" rpy="0.0 0.0 0.0"/>
<geometry>
<box size="3.0 3.0 10.0"/>
</geometry>
<material name="234">
<color rgba="0.8 0.5 0 0.5"/>
</material>
</visual>
<collision>
<origin xyz="0.0 0.0 5.0" rpy="0.0 0.0 0.0"/>
<geometry>
<box size="1.0 1.0 10.0"/>
</geometry>
</collision>
</link>
<link name="link_two"> 第二个机构
<inertial>
<origin xyz="0.0 0.0 5.0" rpy="0.0 0.0 0.0"/>
<mass value="5"/>
<inertia ixx="100" ixy="0" ixz="0" iyy="100" izz="100" iyz="0"/>
</inertial>
<visual>
<origin xyz="0.0 0.0 5.0" rpy="0.0 0.0 0.0"/>
<geometry>
<box size="1.0 1.0 10.0"/>
</geometry>
<material name="234">
<color rgba="0.0 5.0 1.0 1.0"/>
</material>
</visual>
<collision>
<origin xyz="0.0 0.0 5.0" rpy="0.0 0.0 0.0"/>
<geometry>
<box size="1.0 1.0 10.0"/>
</geometry>
</collision>
</link>
<joint name="joint_one" type="revolute"> 第一个关节,连接基座和机构1
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<parent link="link_base"/>
<child link="link_one"/>
<axis xyz="1.0 0.0 0.0"/>
<limit lower="-3.0" upper="3.0" effort="10.0" velocity="3.0"/>
</joint>
<joint name="joint_two" type="revolute"> 第二个关节,连接机构1和机构2
<origin xyz="0.0 0.0 10.0" rpy="0.0 0.0 0.0"/>
<parent link="link_one"/>
<child link="link_two"/>
<axis xyz="1.0 0.0 0.0"/>
<limit lower="-3.0" upper="3.0" effort="10.0" velocity="3.0"/>
</joint>
</robot>
2、用rviz查看urdf模型
2d_robot_rviz.launch
xml
<?xml version="1.0"?>
<launch>
<arg name="model" />
<!-- 加载机器人模型参数 -->
<param name="robot_description" textfile="/media/wangyuanhao/DATA/ros_data/2d_robot_ws/src/2d_robot/urdf/2d_robot_model.urdf" />
<!-- 设置GUI参数,显示关节控制插件 -->
<param name="use_gui" value="true"/>
<!-- 运行joint_state_publisher节点,发布机器人的关节状态 -->
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
<!-- 运行robot_state_publisher节点,发布tf -->
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
<!-- 运行rviz可视化界面 -->
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find urdf_tutorial)/urdf.rviz" />
</launch>
roslaunch命令
bash
roslaunch 2d_robot 2d_robot_rviz.launch
启动rviz后添加机器人类型,并设置fixed frame
3、urdf转sdf
bash
gz sdf -p 2d_robot_model.urdf > 2d_robot_model.sdf
三、gazebo仿真
1、编写模型文件
要让sdf模型文件可以在world文件中引用需要sdf和config两个文件,新建一个文件夹命名为2d_robot,其中分别有model.sdf和model.config文件。
model.sdf就是一般的sdf文件,config文件如下:
model.config
xml
<?xml version="1.0"?>
<model>
<name>My Robot</name>
<version>1.0</version>
<sdf version='1.4'>model.sdf</sdf>
<author>
<name>My Name</name>
<email>me@my.email</email>
</author>
<description>
My awesome robot.
</description>
</model>
2、编写world文件(导入sdf模型)
2d_robot.world
xml
<?xml version="1.0" ?>
<sdf version="1.4">
<world name="default">
<include>
<uri>model://ground_plane</uri>
</include>
<include>
<uri>model://sun</uri>
</include>
<include>
<scale>1 1 1</scale>
<uri>///media/wangyuanhao/DATA/ros_data/2d_robot_ws/src/2d_robot/sdf/2d_robot</uri>
<name>2d_robot</name>
<pose>-2.0 7.0 0 0 0 0</pose>
</include>
</world>
</sdf>
2、将world文件导入gazebo(空白世界+world文件)
2d_robot_gazebo.launch
xml
<launch>
<!-- We resume the logic in empty_world.launch, changing only the name of the world to be launched -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" value="$(find 2d_robot)/worlds/2d_robot.world"/>
<!-- more default parameters can be changed here -->
</include>
</launch>
roslaunch启动命令
bash
roslaunch 2d_robot 2d_robot_gazebo.launch
3、world文件中加入gazebo插件
3.1添加世界插件hello_world
3.1.1编写插件
hello_world.cc
cpp
#include <gazebo/gazebo.hh>
namespace gazebo
{
class WorldPluginTutorial : public WorldPlugin
{
public: WorldPluginTutorial() : WorldPlugin()
{
printf("Hello World!\n");
}
public: void Load(physics::WorldPtr _world, sdf::ElementPtr _sdf)
{
}
};
GZ_REGISTER_WORLD_PLUGIN(WorldPluginTutorial)
}
3.1.2编译插件
CMakeLists.txt
xml
cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
find_package(gazebo REQUIRED)
include_directories(${GAZEBO_INCLUDE_DIRS})
link_directories(${GAZEBO_LIBRARY_DIRS})
list(APPEND CMAKE_CXX_FLAGS "${GAZEBO_CXX_FLAGS}")
add_library(hello_world SHARED hello_world.cc)
target_link_libraries(hello_world ${GAZEBO_LIBRARIES})
这里要注意一下不能使用boost1.69以上的库,所以要确认自己的版本。
添加so文件路径
我这里是把so文件放到了gazebo的插件路经的文件夹中,我电脑的路径是/usr/lib/x86_64-linux-gnu/gazebo-9/plugins。
3.1.3在world中引用
2d_robot.world
xml
<?xml version="1.0" ?>
<sdf version="1.4">
<world name="default">
<plugin name="hello_world" filename="libhello_world.so"/>
<include>
<uri>model://ground_plane</uri>
</include>
<include>
<uri>model://sun</uri>
</include>
<include>
<scale>1 1 1</scale>
<uri>///media/wangyuanhao/DATA/ros_data/2d_robot_ws/src/2d_robot/sdf/2d_robot</uri>
<name>2d_robot</name>
<pose>-2.0 7.0 0 0 0 0</pose>
</include>
</world>
</sdf>
最终效果:
3.2添加模型插件pos_update
https://blog.csdn.net/Robot_Starscream/article/details/120323907
https://zhuanlan.zhihu.com/p/363385163
3.2.1编写插件
这里主要就是借鉴了第一个链接里面的代码,只是可能因为版本不同,get_angle的函数没有了换成了GetForce。
back_wheel_steering.hh
cpp
#ifndef _BACK_WHEEL_ROLLINGANGLE_HH_
#define _BACK_WHEEL_ROLLINGANGLE_HH_
#include <boost/thread.hpp>
#include <cmath>
#include <fstream>
#include <iostream>
#include <string>
#include <thread>
// gazebo
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/gazebo_client.hh>
#include <gazebo/msgs/msgs.hh>
#include <gazebo/transport/transport.hh>
#include "std_msgs/Float64.h"
#include "ros/callback_queue.h"
#include "ros/ros.h"
#include "ros/subscribe_options.h"
#include "ros/subscription_callback_helper.h"
namespace gazebo{
class back_wheel_steering : public ModelPlugin{
public:
virtual ~back_wheel_steering();
back_wheel_steering();
void virtual Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);
void OnUpdate();
double B_wheel_steering;
private:
physics::ModelPtr model;
physics::JointPtr prismaticJoint;
event::ConnectionPtr BWP_updateConnection;
ros::NodeHandle P_joint_steering_Node;
ros::Publisher rosPub_P_joint_steering;
std::string P_joint_steering_PubTopic;
std_msgs::Float64 B_wheel_steering_msg;
int SimulateCount=0;
int PrintStep=1000;
};
}
#endif
back_wheel_steering.cc
cpp
#include "back_wheel_steering.hh"
#define IS_DEBUG 0
namespace gazebo
{
back_wheel_steering::back_wheel_steering(){
std::cout << "start1" << std::endl;
std::cout << "start2" << std::endl;
}
back_wheel_steering::~back_wheel_steering(){}
void back_wheel_steering::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf){
// Just output a message for now
//std::cerr << "\nThe velodyne plugin is attach to model[" << _model->GetName() << "]\n";
std::cout << "go in Load" << std::endl;
this->model = _model;
this->prismaticJoint = this->model->GetJoint(_sdf->GetElement("prismaticJoint")->Get<std::string>()); //_sdf是插件标签的指针,等于说获取了插件的参数
this->P_joint_steering_PubTopic = _sdf->GetElement("P_joint_position_Topic")->Get<std::string>();
rosPub_P_joint_steering = this->P_joint_steering_Node.advertise<std_msgs::Float64>(P_joint_steering_PubTopic, 10);
this->BWP_updateConnection = event::Events::ConnectWorldUpdateBegin(
std::bind(&back_wheel_steering::OnUpdate, this));
}
void back_wheel_steering::OnUpdate(){
B_wheel_steering = this->prismaticJoint->GetForce(0);
B_wheel_steering_msg.data = B_wheel_steering;
rosPub_P_joint_steering.publish(B_wheel_steering_msg);
SimulateCount++;
// std::cout << "++++++++++++++++++++++++++++++++" << std::endl;
if ((SimulateCount % PrintStep == 0)){
std::cout << "++++++++++++++++++++++++++++++++" << std::endl;
std::cout<< "back_wheel_steeringangle:" << B_wheel_steering<<std::endl;
std::cout << "++++++++++++++++++++++++++++++++" << std::endl;
}
}
GZ_REGISTER_MODEL_PLUGIN(back_wheel_steering)
}
3.2.2编译插件
因为代码中使用了ros的msg,所以引入了ros的一些依赖。
CMakeLists.txt
xml
cmake_minimum_required(VERSION 3.0.2)
project(gazebo_joint_plug_pkg)
find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
std_msgs
)
include_directories(
# include
${catkin_INCLUDE_DIRS}
)
find_package(gazebo REQUIRED)
include_directories(${GAZEBO_INCLUDE_DIRS})
link_directories(${GAZEBO_LIBRARY_DIRS})
list(APPEND CMAKE_CXX_FLAGS "${GAZEBO_CXX_FLAGS}")
add_library(joint_plug SHARED src/back_wheel_steering.cc)
target_link_libraries(joint_plug ${GAZEBO_LIBRARIES})
3.2.3在world中引用
注意这部分代码要插入到sdf模型文件中,joint_two是关节名,要和sdf文件中的一样。
xml
<plugin name="joint_two_control" filename="libjoint_two_control.so">
<prismaticJoint>joint_two</prismaticJoint>
<P_joint_position_Topic>/joint_two_pos</P_joint_position_Topic>
</plugin>
最终效果: