给KUKA youbot机械臂添加dynamic reconfig

最新推荐文章于 2024-07-11 16:09:52 发布

yaked19

最新推荐文章于 2024-07-11 16:09:52 发布

阅读量3.1k

点赞数

分类专栏： ROS 文章标签： ROS KUKA Dynamic Reconfigure

本文链接：https://blog.csdn.net/yaked/article/details/45061889

版权

ROS 专栏收录该内容

42 篇文章 39 订阅

订阅专栏

首先，关于Dynamic Reconfig 的基本的操作和tutorial，官网上比较多，之前我的一篇文章也写过这个:http://blog.csdn.net/yaked/article/details/44924485

最近，因为自己刚把youbot的手臂控制做了一下，那个是在程序中写死位置，编译，运行程序让他动作的。想改变点位必须每次都重新编译再运行，比较麻烦。如果用dynamic reconfigure就可以实时操控机械臂和夹子的运动了。

1. cfg文件的编写

机械臂的5个关节，就是电机角度，单位是弧度，夹子的开合也是2个参数，单位是米，注意：夹子可以分开控制的，并不是指定一个夹子间距。所以，这里会有两个参数。

建立一个cfg文件夹，放入一个名为youbot_arm_gripper.cfg的文件，代码如下：

#!/usr/bin/env python
PACKAGE = "youbot_ros_simple_trajectory"

from dynamic_reconfigure.parameter_generator_catkin import *
gen = ParameterGenerator()

#       Name    Type    Level 		Description         Default        Min   Max
gen.add("arm1", double_t, 0, "An double radian parameter", 0.11, 0.0100692, 5.84014)
gen.add("arm2", double_t, 0, "An double radian parameter", 0.11, 0.0100692, 2.61799)
gen.add("arm3", double_t, 0, "An double radian parameter", -0.11, -5.02655, -0.015708)
gen.add("arm4", double_t, 0, "An double radian parameter", 0.11, 0.0221239, 3.4292)
gen.add("arm5", double_t, 0, "An double radian parameter", 0.12, 0.110619, 5.64159)

gen.add("gripper_l_param", double_t, 0, "An double parameter unit meter", 0.008, 0, 0.0115)
gen.add("gripper_r_param", double_t, 0, "An double parameter unit meter", 0.008, 0, 0.0115)

#gen.add("double_param", double_t, 0, "A double parameter",    .5, 0,   1)
#gen.add("str_param",    str_t,    0, "A string parameter",  "Hello World")
#gen.add("bool_param",   bool_t,   0, "A Boolean parameter",  True)


exit(gen.generate(PACKAGE, "youbot_ros_simple_trajectory", "youbot_arm_gripper"))

之前的那篇文章中，我提过cfg文件的名字要是最后一行程序的第三个参数，注意保持一致。

2. XML文件

<?xml version="1.0"?>
<package>
  <name>youbot_ros_simple_trajectory</name>
  <version>0.0.0</version>
  <description>The youbot_ros_simple_trajectory package</description>

  <!-- One maintainer tag required, multiple allowed, one person per tag --> 
  <!-- Example:  -->
  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
  <maintainer email="yake@todo.todo">yake</maintainer>


  <!-- One license tag required, multiple allowed, one license per tag -->
  <!-- Commonly used license strings: -->
  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
  <license>TODO</license>


  <!-- Url tags are optional, but mutiple are allowed, one per tag -->
  <!-- Optional attribute type can be: website, bugtracker, or repository -->
  <!-- Example: -->
  <!-- <url type="website">http://wiki.ros.org/youbot_ros_simple_trajectory</url> -->


  <!-- Author tags are optional, mutiple are allowed, one per tag -->
  <!-- Authors do not have to be maintianers, but could be -->
  <!-- Example: -->
  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->


  <!-- The *_depend tags are used to specify dependencies -->
  <!-- Dependencies can be catkin packages or system dependencies -->
  <!-- Examples: -->
  <!-- Use build_depend for packages you need at compile time: -->
   <build_depend>message_generation</build_depend> 
  <!-- Use buildtool_depend for build tool packages: -->
  <!--   <buildtool_depend>catkin</buildtool_depend> -->
  <!-- Use run_depend for packages you need at runtime: -->
  <run_depend>message_runtime</run_depend>
  
  <!-- Use test_depend for packages you need only for testing: -->
  <!--   <test_depend>gtest</test_depend> -->
  <buildtool_depend>catkin</buildtool_depend>
  <build_depend>actionlib</build_depend>
  <build_depend>control_msgs</build_depend>
  <build_depend>roscpp</build_depend>
  <build_depend>rospy</build_depend>
  <build_depend>trajectory_msgs</build_depend>
  <build_depend>brics_actuator</build_depend>
  <build_depend>dynamic_reconfigure</build_depend>
  <build_depend>std_msgs</build_depend>
  
  
  <run_depend>actionlib</run_depend>
  <run_depend>control_msgs</run_depend>
  <run_depend>roscpp</run_depend>
  <run_depend>rospy</run_depend>
  <run_depend>trajectory_msgs</run_depend>
  <run_depend>brics_actuator</run_depend>
  <run_depend>dynamic_reconfigure</run_depend>
  <run_depend>std_msgs</run_depend>

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

  </export>
