安心啃Actionlib with kinova

Actionlib 进修（2019/10/15）

1. 进一步加深了对Actionlib的理解，Action client 与Action Server， 均可声明一个Action只要保证名字与格式一直就行，在kinova的launch文件中，就已经将Pose_Action与Joint_Action的Server端启动了，所以用户只需要通过client向Action发生request即可。
2. 下面代码即是Kinova的launch启动了ActionServer，同时对构造函数中通过初始化列表来初始化字段，即也就是与构造函数同名的类下面的定义的私有成员变量。

namespace kinova
{

KinovaPoseActionServer::KinovaPoseActionServer(KinovaComm &arm_comm, const ros::NodeHandle &nh, const std::string &kinova_robotType, const std::string &kinova_robotName)
    : arm_comm_(arm_comm),
      node_handle_(nh, "pose_action"),
      kinova_robotType_(kinova_robotType),
      kinova_robotName_(kinova_robotName),
      action_server_(node_handle_, "tool_pose",
                     boost::bind(&KinovaPoseActionServer::actionCallback, this, _1), false)
{
    double position_tolerance;
    double EulerAngle_tolerance;
    node_handle_.param<double>("stall_interval_seconds", stall_interval_seconds_, 1.0);
    node_handle_.param<double>("stall_threshold", stall_threshold_, 0.005);
    node_handle_.param<double>("rate_hz", rate_hz_, 10.0);
    node_handle_.param<double>("position_tolerance", position_tolerance, 0.01);
    node_handle_.param<double>("EulerAngle_tolerance", EulerAngle_tolerance, 2.0*M_PI/180);

    //    tf_prefix_ = kinova_robotType_ + "_" + boost::lexical_cast<string>(same_type_index); // in case of multiple same_type robots
    tf_prefix_ = kinova_robotName_ + "_";

    position_tolerance_ = static_cast<float>(position_tolerance);
    EulerAngle_tolerance_ = static_cast<float>(EulerAngle_tolerance);
    std::stringstream ss;
    ss << tf_prefix_ << "link_base";
    link_base_frame_ = ss.str();

    action_server_.start();

//    if(ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug))
//    {
//        ros::console::notifyLoggerLevelsChanged();
//    }
}
}  // namespace kinova

如上代码中

KinovaPoseActionServer::KinovaPoseActionServer(KinovaComm &arm_comm, const ros::NodeHandle &nh, const std::string &kinova_robotType, const std::string &kinova_robotName)
    : arm_comm_(arm_comm),
      node_handle_(nh, "pose_action"),
      kinova_robotType_(kinova_robotType),
      kinova_robotName_(kinova_robotName),
      action_server_(node_handle_, "tool_pose",
                     boost::bind(&KinovaPoseActionServer::actionCallback, this, _1), false)

后面的：是使用初始化列表来初始化字段。即通过构造函数来初始化class KinovaPoseActionServer中的一些私有成员变量！类的定义如下图所示。

namespace kinova
{

class KinovaPoseActionServer
{
 public:
    KinovaPoseActionServer(KinovaComm &, const ros::NodeHandle &n, const std::string &kinova_robotType, const std::string &kinova_robotName);
    ~KinovaPoseActionServer();

    void actionCallback(const kinova_msgs::ArmPoseGoalConstPtr &);

 private:
    ros::NodeHandle node_handle_;
    std::string kinova_robotType_;
    std::string kinova_robotName_;
    KinovaComm &arm_comm_;
    actionlib::SimpleActionServer<kinova_msgs::ArmPoseAction> action_server_;
    tf::TransformListener listener;

    ros::Time last_nonstall_time_;
    kinova::KinovaPose last_nonstall_pose_;

    std::string link_base_frame_;

    // Parameters
    double stall_interval_seconds_;
    double stall_threshold_;
    double rate_hz_;
    float position_tolerance_;
    float EulerAngle_tolerance_;
    std::string tf_prefix_;
};

}  // namespace kinova
#endif  // KINOVA_DRIVER_KINOVA_POSE_ACTION_H_s

3. 实现了在kinova机械臂运动过程中，通过订阅Joint主题，得到当前关节角，实现了瓶盖的转动。代码如下：
   1）首先定义了一个回调函数：

void currentjointangleFeedback(const kinova_msgs::JointAnglesConstPtr & joint_angle_command)
{
  joints.joint1 = joint_angle_command->joint1;
  joints.joint2 = joint_angle_command->joint2;
  joints.joint3 = joint_angle_command->joint3;
  joints.joint4 = joint_angle_command->joint4;
  joints.joint5 = joint_angle_command->joint5;
  joints.joint6 = joint_angle_command->joint6;
  currcent_flag = true;
  ROS_INFO("CallBack: Joint6: %f",joints.joint6);
}

2）订阅声明

  ros::Subscriber Pose_sub = nh.subscribe("/j2n6s300_driver/out/tool_pose", 1, currentPoseFeedback);

3）回调一次，通过一次回调得到当前关节角。

  while(ros::ok())
  {
    ros::spinOnce();
    rate.sleep();

    if(currcentJoint_flag == true)
    {
      armJoint = subJOINTS;
      armJoint[5]=armJoint[5]+100;
      currcentJoint_flag = false;
      break;
    }
  }
  joint_pose_client(armJoint);

总结与计划（2019/10/8）

1. 整合kinect2的识别与ROS，同时实现定位抓取。
   1）具体实现步骤：类比Aruco代码？
   2）寻找ROS-kinect2成熟的物体识别代码包？

2. 上面的bug已经找出来了，需要具体研读代码分析原因。

3. C++菜鸟教程！！

安心啃Actionlib with kinova（2019/9/30）

1.找到了bug所在地了，在/home/ljq/catkin_ws/src/kinova-ros/kinova_driver/src/kinova_tool_pose_action.cpp
2. 深入研究下面两段代码：

actionlib::SimpleActionClient <kinova_msgs::SetFingersPositionAction> *finger_client = new actionlib::SimpleActionClient<kinova_msgs::SetFingersPositionAction>(
        "j2n6s300_driver/fingers_action/finger_positions");

    actionlib::SimpleActionServer<kinova_msgs::ArmPoseAction> action_server_;