文章目錄

MoveIt! Setup Assistant 配置完的文件修改，參考：傳送門

1. 瞭解整體概況

Driver：包括Action Server和Joint_state publisher，接收Moveit下發的數據，反饋機器人狀態
Moveit：ActionClient與Driver的Action Server對接，Joint_state subscriber接收Joint_state publisher反饋信息，MoveIT function負責任務操作

爲了實現客戶端和服務器進行通信，ROS定義了一些標準的通信消息，用於規範兩者的工作，包括目標(Goal)，反饋(Feedback)和結果(Result)。

Goal：目標是由ActionClient發送到ActionServer的一種通訊消息，其包含的信息爲控制對象需要完成的動作
Feedback：反饋的作用在於提供給ActionServer一種用於傳遞任務執行狀況給ActionClient的方法
Result：在完成任務的執行後，ActionServer會發送將結果信息到ActionClient，其與反饋不同的地方在於，在整個任務的執行過程中只發送一次

   1 #include <ros/ros.h>
   2 #include <actionlib/server/simple_action_server.h>
   3 #include <actionlib_tutorials/FibonacciAction.h>
   4 
   5 class FibonacciAction
   6 {
   7 protected:
   8 
   9   ros::NodeHandle nh_;
  10   actionlib::SimpleActionServer<actionlib_tutorials::FibonacciAction> as_; // NodeHandle instance must be created before this line. Otherwise strange error occurs.
  11   std::string action_name_;
  12   // create messages that are used to published feedback/result
  13   actionlib_tutorials::FibonacciFeedback feedback_;
  14   actionlib_tutorials::FibonacciResult result_;
  15 
  16 public:
  17 
  18   FibonacciAction(std::string name) :
  19     as_(nh_, name, boost::bind(&FibonacciAction::executeCB, this, _1), false),
  20     action_name_(name)
  21   {
  22     as_.start();
  23   }
  24 
  25   ~FibonacciAction(void)
  26   {
  27   }
  28 
  29   void executeCB(const actionlib_tutorials::FibonacciGoalConstPtr &goal)
  30   {
  31     // helper variables
  32     ros::Rate r(1);
  33     bool success = true;
  34 
  35     // push_back the seeds for the fibonacci sequence
  36     feedback_.sequence.clear();
  37     feedback_.sequence.push_back(0);
  38     feedback_.sequence.push_back(1);
  39 
  40     // publish info to the console for the user
  41     ROS_INFO("%s: Executing, creating fibonacci sequence of order %i with seeds %i, %i", action_name_.c_str(), goal->order, feedback_.sequence[0], feedback_.sequence[1]);
  42 
  43     // start executing the action
  44     for(int i=1; i<=goal->order; i++)
  45     {
  46       // check that preempt has not been requested by the client
  47       if (as_.isPreemptRequested() || !ros::ok())
  48       {
  49         ROS_INFO("%s: Preempted", action_name_.c_str());
  50         // set the action state to preempted
  51         as_.setPreempted();
  52         success = false;
  53         break;
  54       }
  55       feedback_.sequence.push_back(feedback_.sequence[i] + feedback_.sequence[i-1]);
  56       // publish the feedback
  57       as_.publishFeedback(feedback_);
  58       // this sleep is not necessary, the sequence is computed at 1 Hz for demonstration purposes
  59       r.sleep();
  60     }
  61 
  62     if(success)
  63     {
  64       result_.sequence = feedback_.sequence;
  65       ROS_INFO("%s: Succeeded", action_name_.c_str());
  66       // set the action state to succeeded
  67       as_.setSucceeded(result_);
  68     }
  69   }
  70 
  71 
  72 };
  73 
  74 
  75 int main(int argc, char** argv)
  76 {
  77   ros::init(argc, argv, "fibonacci");
  78 
  79   FibonacciAction fibonacci("fibonacci");
  80   ros::spin();
  81 
  82   return 0;
  83 }

前提：

底層驅動已完成
機械臂可正常執行

假設：

添加路徑點函數：add_wayPoint()
按軌跡移動函數：trackMove()

改造驅動：

使用ROS封裝驅動