</package>

3. CMakeList

cmake_minimum_required(VERSION 2.8.3)
project(youbot_ros_simple_trajectory)

## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
  actionlib
  control_msgs
  roscpp
  rospy
  trajectory_msgs
  brics_actuator
  dynamic_reconfigure
  std_msgs
  message_generation
)

## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)


## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()

################################################
## Declare ROS messages, services and actions ##
################################################

## To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml:
##   * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
##   * If MSG_DEP_SET isn't empty the following dependencies might have been
##     pulled in transitively but can be declared for certainty nonetheless:
##     * add a build_depend tag for "message_generation"
##     * add a run_depend tag for "message_runtime"
## * In this file (CMakeLists.txt):
##   * add "message_generation" and every package in MSG_DEP_SET to
##     find_package(catkin REQUIRED COMPONENTS ...)
##   * add "message_runtime" and every package in MSG_DEP_SET to
##     catkin_package(CATKIN_DEPENDS ...)
##   * uncomment the add_*_files sections below as needed
##     and list every .msg/.srv/.action file to be processed
##   * uncomment the generate_messages entry below
##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)

## Generate messages in the 'msg' folder
 #add_message_files(
  # FILES
   #youbot_ros_simple_trajectory_Data.msg

 #)

## Generate services in the 'srv' folder
# add_service_files(
#   FILES
#   Service1.srv
#   Service2.srv
# )

## Generate actions in the 'action' folder
# add_action_files(
#   FILES
#   Action1.action
#   Action2.action
# )

## Generate added messages and services with any dependencies listed here
 #generate_messages(
  #DEPENDENCIES
  #control_msgs
  #trajectory_msgs
 #)

generate_dynamic_reconfigure_options(
  cfg/youbot_arm_gripper.cfg
  
  #...
)

###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if you package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
 INCLUDE_DIRS include
 #LIBRARIES youbot_ros_simple_trajectory
 CATKIN_DEPENDS actionlib control_msgs roscpp rospy trajectory_msgs brics_actuator dynamic_reconfigure std_msgs message_runtime
 #DEPENDS system_lib
)

###########
## Build ##
###########

## Specify additional locations of header files
## Your package locations should be listed before other locations
# include_directories(include)
include_directories(
  ${catkin_INCLUDE_DIRS}
)

include_directories(${dynamic_reconfigure_PACKAGE_PATH}/cmake/cfgbuild.cmake)

## Declare a cpp library
# add_library(youbot_ros_simple_trajectory
#   src/${PROJECT_NAME}/youbot_ros_simple_trajectory.cpp
# )

## Declare a cpp executable
add_executable(youbot_ros_p2p src/youbot_ros_simple_trajectory.cpp)


add_executable(youbot_ctl src/youbot_trajectory_controller.cpp)
add_dependencies(youbot_ctl youbot_ros_simple_trajectory_generate_messages_cpp)
add_dependencies(youbot_ctl ${PROJECT_NAME}_gencfg)
## Add cmake target dependencies of the executable/library
## as an example, message headers may need to be generated before nodes
add_executable(dy_server src/dy_server.cpp)
add_dependencies(dy_server ${PROJECT_NAME}_gencfg)
add_dependencies(dy_server youbot_ros_simple_trajectory_generate_messages_cpp)
## Specify libraries to link a library or executable target against
 target_link_libraries(youbot_ros_p2p
   ${catkin_LIBRARIES}
 )
 
 target_link_libraries(youbot_ctl
   ${catkin_LIBRARIES}
 )

target_link_libraries(dy_server
   ${catkin_LIBRARIES}
 )
#############
## Install ##
#############

# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html

## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# install(PROGRAMS
#   scripts/my_python_script
#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )

## Mark executables and/or libraries for installation
# install(TARGETS youbot_ros_simple_trajectory youbot_ros_simple_trajectory_node
#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )

## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/
#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
#   FILES_MATCHING PATTERN "*.h"
#   PATTERN ".svn" EXCLUDE
# )

## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
#   # myfile1
#   # myfile2
#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )

#############
## Testing ##
#############

## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_youbot_ros_simple_trajectory.cpp)
# if(TARGET ${PROJECT_NAME}-test)
#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()

## Add folders to be run by python nosetests
# catkin_add_nosetests(test)

