通过ROS控制真实机械臂(6)---自定义RViz插件panel界面设计

1. 自定义消息，用于发布每个关节的状态。

GoalPoint.msg

int8 bus
float32 joint1
float32 joint2
float32 joint3
float32 joint4
float32 joint5
float32 joint6

JointPos.msg

float32 joint1
float32 joint2
float32 joint3
float32 joint4
float32 joint5
float32 joint6

TraPoint.msg

int8 bus
int32 num_of_trapoint
float32[] trapoints

2.便于控制，通过Rviz的panel插件，制作一个控制的GUI界面，先在Qt Creater中完成界面设计, 通过调节Bus Interface的选项，可以选择按钮调节的速度，通过Mode选项，Continuous是即时发送数据，第二个用于购买的工业级机械臂的接口，第三个是最常用的通过Moveit规划的路径控制，通过调节每个Joint的数值为机械臂选取目标点，或者通过下面的五个预定义的位姿按钮，最后点击SendGoal控制机械臂的运动。最后一个是根据机械臂发布的关节状态信息，来改变Rviz和界面中的数据的。

接下来创建ROS包，这是通过Rviz插件的方法在ROS中编写GUI界面 ，最后编译生成的是.SO文件，没有启动的节点，只需要运行Rviz之后，调出panel插件即可。

redwall_arm_panel.h

#ifndef RedWallArm_PANEL_H
#define RedWallArm_PANEL_H

#include  <ros/ros.h>
#include  <rviz/panel.h>
#include  <sensor_msgs/JointState.h>
#include  <std_msgs/Float32MultiArray.h>
#include  <redwall_arm_msgs/GoalPoint.h>
#include  <QWidget>
#include  <QTimer>


namespace Ui {
    class RedWallArmPanel;
}


namespace redwall_arm_panel
{
    class RedWallArmPanel : public rviz::Panel
    {
        Q_OBJECT

    public:
        explicit RedWallArmPanel(QWidget *parent = 0);
        ~RedWallArmPanel();

    public:
        void initROS();

    private Q_SLOTS:
        void on_pbn_joint1Left_pressed();
        void on_pbn_joint1Right_pressed();
        void on_pbn_joint2Left_pressed();
        void on_pbn_joint2Right_pressed();
        void on_pbn_joint3Left_pressed();
        void on_pbn_joint3Right_pressed();
        void on_pbn_joint4Left_pressed();
        void on_pbn_joint4Right_pressed();
        void on_pbn_joint5Left_pressed();
        void on_pbn_joint5Right_pressed();
        void on_pbn_joint6Left_pressed();
        void on_pbn_joint6Right_pressed();

        void on_pbn_zero_clicked();
        void on_pbn_classicPos1_clicked();
        void on_pbn_classicPos2_clicked();
        void on_pbn_classicPos3_clicked();
        void on_pbn_classicPos4_clicked();
        void on_pbn_sendGoal_clicked();

        void on_rbx_pcan_clicked();
        void on_rbx_tcp_clicked();

        void on_rbx_continuous_clicked();
        void on_rbx_goal_clicked();
        void on_rbx_moveit_clicked();
        void on_rbx_sync_clicked();

        void on_pbn_setIO_clicked();

    protected Q_SLOTS:
        void sendCommand();

    protected:
        QTimer* m_timer;

        // The ROS publisher/subscriber
        ros::Publisher cmd1_publisher_;
        ros::Publisher cmd2_publisher_;
        ros::Publisher cmd3_publisher_;
        ros::Publisher goal_publisher_;

        sensor_msgs::JointState jointMsg;
        std_msgs::Float32MultiArray joints;
        redwall_arm_msgs::GoalPoint goalPoint;

        ros::Subscriber subJointState_;
        void jointStateCallback(const sensor_msgs::JointState::ConstPtr& msg);
        int pointCompare(void);

        // The ROS node handle.
        ros::NodeHandle nh_;

        float jointVelocities[6];
        float jointPosition[6];
        float lastJointPosition[6];

    private:
        Ui::RedWallArmPanel *ui;
        float m_step;
        float m_speed;

        //int m_jointStateSync;
        int m_controMode;
        int m_busInterface;
    };
}//end of redwall_arm_rviz_plugin

#endif // RedWallArm_PANEL_H

redwall_arm_panel.cpp

#include "redwall_arm_panel.h"
#include "ui_redwall_arm_panel.h"
#include <math.h>


namespace redwall_arm_panel
{

RedWallArmPanel::RedWallArmPanel(QWidget *parent) :
    rviz::Panel(parent),
    ui(new Ui::RedWallArmPanel),
    m_step(0.16),
    m_speed(50.0),
    m_controMode(3),
    m_busInterface(1)

{
    ui->setupUi(this);

    ui->rbx_pcan->setChecked(false);
    ui->rbx_tcp->setChecked(true);

    ui->rbx_continuous->setChecked(false);
    ui->rbx_goal->setChecked(false);
    ui->rbx_moveit->setChecked(false);
    ui->rbx_sync->setChecked(true);

    ui->pbn_setIO->setEnabled(false);
    ui->pbn_sendGoal->setEnabled(false);

    u