加入action服務端代碼

在CMakeLists.exe和package.xml中添加相關以來

find_package(catkin REQUIRED COMPONENTS

  actionlib    
 #action的本質是基於消息的，因此，還需要添加moveit的相關消息
moveit_msgs
)

catkin_package(
  INCLUDE_DIRS include

  CATKIN_DEPENDS   actionlib  moveit_msgs
)

<build_depend>actionlib</build_depend>
<build_depend>moveit_msgs</build_depend>

<exec_depend>actionlib</exec_depend>
<exec_depend>moveit_msgs</exec_depend>

添加action server頭文件

//action服務端的相關定義，請加入到驅動節點的頭文件中
#include "actionlib/server/action_server.h"    

//action服務端的目標控制句柄定義，與接收的目標相關聯後，可以用來實現action的信息反饋等操作       
#include "actionlib/server/server_goal_handle.h"

定義server對象

//定義action服務端
actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>  as_;    

//定義action服務端目標控制句柄
actionlib::ServerGoalHandle<control_msgs::FollowJointTrajectoryAction> goal_handle_; 

 //用來反饋action目標的執行情況，客戶端由此可以得知服務端是否執行成功了
control_msgs::FollowJointTrajectoryResult result_;

初始化服務端對象

LwrDriver::LwrDriver():
  as_(nh, "lwr/follow_joint_trajectory",
      boost::bind(&LwrDriver::goalCB, this, _1),
      false)
{
  as_.start();       //action服務端啓動

}
// nh：action所在ros節點的節點句柄
// controllers.yaml中的name和action_ns共同決定了客戶端action的名字
// boost::bind(&LwrDriver::goalCB, this,_1)固定用法，不再深究，綁定了LwrDriver::goalCB函數，goalCB()是LwrDriver類成員函數，此處表示其是一個接受不action目標的回調函數。當action客戶端請求一個動作時，這個動作被認爲是一個目標，傳遞給action服務端，此時就moveit的action客戶端而言，這個目標就是機械臂運動軌跡，服務端接收到目標後，會自動回調goalCB這個函數，而目標（軌跡）會作爲參數傳遞進來。

回調函數goalCB()

//轉自：https://blog.csdn.net/lingchen2348/article/details/80379166
void LwrDriver::goalCB( actionlib::ServerGoalHandle<control_msgs::FollowJointTrajectoryAction> gh)
{

// 複製一個副本，防止破壞原數據
  actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal = *gh.getGoal();    //make a copy that we can modify

 //將之前定義的句柄與傳入的action目標綁定在一起，
 goal_handle_ = gh;   


//提取正確數據，重新進行映射
  reorder_traj_joints(goal.trajectory);

	//添加路點函數
  Add_waypoint(goal.trajectory);


	 //執行軌跡函數
	if（TrackMove()） //假設軌跡成功後返回值爲1，失敗時返回值爲0
	{
	 //告訴客戶端成功了
	  goal_handle_.setAccepted();    
	  result_.error_code = result_.SUCCESSFUL;
	
	  goal_handle_.setSucceeded(result_);
	}

}

2.實例

直接轉我偉哥的，他的就是我的/手動滑稽

框架

MyRobot類：

/*
1.主要實現使控制器管理器controller manager（以及控制器管理器內的控制器）能夠訪問機器人的關節狀態以及機器人的命令；當控制器管理器運行時，控制器將讀取機器人中的pos，vel和eff變量，並將所需的命令寫入cmd變量。
2.確保pos，vel和eff變量始終具有最新的關節狀態joint states是你的工作，你還需要確保寫入cmd變量的內容由機器人執行。 爲此，你可以向機器人類添加read()和write()函數
*/
#include <hardware_interface/joint_command_interface.h>
#include <hardware_interface/joint_state_interface.h>
#include <hardware_interface/robot_hw.h>