4. cpp文件中的单元测试（unit test）

在这里我说一下，我们做过的关于tutorials一般是写一个server和一个client，一开始，我也一直纠结在这个地方，因为如果要那样的话，我还得分出一个node来写client，所以最开始，我尝试了一下，并没有和上一篇的程序整个结合起来，相当于只是单独测试dynamic reconfigure这个模块。

一个是server一个是client, 到这里，我们可以说GUI和程序就已经联系上了，这两个小程序是来自官方的tutorial

http://wiki.ros.org/dynamic_reconfigure/Tutorials

你也可以改改试试看

5. cpp文件深入

先说思路，以前控制做法是将5个角度与gripper的2个参数放在数组中，既然是动态调整，我们就必须给它五个参数和2个gripper一个存放的地方，先由GUI 读取存到这7个变量上，再依次赋值给数组。实际中，是怎么处理的呢，我先放上程序：

#include <iostream>
#include <assert.h>

#include <ros/ros.h>
#include <boost/units/io.hpp>
#include <boost/units/systems/angle/degrees.hpp>
#include <boost/units/conversion.hpp>

#include <boost/units/systems/si/length.hpp>
#include <boost/units/systems/si/plane_angle.hpp>

#include "trajectory_msgs/JointTrajectory.h"
#include "brics_actuator/CartesianWrench.h"
#include "brics_actuator/JointPositions.h"
#include <control_msgs/FollowJointTrajectoryActionGoal.h>

#include <dynamic_reconfigure/server.h>
#include "youbot_ros_simple_trajectory/youbot_arm_gripperConfig.h"

using namespace std;



static const string JOINTNAME_PRE = "arm_joint_";
static const string GRIPPER_JOINTNAME_PRE = "gripper_finger_joint_";

static const uint NUM_ARM_JOINTS = 5;
static const uint NUM_GRIPPER_JOINTS = 2;

ros::Publisher armPositionsPublisher;
ros::Publisher gripperPositionPublisher;

vector<control_msgs::FollowJointTrajectoryActionGoal::ConstPtr> trajectories;

  brics_actuator::JointPositions arm_command;
  brics_actuator::JointPositions gripper_command;
  
  vector <brics_actuator::JointValue> armJointPositions;
  vector <brics_actuator::JointValue> gripperJointPositions;

 // Declare variables that can be modified by launch file or command line.
  double arm1_ = 0.11;
  double arm2_ = 0.11;
  double arm3_ = -0.11;
  double arm4_ = 0.11;
  double arm5_ = 0.12;
  
  double gripper_l_param_ = 0.008;
  double gripper_r_param_= 0.008;
  
  int rate;
  string topic;

//static const double INIT_POS[] = {2.94961, 1.352, -2.591, 0.1, 0.12}; // Twist fold
//static const double INIT_POS[] = {2.56, 1.05, -2.43, 1.73, 0.12}; // Upstraight
//static const double INIT_POS[] = {0.11, 0.11, -0.11, 0.11, 0.12}; // Home 
//static const double Gripper_Init_Pos[] = {0.008, 0.008};

double INIT_POS[] = {arm1_, arm2_, arm3_, arm4_, arm5_}; // Home 
double Gripper_Init_Pos[] = {gripper_l_param_, gripper_r_param_};

// dynamic callback
void callback(youbot_ros_simple_trajectory::youbot_arm_gripperConfig &config, uint32_t level)
{
  ROS_INFO("Reconfigure Server received from GUI : %f %f %lf %f %f %f %f ",
           config.arm1,
           config.arm2,
           config.arm3,
           config.arm4,
           config.arm5,
           config.gripper_l_param,
           config.gripper_r_param
		  );
		  
  arm1_ = config.arm1;
  arm2_ = config.arm2;
  arm3_ = config.arm3;
  arm4_ = config.arm4;
  arm5_ = config.arm5;
  
  gripper_l_param_ = config.gripper_l_param;
  gripper_r_param_ = config.gripper_r_param;
  
  double INIT_POS[] = {arm1_, arm2_, arm3_, arm4_, arm5_}; // Add GUI arguments to array
  double Gripper_Init_Pos[] = {gripper_l_param_, gripper_r_param_};
  ROS_INFO("%f %f", INIT_POS[2], Gripper_Init_Pos[1]);
  // do sotmehing for now
  std::stringstream jointName;
	
  for (int i = 0; i < NUM_ARM_JOINTS; ++i)
  {
    jointName.str("");
    jointName << JOINTNAME_PRE << (i + 1);

    arm_command.positions[i].joint_uri = jointName.str();
    arm_command.positions[i].value = INIT_POS[i];

    arm_command.positions[i].unit = boost::units::to_string(boost::units::si::radians);
  }
   
   armPositionsPublisher.publish(arm_command); 
   cout << arm_command << endl;
    
    for( int k = 0; k < NUM_GRIPPER_JOINTS; ++k)
  {
  std::stringstream gripperJointName;
  	gripperJointName.str(""); 
  	(k == 0) ?	gripperJointName<< GRIPPER_JOINTNAME_PRE << "l": (gripperJointName<< GRIPPER_JOINTNAME_PRE << "r");
  		
    gripper_command.positions[k].joint_uri = gripperJointName.str();
    gripper_command.positions[k].value = Gripper_Init_Pos[k];
    gripper_command.positions[k].unit = boost::units::to_string(boost::units::si::meter);
   }
  gripperPositionPublisher.publish(gripper_command);
  cout << gripper_command << endl;
  
  
}



