Moveit白泽四足机器人联合控制（GUI界面和实物同步）

目录

​​导航在这里：白泽四足机器人导航贴​​

​​1.导出moveit包​​

​​2.处理关节变量​​

​​3.arduino订阅并驱动舵机​​

​​ 4.运行测试​​

导航在这里：白泽四足机器人导航贴

与MoveIt!交互的三种方式：命令行、Rviz（GUI）、C++ API

1.导出moveit包

首先，我们需要通过moveit的GUI assistant来导出包。加载urdf模型进软件输出moveit处理后的包。一般moveit处理后的包名为“原始报名_moveit_config"。

2.处理关节变量

其次，在moveit运动规划的时候，会发布出/joint_states这个话题，这个话题包含了四足机器人的关节角度等信息，我们需要做的就是自己编写一个节点将这个话题订阅起来，然后把里面的数据进行处理之后再以自己的话题消息格式发布出来，然后让arduino等下位机通过话题消息订阅自定义的关节信息并控制舵机。

ROS节点程序：

#include "ros/ros.h"
#include "std_msgs/String.h"
#include "sensor_msgs/JointState.h"
#include "hello/arduino_8_dof_mpu_robot.h"

#define PI 3.1415
/**
 * This tutorial demonstrates simple receipt of messages over the ROS system.
 */



    //4.组织被发布的消息，编写发布逻辑并发布消息

int direct[8]={
  -1,1,1,-1,
  -1,1,1,-1
};

// int direct1[8]={
//   1,-1,-1,1,
//   -1,1,1,-1
// };

char jz[8]={-8,0,5,-3,0,-2,-9,5};

hello::arduino_8_dof_mpu_robot servo_msg;
 
void jointstatesCallback(const sensor_msgs::JointStateConstPtr& msg)
{
                 

                float pos[3],vel[3];
                // pos=msg.position;
                pos[0]=msg->position[0]*180/PI;
                pos[1]=msg->position[1]*180/PI;
                servo_msg.servo_angle8 = 90+jz[0]*direct[0]+(int(pos[0])*direct[0]);
                servo_msg.servo_angle9 =  90+jz[1]*direct[1]+(pos[0])*direct[1];
                servo_msg.servo_angle10 =  90+jz[2]*direct[2]+(int(pos[0]))*direct[2];
                servo_msg.servo_angle11 =  90+jz[3]*direct[3]+(int(pos[0])*direct[3]);
                servo_msg.servo_angle12 = 90+jz[4]*direct[4]+direct[4]*(int(pos[1]));
                servo_msg.servo_angle13 =  90+jz[5]*direct[5]+direct[5]*(int(pos[1]));
                servo_msg.servo_angle14 = 90+jz[6]*direct[6]+direct[6]*(int(pos[1]));
                servo_msg.servo_angle15 =  90+jz[7]*direct[7]+direct[7]*(int(pos[1]));
                ROS_INFO("I heard: [%f] [%f] [%f] [%f] [%f] [%f]",pos[0],pos[1],pos[2],vel[0],vel[1],vel[2]);
                 
                 ROS_INFO("------------------publish");
}
 
int main(int argc, char **argv)
{
 
  ros::init(argc, argv, "listener");
 
  ros::NodeHandle nh;

  ros::Publisher pub = nh.advertise<hello::arduino_8_dof_mpu_robot>("arduino_8_dof_mpu_robot",1000);

    ros::Subscriber sub = nh.subscribe("/joint_states", 1000, jointstatesCallback);

ros::Rate r(20);
 while(ros::ok())
 {
        
        
        
        pub.publish(servo_msg);
         ros::spinOnce();
         r.sleep();
 }
 
  return 0;
}

3.arduino订阅并驱动舵机

#include <ros.h>
#include <hello/arduino_8_dof_mpu_robot.h>
#include <Servo.h>
#include <avr/wdt.h>
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"

MPU6050 mpu; //实例化一个 MPU6050 对象，对象名称为 mpu
int16_t ax, ay, az, gx, gy, gz;

#define servo8_p 6
#define servo9_p 5
#define servo10_p 8
#define servo11_p 9
#define servo12_p 10
#define servo13_p 11
#define servo14_p 12
#define servo15_p 13