class MyRobot : public hardware_interface::RobotHW//繼承hardware_interface::RobotHW類
{
public:
  MyRobot() 
 { 
   // connect and register the joint state interface
   hardware_interface::JointStateHandle state_handle_a("A", &pos[0], &vel[0], &eff[0]);
   jnt_state_interface.registerHandle(state_handle_a);//讀狀態

   hardware_interface::JointStateHandle state_handle_b("B", &pos[1], &vel[1], &eff[1]);
   jnt_state_interface.registerHandle(state_handle_b);

   registerInterface(&jnt_state_interface);

   // connect and register the joint position interface
   hardware_interface::JointHandle pos_handle_a(jnt_state_interface.getHandle("A"), &cmd[0]);//寫命令
   jnt_pos_interface.registerHandle(pos_handle_a);

   hardware_interface::JointHandle pos_handle_b(jnt_state_interface.getHandle("B"), &cmd[1]);
   jnt_pos_interface.registerHandle(pos_handle_b);

   registerInterface(&jnt_pos_interface);
  }

private:
  hardware_interface::JointStateInterface jnt_state_interface;
  hardware_interface::PositionJointInterface jnt_pos_interface;
  double cmd[2];
  double pos[2];
  double vel[2];
  double eff[2];
};

main函數

main()
{
  MyRobot robot;
  controller_manager::ControllerManager cm(&robot);

  while (true)
  {
     robot.read();
     cm.update(robot.get_time(), robot.get_period());
     robot.write();
     sleep();
  }
}

controllers.yaml

controller_list:
  - name: /dh_arm_controller 
    action_ns: "follow_joint_trajectory"
    type: FollowJointTrajectory
    default: true
    joints:
      - arm_joint_1
      - arm_joint_2
      - arm_joint_3
      - arm_joint_4


joints:
      - arm_joint_1
      - arm_joint_2
      - arm_joint_3
      - arm_joint_4

實例1

文件級：
├── CMakeLists.txt
├── config
│ └── dh_arm_control.yaml
├── include
│ └── dhrobot_controller
│ └── arm_hardware_interface.h
├── launch
│ └── arm_ros_control.launch
├── package.xml
└── src
└── arm_hardware_interface.cpp
dh_arm_control.yaml

controller_list:
  - name: /dh_arm_controller 
    action_ns: "follow_joint_trajectory"
    type: FollowJointTrajectory
    default: true
    joints:
      - arm_joint_1
      - arm_joint_2
      - arm_joint_3
      - arm_joint_4


joints:
      - arm_joint_1
      - arm_joint_2
      - arm_joint_3
      - arm_joint_4

arm_hardware_interface.h



#ifndef ARM_HW_INTERFACE
#define ARM_HW_INTERFACE

#include <ros/ros.h>
#include <urdf/model.h>
#include <pthread.h>
#include <time.h>

#include <boost/shared_ptr.hpp>
#include <boost/thread.hpp>
#include <boost/thread/mutex.hpp>

#include <std_msgs/Float64.h>
#include <sensor_msgs/JointState.h>
#include <dynamixel_msgs/JointState.h>

#include <hardware_interface/joint_command_interface.h>
#include <hardware_interface/joint_state_interface.h>
#include <hardware_interface/robot_hw.h>

#include <controller_manager/controller_manager.h>
#include <controller_manager_msgs/ListControllers.h>

class ArmHWInterface : public hardware_interface::RobotHW
{
public:
    ArmHWInterface();
    void read(const dynamixel_msgs::JointStateConstPtr &msg);
    void publishCmd();
    ros::NodeHandle getnode();

private:
    hardware_interface::JointStateInterface jnt_state_interface;
    hardware_interface::PositionJointInterface jnt_cmd_interface;
    double cmd[5];
    double pos[5];
    double vel[5];
    double eff[5];

    controller_manager_msgs::ListControllersRequest list_req;
    controller_manager_msgs::ListControllersResponse list_resp;

    bool loaded_flag;

    ros::NodeHandle _n;
    ros::Publisher pub_cmd[5];
    std_msgs::Float64 cmd_msg[5];
    ros::Time start_time_;
    ros::Duration start_dur_;

    ros::Subscriber sub_js[5];
    ros::ServiceClient client_controller_list;

};

#endif

arm_hardware_interface.cpp



#include "dhrobot_controller/arm_hardware_interface.h"

boost::mutex Arm_io_mutex;

