Actionlib 进修(2019/10/15)
- 进一步加深了对Actionlib的理解,Action client 与Action Server, 均可声明一个Action只要保证名字与格式一直就行,在kinova的launch文件中,就已经将Pose_Action与Joint_Action的Server端启动了,所以用户只需要通过client向Action发生request即可。
- 下面代码即是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
- 实现了在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)
-
整合kinect2的识别与ROS,同时实现定位抓取。
1)具体实现步骤:类比Aruco代码?
2)寻找ROS-kinect2成熟的物体识别代码包? -
上面的bug已经找出来了,需要具体研读代码分析原因。
-
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_;