Servo servo8;
Servo servo9;
Servo servo10;
Servo servo11;
Servo servo12;
Servo servo13;
Servo servo14;
Servo servo15;


ros::NodeHandle  nh;

hello::arduino_8_dof_mpu_robot servo_msg;
hello::arduino_8_dof_mpu_robot receive_msg;
ros::Publisher pub("receive", &receive_msg);

void callBack( const hello::arduino_8_dof_mpu_robot& servo_msg){
  servo8.write(servo_msg.servo_angle8);
  servo9.write(servo_msg.servo_angle9);
  servo10.write(servo_msg.servo_angle10);
  servo11.write(servo_msg.servo_angle11);
  servo12.write(servo_msg.servo_angle12);
  servo13.write(servo_msg.servo_angle13);
  servo14.write(servo_msg.servo_angle14);
  servo15.write(servo_msg.servo_angle15);

  receive_msg.servo_angle8=servo_msg.servo_angle8;
  receive_msg.servo_angle9=servo_msg.servo_angle9;
  receive_msg.servo_angle10=servo_msg.servo_angle10;
  receive_msg.servo_angle11=servo_msg.servo_angle11;
  receive_msg.servo_angle12=servo_msg.servo_angle12;
  receive_msg.servo_angle13=servo_msg.servo_angle13;
  receive_msg.servo_angle14=servo_msg.servo_angle14;
  receive_msg.servo_angle15=servo_msg.servo_angle15;
  
  delay(10);
  nh.spinOnce();
}


ros::Subscriber<hello::arduino_8_dof_mpu_robot> sub("arduino_8_dof_mpu_robot", &callBack);

void setup() {
  // put your setup code here, to run once:
  pinMode(servo8_p,OUTPUT);
  pinMode(servo9_p,OUTPUT);
  pinMode(servo10_p,OUTPUT);
  pinMode(servo11_p,OUTPUT);
  pinMode(servo12_p,OUTPUT);
  pinMode(servo13_p,OUTPUT);
  pinMode(servo14_p,OUTPUT);
  pinMode(servo15_p,OUTPUT);

  servo8.attach(servo8_p);
  servo9.attach(servo9_p);
  servo10.attach(servo10_p);
  servo11.attach(servo11_p);
  servo12.attach(servo12_p);
  servo13.attach(servo13_p);
  servo14.attach(servo14_p);
  servo15.attach(servo15_p);
  
  for(int i=60;i<=120;i++)
  {
    servo8.write(i);
    servo12.write(i);
    delay(15);
  }
  for(int i=120;i>=60;i--)
  {
    servo8.write(i);
    servo12.write(i);
    delay(15);
  }
  servo8.write(90);
  servo9.write(90);
  servo10.write(90);
  servo11.write(90);
  servo12.write(90);
  servo13.write(90);
  servo14.write(90);
  servo15.write(90);
  
  nh.initNode();
  nh.advertise(pub);
  nh.subscribe(sub);

//  Serial.println("Ultrasonic and dianweiji: ");
  Wire.begin();                            //加入 I2C 总线序列
  Wire.setClock(400000);
//  Serial.begin(57600);                       //开启串口，设置波特率
  delay(1000);
  mpu.initialize();                       //初始化MPU6050
  wdt_enable(WDTO_60MS); //开启看门狗，并设置溢出时间为60毫秒

}

void loop() {
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);     //IIC获取MPU6050六轴数据 ax ay az gx gy gz  

  receive_msg.ax=ax;
  receive_msg.ay=ay;
  receive_msg.az=az;
  receive_msg.gx=gx;
  receive_msg.gy=gy;
  receive_msg.gz=gz;
  
  Serial.print(ax);Serial.print(",");
  Serial.print(ay);Serial.print(",");
  Serial.print(az);Serial.print(",");
  Serial.print("---------------");Serial.println(millis());
  pub.publish(&receive_msg);
  delay(5);
  nh.spinOnce();
  wdt_reset(); //喂狗操作，使看门狗定时器复位
}

 4.运行测试

（1）将moveit功能包下面的demo.launch启动文件运行起来。

（2）运行上位机自己编写的节点程序。

（3）将arduino下位机节点程序烧录到arduino板子。

（4）运行rosserial_python功能包下面的serial_node.py程序将arduino作为节点接入ros master。

可以再moveit图形化界面里面进行路径规划并执行操作了。