通过ROS控制仿真机械臂\真实机械臂(1)
先看补充
配置步骤
- 配置Movelt! + Gazebo (Gazebo仿真,这步配置好的可以直接跳到控制真实机器人部分)
- 控制真实机器人
- 通过串口发送数据到STM32(后面填坑)
在这篇文章中,将会介绍ROS相关配置,实现控制系统在控制真实机械臂与控制仿真机械臂之间的切换;
Gazebo仿真
用于验证机器人相关配置,可以减少控制真实机器人的配置过程中出现的问题;
Urdf/Xacro
这部分的教程网上已经很多了所以不再赘述,只强调几个影响后续机器人控制的关键点:
-
ROS控制器
这里的机器人工作空间(robotNamespace
)“br_robot” 与 机器人名字(robot name
) 一致即可,后续ROS的话题配置也要用到这个工作空间的命名;
-
连杆配置
如果不涉及机器人力控的话,应将机器人的质量设置为一个较小的值,可以解决在Gazebo仿真时,机器人站不稳、地面爬行的现象;
-
关节配置
机器人D-H表验证无误(可以通过Matlab进行验证!)后,却在Rviz中出现机器人关节正反运动方向错误的情况。这个问题可以在关节部分的配置中解决;
利用当前关节坐标系的x、y、z轴分量确定旋转轴位置以及运动方向。一般与D-H表建立的Z轴一致,可以根据实际情况调整。且遵守右手定则;
Movelt!
Moelt配置没有强调的地方,除了在配置过程中与古月居有一处参数不一致。这里按照古月居的参数填会导致Movelt!配置失败!需要注意一下;
Movelt! + Gazebo
构建Movelt! + Gazebo仿真,包括三个步骤:
1、配置 Joint Trajectory Controller
机器人侧的控制器接口,用于接收action消息;
-
参数配置(.yaml文件)
-
控制器启动文件(.launch文件)
要注意“.yaml”文件与“.launch”文件的对应,图中相同颜色框的内容一致、启动文件.launch中的rosparam
文件名也要与“.yaml”文件名对应;
2、配置 Joint State Controller
机器人侧的控制器接口,用于发布机器人状态;
-
参数配置(.yaml文件)
-
控制器启动文件(.launch文件)
与配置 Joint Trajectory Controller 过程相似;
3、配置 Follow Joint Trajectory
ROS侧的控制器接口,用于发布action消息;
-
参数配置(.yaml文件)
需要注意的是,这里的"name
"是第一步中两种参数:ns以及args 的组合(意思是:br_robot空间下的arm_joint_controller); -
控制器启动文件(.launch文件)
要注意以上三个步骤中,对".launch"文件中的"机器人工作空间(robotNamespace)“以及对应”.yaml文件路径 "的修改,robotNamespace
要与前面Urdf/Xacro中的一致;
最后
使用一个.launch文件启动上述三个ROS节点以及一个空白环境节点;
启动节点进行Gazebo仿真
$ roslaunch gazebo_src rbd_moveit.launch
文件树展示
红色方框:启动所有节点的".launch"文件;
Gazebo仿真验证无误后,进行下一步——真实机器人控制(重点);
控制真实机器
控制真实机器人原理
如上图所示,通过上面的操作,我们通过话题A(即"br_robot/arm_joint_controller
")建立了Ros与Gazebo间的通信;
因此,要实现Ros控制真实机器人,要新建立一个发布action的话题并且将Ros话题名称与真实机器人统一;
(其实这里说真实机器人并不准确,因为Ros并不直接与机器人通信,只是为了方便大家理解)
控制真实机器人实现(ROS侧)
ROS的Gazebo仿真无误后可以尝试控制真实机械臂;这一步的关键是修改机器人侧的控制器接口配置,修改话题名称;
具体而言:
修改 Follow Joint Trajectory
- 参数配置(.yaml文件)
- 控制器启动文件(.launch文件)
为了区分Gazebo与真实机器人,方便后续控制对象在真实机器人与仿真机器人之间切换,将话题名称改为"br_robot";
同时也需要修改".launch文件"中的"rosparam file",修改为刚刚建立的".yaml文件"路径;
修改 demo.launch文件
一共修改两处:
-
修改参数"
fake_execution
"
-
删除 “fake joint states”
这段代码的作用是发布仿真机器人的关节状态,实现仿真机器人的状态反馈,这就是为什么在Gazebo仿真中Rviz可以读取机器人状态的原因;在我们控制真实机器人时,也需要自己编写代码发布机器人状态,这段代码不删除会导致两种消息产生冲突 ;
修改move.group文件
在控制真实机器人时,可能会出现执行超时的问题,解决方法如下:将"trajectory_execution/execution_duration_monitoring
"的值修改为"false";
控制真实机器人实现(机器人侧)
完成ROS侧的配置修改后,还需要对机器人侧进行配置:建立话题通信,接收ROS发布的action话题消息;
//这里是cpp实现
#include <ros/ros.h>
#include <iostream>
#include <actionlib/server/simple_action_server.h>
#include <control_msgs/FollowJointTrajectoryAction.h>
#include <std_msgs/Float32MultiArray.h>
#include <sstream>
#include <stdlib.h>
#include <sys/types.h>
#include <stdio.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <string.h>
#include <signal.h>
#include <arpa/inet.h>
#include <unistd.h>
#include <errno.h>
#include <pthread.h>
using namespace std;
typedef actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction> Server;
vector<vector<double>> _Pos_all;
void _cout_vec();
std::basic_string<char> Link1;
std::basic_string<char> Link2;
std::basic_string<char> Link3;
std::basic_string<char> Link4;
std::basic_string<char> Link5;
std::basic_string<char> Link6;
/* 收到action的goal后调用的回调函数 */
void execute(const control_msgs::FollowJointTrajectoryGoalConstPtr& goal, Server* as)
{
vector<double> row(6);
// ros::Rate rate(1);
ROS_INFO("receive start!");
Link1 = goal->trajectory.joint_names[0];
Link2 = goal->trajectory.joint_names[1];
Link3 = goal->trajectory.joint_names[2];
Link4 = goal->trajectory.joint_names[3];
Link5 = goal->trajectory.joint_names[4];
Link6 = goal->trajectory.joint_names[5];
for (int j = 0; j <goal->trajectory.points.size();j++)
{
for (int i = 0; i < 6; i++)
{
row[i] = goal->trajectory.points[j].positions[i];
}
_Pos_all.push_back(row);
}
as->setSucceeded();
_cout_vec();
}
/* 主函数主要用于动作订阅和套接字通信 */
int main(int argc, char** argv)
{
ros::init(argc, argv, "real_robot_server");
//real_robot_server:节点名称,任意
ros::NodeHandle nh;
// 定义一个服务器
Server server(nh, "br_robot/follow_joint_trajectory", boost::bind(&execute, _1, &server), false);
//话题名称:br_robot/follow_joint_trajectory;br_robot要对应ROS侧的话题名称;
// 服务器开始运行
server.start();
ROS_INFO("server start!");
ros::spin();
}
void _cout_vec()
{
for (int i = 0; i < _Pos_all.size(); i++)
{
printf("goal[%d]=[%f,%f,%f,%f,%f,%f]\n",i,_Pos_all[i][0],_Pos_all[i][1],_Pos_all[i][2],_Pos_all[i][3],_Pos_all[i][4],_Pos_all[i][5]);
}
}
如果通信建立成果,则会在终端打印action数据;也可以通过rostopic
查看是否成功建立通信;
实验
启动.launch文件后,再启动节点;
$ rosrun msgs_cpp server_demo
$ roslaunch gazebo_src rbd_moveit.launch
在Rviz中,点击plan and execute
,看到终端窗口有信息打印出来;
运行 $ rostopic list
此时,已经出现了/br_robot/follow_joint_trajectory
消息;
写在最后
如果看到这里,那么首先恭喜你离控制真实机器人的目标更近一步了;在下一篇文章中,我将解决剩余的问题:
/br_robot/follow_joint_trajectory/goal
的数据发送至单片机- 向Ros反馈真实机器人当前关节状态
问题2是Ros感知真实机器人状态的问题;否则Ros无法感知真实机器人的状态,每次规划都要从机器人初始位置开始,无法实现连续规划。
补充:
这个系列中不涉及到机器人的底层控制;但是这个系列的学习与机器人底层开发并无先后顺序,本系列到将路径点数据传递到单片机为止;
补充2:
最终可以实现更改几个参数就实现控制对象切换的效果;各位务必梳理清思路;