void trajectoryCallback(const control_msgs::FollowJointTrajectoryActionGoal::ConstPtr& msg)
{
  ROS_INFO("callback: Trajectory received");
  //cout << "Msg-Header" << endl << msg->header << endl;
  trajectories.push_back(msg);
}

void moveToInitPos(brics_actuator::JointPositions &command)
{
  std::stringstream jointName;
	
  for (int i = 0; i < NUM_ARM_JOINTS; ++i)
  {
    jointName.str("");
    jointName << JOINTNAME_PRE << (i + 1);

    command.positions[i].joint_uri = jointName.str();
    command.positions[i].value = INIT_POS[i];

    command.positions[i].unit = boost::units::to_string(boost::units::si::radians);
    cout << "Joint " << command.positions[i].joint_uri << " = " << command.positions[i].value << " " << command.positions[i].unit << endl;
  }
  
  cout << command << endl;
  armPositionsPublisher.publish(command); // trully send
  
  cout << "sending command for arm joint init position... and wait" << endl;

  ROS_INFO("End of arm_joint_init-pos");
  // ros::Duration(5).sleep();
}


void moveToGripperInitPos(brics_actuator::JointPositions &command)
{
  std::stringstream gripperJointName;
  
  for( int k = 0; k < NUM_GRIPPER_JOINTS; ++k)
  {
  	gripperJointName.str(""); 
  	(k == 0) ?	gripperJointName<< GRIPPER_JOINTNAME_PRE << "l": (gripperJointName<< GRIPPER_JOINTNAME_PRE << "r");
  		
    command.positions[k].joint_uri = gripperJointName.str();
    command.positions[k].value = Gripper_Init_Pos[k];
    command.positions[k].unit = boost::units::to_string(boost::units::si::meter);
    
    cout << "Joint " << command.positions[k].joint_uri << " = " << command.positions[k].value << " " << command.positions[k].unit << endl;
  }
  
  cout << command << endl;
  gripperPositionPublisher.publish(command); // trully send
  
  cout << "command for gripper joint init position... and wait" << endl;
  
  ROS_INFO("End of gripper_joint_init-pos");
}



int main(int argc, char **argv) {

  ros::init(argc, argv, "youbot_trajectory_controller");
  ros::NodeHandle n;
  uint loop_counter = 0;
  
  
  armJointPositions.resize(NUM_ARM_JOINTS);
  gripperJointPositions.resize(NUM_GRIPPER_JOINTS);
  
  arm_command.positions = armJointPositions;
  gripper_command.positions = gripperJointPositions;
  
  armPositionsPublisher = n.advertise<brics_actuator::JointPositions > ("arm_1/arm_controller/position_command", 1);

  
  gripperPositionPublisher = n.advertise<brics_actuator::JointPositions> ("arm_1/gripper_controller/position_command", 1);


  ros::Subscriber armTrajectory;
  armTrajectory = n.subscribe("/arm_controller/follow_joint_trajectory/goal", 1, trajectoryCallback);

  ros::Duration(1).sleep();


  dynamic_reconfigure::Server<youbot_ros_simple_trajectory::youbot_arm_gripperConfig> server;
  dynamic_reconfigure::Server<youbot_ros_simple_trajectory::youbot_arm_gripperConfig>::CallbackType f;

  f = boost::bind(&callback, _1, _2);
  server.setCallback(f);

  moveToInitPos(arm_command);
  moveToGripperInitPos(gripper_command);
  //ros::Duration(10).sleep();
  ROS_INFO("Init Pos for arm and gripper have reached.");

  ros::Rate r(40);

  // Main loop.
  while (n.ok())
  {
    ros::spinOnce();
    r.sleep();
  }
	return 0;
}

实际中，我们有一个callback函数，每次动态改变GUI上的内容，就会调用它，在callback函数中，我们就进行了之前讨论的操作，先保存数据，再赋值给数组，最后封装，将消息发送出去就行了。

最终，我们的节点图如下：