ArmHWInterface::ArmHWInterface()
{
    for(int i=0; i<4; i++)
    {
        std::string joint_cmd_name="arm_joint_";
        std::string joint_state_name="arm_joint_";
        std::string joint_num=boost::lexical_cast<std::string>(i+1);
        joint_cmd_name.append(joint_num);
        joint_state_name.append(joint_num);
        joint_cmd_name.append("_controller/command");
        joint_state_name.append("_controller/state");
        pub_cmd[i]=_n.advertise<std_msgs::Float64>(joint_cmd_name, 1);
        sub_js[i]=_n.subscribe(joint_state_name, 1, &ArmHWInterface::read, this);
        client_controller_list=_n.serviceClient<controller_manager_msgs::ListControllers>("/controller_manager/list_controllers");
        loaded_flag=false;
    }

    for(int i=0; i<4; i++)
    {
        std::string joint_name="arm_joint_";
        std::string joint_num=boost::lexical_cast<std::string>(i+1);
        joint_name.append(joint_num);
        hardware_interface::JointStateHandle jnt_state_handle_tmp(joint_name, &pos[i], &vel[i], &eff[i]);
        jnt_state_interface.registerHandle(jnt_state_handle_tmp);
    }

    registerInterface(&jnt_state_interface);

    for(int i=0; i<4; i++)
    {
        std::string joint_name="arm_joint_";
        std::string joint_num=boost::lexical_cast<std::string>(i+1);
        joint_name.append(joint_num);
        hardware_interface::JointHandle jnt_handle_tmp(jnt_state_interface.getHandle(joint_name), &cmd[i]);
        jnt_cmd_interface.registerHandle(jnt_handle_tmp);
    }

    registerInterface(&jnt_cmd_interface);

    start_time_=ros::Time::now();
    start_dur_.operator +=(ros::Duration(3));
}

void ArmHWInterface::publishCmd()
{
    if(ros::Time::now()-start_time_<start_dur_)
        return;
    for(int i=0; i<4; i++)
    {
        cmd_msg[i].data=cmd[i];
        pub_cmd[i].publish(cmd_msg[i]);
    }
}

void ArmHWInterface::read(const dynamixel_msgs::JointStateConstPtr &msg)
{
    if(msg->motor_ids.size()<=0)
    {
        return;
    }
    if(msg->motor_ids[0]>6 || msg->motor_ids[0]<0)
    {
        return;
    }
    int msg_num=msg->motor_ids[0];
    double bm=msg->current_pos-pos[msg_num];
    boost::mutex::scoped_lock lock(Arm_io_mutex);
    pos[msg_num]=msg->current_pos;
    if(ros::Time::now()-start_time_>start_dur_)
    {
        if(bm>=0)
            vel[msg_num]=msg->velocity;
        else
            vel[msg_num]=-1*msg->velocity;
    }
    else
        vel[msg_num]=0;

    if(fabs(vel[msg_num])<1.2)
        vel[msg_num]=0;

    eff[msg_num]=msg->load;
}

ros::NodeHandle ArmHWInterface::getnode()
{
    return _n;
}

static void timespecInc(struct timespec &tick, int nsec)
{
  int SEC_2_NSEC = 1e+9;
  tick.tv_nsec += nsec;
  while (tick.tv_nsec >= SEC_2_NSEC)
  {
    tick.tv_nsec -= SEC_2_NSEC;
    ++tick.tv_sec;
  }
}

void* update_loop(void* threadarg)
{
    controller_manager::ControllerManager *c=(controller_manager::ControllerManager *)threadarg;
    ros::Rate r(50);
    ros::Duration d(0.02);

    while(ros::ok())
    {
        boost::mutex::scoped_lock lock(Arm_io_mutex);
        c->update(ros::Time::now(), d);
        lock.unlock();
        r.sleep();
    }
}

int main(int argc, char** argv)
{
    ros::init(argc,argv,"Dhrobot Arm_hardware_interface", ros::init_options::AnonymousName);
    ArmHWInterface c1;


    controller_manager::ControllerManager cm(&c1);
    pthread_t tid;
    pthread_create(&tid, NULL, update_loop, (void *)&cm);


    ROS_INFO("Arm bring up successfully");
    // loop
    ros::Rate r(50);
    while(ros::ok())
    {
        boost::mutex::scoped_lock lock(Arm_io_mutex);
        c1.publishCmd();
        lock.unlock();
        ros::spinOnce();
        r.sleep();
    }
    return 0;
}

CMakeLists.txt

cmake_minimum_required(VERSION 2.8.3)
project(dhrobot_controller)


find_package(catkin REQUIRED COMPONENTS
  actionlib
  urdf
  cmake_modules
  control_msgs
  control_toolbox
  controller_manager
  hardware_interface
  joint_limits_interface
  roscpp
  sensor_msgs
  std_msgs
  trajectory_msgs
  transmission_interface
)

catkin_package(


)

include_directories(
  include/
  ${catkin_INCLUDE_DIRS}
)

add_executable(arm_hardware_interface src/arm_hardware_interface.cpp)
target_link_libraries(arm_hardware_interface ${catkin_LIBRARIES})

package.xml

<?xml version="1.0"?>
<package>
  <name>dhrobot_controller</name>
  <version>1.0.0</version>
  <description>The dhrobot_controller package</description>


  <maintainer email="[email protected]">wxw</maintainer>
  <license>BSD</license>



  <buildtool_depend>catkin</buildtool_depend>
  <build_depend>actionlib</build_depend>
  <build_depend>urdf</build_depend>
  <build_depend>cmake_modules</build_depend>
  <build_depend>control_msgs</build_depend>
  <build_depend>control_toolbox</build_depend>
  <build_depend>controller_manager</build_depend>
  <build_depend>hardware_interface</build_depend>
  <build_depend>joint_limits_interface</build_depend>
  <build_depend>roscpp</build_depend>
  <build_depend>sensor_msgs</build_depend>
  <build_depend>std_msgs</build_depend>
  <build_depend>trajectory_msgs</build_depend>
  <build_depend>transmission_interface</build_depend>
  <run_depend>actionlib</run_depend>
  <run_depend>urdf</run_depend>
  <run_depend>cmake_modules</run_depend>
  <run_depend>control_msgs</run_depend>
  <run_depend>control_toolbox</run_depend>
  <run_depend>controller_manager</run_depend>
  <run_depend>hardware_interface</run_depend>
  <run_depend>joint_limits_interface</run_depend>
  <run_depend>roscpp</run_depend>
  <run_depend>sensor_msgs</run_depend>
  <run_depend>std_msgs</run_depend>
  <run_depend>trajectory_msgs</run_depend>
  <run_depend>transmission_interface</run_depend>


  <!-- The export tag contains other, unspecified, tags -->
  <export>
    <!-- Other tools can request additional information be placed here -->

  </export>
</package>

arm_ros_control.launch

<launch>    
<param name="robot_description" command="$(find xacro)/xacro --inorder '$(find dhrobot_description)/urdf/dhrobot.urdf.xacro'" />

  <node name="arm_hardware_interface" pkg="dhrobot_controller" type="arm_hardware_interface" output="screen"/>
  
  <rosparam file="$(find dhrobot_controller)/config/dh_arm_control.yaml" command="load"/>
  <node name="dh_arm_controller_spawner" pkg="controller_manager" type="controller_manager" args="spawn dh_arm_controller" respawn="false" output="screen"/>
</launch>

實例2

server端：

#include <ros/ros.h>
#include <actionlib/server/simple_action_server.h>
#include <control_msgs/FollowJointTrajectoryActionGoal.h>
#include <control_msgs/FollowJointTrajectoryActionResult.h>
#include <control_msgs/FollowJointTrajectoryAction.h>
#include <iostream>
#include <string.h>
#include <stdio.h>
#include <vector>
 
using namespace std;
 
class FollowJointTrajectoryAction
{
public:
 
 FollowJointTrajectoryAction(std::string name) :
   as_(nh_, name, false),
   action_name_(name)
 {
    as_.registerGoalCallback(boost::bind(&FollowJointTrajectoryAction::goalCB, this));
    as_.registerPreemptCallback(boost::bind(&FollowJointTrajectoryAction::preemptCB, this));
    as_.start();
    ROS_INFO("action start");
 }
   
 ~FollowJointTrajectoryAction(void)
  {
  }
  
 void goalCB()
 {
     ROS_INFO("goal is receive"); 
    if(as_.isNewGoalAvailable())
     {
	  
     }
    else
     {
       ROS_INFO("goal is not available"); 
     }
	control_msgs::FollowJointTrajectoryResult result;
	result.error_code = 0;
	as_.setSucceeded(result);
 } 
 
 void preemptCB()
 {
   ROS_INFO("%s: Preempted", action_name_.c_str());
   // set the action state to preempted
   as_.setPreempted();
 }
 
  protected: 
  ros::NodeHandle nh_;
  actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction> as_;
  std::string action_name_;
   
};

joint_state_publish：

    <!-- publish joint states -->
  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
    <param name="/use_gui" value="false"/>
    <rosparam param="/source_list">[<span style="color:#ff0000;">/move_group/robothand_controller_joint_states</span>]</rosparam>
  </node>

實例3

controller.yaml

controller_list:
  - name: left_arm_controller
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    default: true
    joints:
      - left_shoulder_stevo_to_axis
      - left_shoulder_stevo_lift_to_axis
      - left_big_arm_up_to_axis
      - left_small_arm_up_to_axis
      - left_wrist_run_stevo_to_axis
  - name: rigth_arm_controller
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    default: true
    joints:
      - right_shoulder_stevo_to_axis
      - right_shoulder_stevo_lift_to_axis
      - right_big_arm_up_to_axis
      - right_small_arm_up_to_axis
      - right_wrist_run_stevo_to_axis
  - name: right_gripper_controller
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    default: true
    joints:
      - right_hand_run_stevo_to_right_hand_run_stevo_axis
  - name: left_gripper_controller
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    default: true
    joints:
      - left_hand_run_stevo_to_left_hand_run_stevo_axis

JointTrajectory msg結構的查看：rosmsg show JointTrajectory

joint.py

from ros_arduino_msgs.srv import *
class Joint:

    ## @brief Constructs a Joint instance.
    ##
    ## @param servoNum The servo number.
    ## 
    ## @param name The joint name.
    def __init__(self, name, servoNum, range):
        self.name = name #關節名稱
        self.servoNum=servoNum #對應的舵機編號
        self.range=range #舵機的控制範圍，這裏是0~180度

        self.position = 0.0 
        self.velocity = 0.0
        self.last = rospy.Time.now()
        ## @brief Set the current position.
    def setCurrentPosition(self):
        rospy.wait_for_service('/arduino/servo_write')
        try:
            servo_write=rospy.ServiceProxy('/arduino/servo_write',ServoWrite)
            servo_write(self.servoNum,self.position)
        except rospy.ServiceException, e:
            print "Service call failed: %s"%e

follow_controller.py 主要的驅動

初始化joints列表，同時啓動action Server:

def __init__(self, name):
        self.name = name

        # rates
        self.rate = 20.0

        # left Arm jonits list
        self.left_shoulder_stevo_to_axis=Joint(left_shoulder_stevo_to_axis,6,PI)
        self.left_shoulder_stevo_lift_to_axis=Joint(left_shoulder_stevo_lift_to_axis,7,PI)
        self.left_big_arm_up_to_axis=Joint(left_big_arm_up_to_axis,8,PI)
        self.left_small_arm_up_to_axis=Joint(left_small_arm_up_to_axis,9,PI)
        self.left_wrist_run_stevo_to_axis=Joint(left_wrist_run_stevo_to_axis,10,PI)

        self.joints=list()
        self.joints.append(left_shoulder_stevo_to_axis)
        self.joints.append(left_shoulder_stevo_lift_to_axis)
        self.joints.append(left_big_arm_up_to_axis)
        self.joints.append(left_small_arm_up_to_axis)
        self.joints.append(left_wrist_run_stevo_to_axis)


        # left hand joint
        self.left_hand_run_stevo_to_left_hand_run_stevo_axis=Joint(left_hand_run_stevo_to_left_hand_run_stevo_axis,11,PI)
        self.joints.append(left_hand_run_stevo_to_left_hand_run_stevo_axis)

        # right Arm jonits
        self.right_shoulder_stevo_to_axis=Joint(right_shoulder_stevo_to_axis,0,PI)
        self.right_shoulder_stevo_lift_to_axis=Joint(right_shoulder_stevo_lift_to_axis,1,PI)
        self.right_big_arm_up_to_axis=Joint(right_big_arm_up_to_axis,2,PI)
        self.right_small_arm_up_to_axis=Joint(right_small_arm_up_to_axis,3,PI)
        self.right_wrist_run_stevo_to_axis=Joint(right_wrist_run_stevo_to_axis,4,PI)

        self.joints.append(right_shoulder_stevo_to_axis)
        self.joints.append(right_shoulder_stevo_lift_to_axis)
        self.joints.append(right_big_arm_up_to_axis)
        self.joints.append(right_small_arm_up_to_axis)
        self.joints.append(right_wrist_run_stevo_to_axis)

        # left hand joint
        self.right_hand_run_stevo_to_right_hand_run_stevo_axis=Joint(right_hand_run_stevo_to_right_hand_run_stevo_axis,5,PI)
        self.joints.append(right_hand_run_stevo_to_right_hand_run_stevo_axis)        

        # set the left arm back to the resting position
        rospy.loginfo("set the left arm back to the resting position")
        self.left_shoulder_stevo_to_axis.setCurrentPosition(PI/2)
        self.left_shoulder_stevo_lift_to_axis.setCurrentPosition(PI/2)
        self.left_big_arm_up_to_axis.setCurrentPosition(PI/2)
        self.left_small_arm_up_to_axis.setCurrentPosition(PI/2)
        self.left_wrist_run_stevo_to_axis.setCurrentPosition(PI/2)
        # set the right arm back t

actionCb函數（Action Service的回調函數，收到msg後就會調用該函數）

def actionCb(self, goal):
        rospy.loginfo(self.name + ": Action goal recieved.")
        traj = goal.trajectory 

        if not traj.points:#判斷收到的消息是否爲空
            msg = "Trajectory empy."
            rospy.logerr(msg)
            self.server.set_aborted(text=msg)
            return  

        try:
            indexes = [traj.joint_names.index(joint.name) for joint in self.joints]#按照joints列表的順序對traj的數據進行排序，把排序數據放到indexes中
        except ValueError as val:
            msg = "Trajectory invalid."
            rospy.logerr(msg)
            self.server.set_aborted(text=msg)
            return

        start = traj.header.stamp#當前的時間戳
        if start.secs == 0 and start.nsecs == 0:
            start = rospy.Time.now()

        r = rospy.Rate(self.rate)
        for point in traj.points:            
            desired = [ point.positions[k] for k in indexes ]#期望的控制點
            for i in indexes
                self.joints[i].position=desired[i]#控制點對應的舵機的位置
                self.joints[i].setCurrentPosition()#發送舵機的控制命令

            while rospy.Time.now() + rospy.Duration(0.01) < start:#如果當前時間小於舵機這個點預期完成時間，則等待
                rospy.sleep(0.01)                        

        rospy.loginfo(self.name + ": Done.")

參考文獻：

ROS學習筆記16 —— 控制真實機械臂的action（實例大集合）

文章目錄

1. 瞭解整體概況

2.實例

框架

實例1

實例2

實例3

詐騙（殺豬盤）網站進行滲透測試

Python 潮流週刊#50：我最喜歡的 Python 3.13 新特性！

外行也能讀懂的網絡硬件設備功能原理速成

ROS終端顯示中文的設置辦法

c/c++中點運算符“.“和箭頭運算符“→“的區別

Qt+Halcon筆記（1）—— 環境配置（兩種方案，博主親測）

MATLAB - 導出設置中大小設置失效問題的解決

【個人收藏筆記】CMakeLists.txt 語法簡明教程

https://yachay.unat.edu.pe/blog/index.php?comment_area=format_blog&comment_component=blog&comment_co

linux以太網驅動總結