webots、ros联合调教自定义控制器实现

山木Hubery

已于 2022-12-18 21:42:46 修改

阅读量1.4k

点赞数 2

文章标签：机器人 c++ vscode

于 2022-12-18 21:29:57 首次发布

本文链接：https://blog.csdn.net/Hubery12138/article/details/128365882

版权

1、功能包配置

首先我们先新建一个空的功能包，指令如下

$ catkin_create_pkg controller std_msgs rospy roscpp sensor_msgs

再进入/usr/local/webots/projects/default/controllers/ros/include/文件夹下面的srv和msg文件夹复制到刚刚创建的功能包内
接下来对功能包内cmakelist文件进行配置

cmake_minimum_required(VERSION 3.0.2)
project(my_ros_controller)

## Compile as C++11, supported in ROS Kinetic and newer
# add_compile_options(-std=c++11)

## 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
  message_generation
  roscpp
  rospy
  sensor_msgs
  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 tag for "message_generation"
##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
##     but can be declared for certainty nonetheless:
##     * add a exec_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
   BoolStamped.msg
   Float64Stamped.msg
   Int32Stamped.msg
   Int8Stamped.msg
   RadarTarget.msg
   RecognitionObject.msg
   StringStamped.msg
   controller.msg
)
##此处加入的controller.msg文件是我自定义的msg文件
 ##Generate services in the 'srv' folder
add_service_files(
   FILES
   controllers.srv
   camera_get_focus_info.srv
   camera_get_info.srv
   camera_get_zoom_info.srv
   display_draw_line.srv
   display_draw_oval.srv
   display_draw_pixel.srv
   display_draw_polygon.srv
   display_draw_rectangle.srv
   display_draw_text.srv
   display_get_info.srv
   display_image_copy.srv
   display_image_delete.srv
   display_image_load.srv
   display_image_new.srv
   display_image_paste.srv
   display_image_save.srv
   display_set_font.srv
   field_get_bool.srv
   field_get_color.srv
   field_get_count.srv
   field_get_float.srv
   field_get_int32.srv
   field_get_node.srv
   field_get_rotation.srv
   field_get_string.srv
   field_get_type.srv
   field_get_type_name.srv
   field_get_vec2f.srv
   field_get_vec3f.srv
   field_import_node.srv
   field_import_node_from_string.srv
   field_remove_node.srv
   field_remove.srv
   field_set_bool.srv
   field_set_color.srv
   field_set_float.srv
   field_set_int32.srv
   field_set_rotation.srv
   field_set_string.srv
   field_set_vec2f.srv
   field_set_vec3f.srv
   get_bool.srv
   get_float_array.srv
   get_float.srv
   get_int.srv
   get_string.srv
   get_uint64.srv
   get_urdf.srv
   gps_decimal_degrees_to_degrees_minutes_seconds.srv
   lidar_get_frequency_info.srv
   lidar_get_info.srv
   lidar_get_layer_point_cloud.srv
   lidar_get_layer_range_image.srv
   motor_set_control_pid.srv
   mouse_get_state.srv
   node_add_force_or_torque.srv
   node_add_force_with_offset.srv
   node_get_center_of_mass.srv
   node_get_contact_point.srv
   node_get_field.srv
   node_get_id.srv
   node_get_number_of_contact_points.srv
   node_get_name.srv
   node_get_orientation.srv
   node_get_parent_node.srv
   node_get_position.srv
   node_get_static_balance.srv
   node_get_status.srv
   node_get_type.srv
   node_get_velocity.srv
   node_remove.srv
   node_reset_functions.srv
   node_move_viewpoint.srv
   node_set_visibility.srv
   node_set_velocity.srv
   pen_set_ink_color.srv
   range_finder_get_info.srv
   receiver_get_emitter_direction.srv
   robot_get_device_list.srv
   robot_set_mode.srv
   robot_wait_for_user_input_event.srv
   save_image.srv
   set_bool.srv
   set_float.srv
   set_float_array.srv
   set_int.srv
   set_string.srv
   skin_get_bone_name.srv
   skin_get_bone_orientation.srv
   skin_get_bone_position.srv
   skin_set_bone_orientation.srv
   skin_set_bone_position.srv
   speaker_is_sound_playing.srv
   speaker_speak.srv
   speaker_play_sound.srv
   supervisor_get_from_def.srv
   supervisor_get_from_id.srv
   supervisor_movie_start_recording.srv
   supervisor_set_label.srv
   supervisor_virtual_reality_headset_get_orientation.srv
   supervisor_virtual_reality_headset_get_position.srv
)
##此处加入的controllers.srv是我自定义的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
  std_msgs 
  sensor_msgs   
)

################################################
## Declare ROS dynamic reconfigure parameters ##
################################################

## To declare and build dynamic reconfigure parameters within this
## package, follow these steps:
## * In the file package.xml:
##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
## * In this file (CMakeLists.txt):
##   * add "dynamic_reconfigure" to
##     find_package(catkin REQUIRED COMPONENTS ...)
##   * uncomment the "generate_dynamic_reconfigure_options" section below
##     and list every .cfg file to be processed

## Generate dynamic reconfigure parameters in the 'cfg' folder
# generate_dynamic_reconfigure_options(
#   cfg/DynReconf1.cfg
#   cfg/DynReconf2.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 your 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 my_ros_controller
  CATKIN_DEPENDS message_generation roscpp rospy sensor_msgs 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
  ${catkin_INCLUDE_DIRS}
)
if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE)
  set(CMAKE_BUILD_TYPE Release)
endif()

link_directories($ENV{WEBOTS_HOME}/lib/controller)

set (LIBRARIES ${CMAKE_SHARED_LIBRARY_PREFIX}Controller${CMAKE_SHARED_LIBRARY_SUFFIX} 
               ${CMAKE_SHARED_LIBRARY_PREFIX}CppController${CMAKE_SHARED_LIBRARY_SUFFIX} 
)

set(EXECUTABLE_OUTPUT_PATH ../)
               
include_directories($ENV{WEBOTS_HOME}/include/controller/c $ENV{WEBOTS_HOME}/include/controller/cpp ./include /usr/local/include/eigen3)

include_directories(
  include 
  "/usr/include/eigen3" 
  $ENV{WEBOTS_HOME}/include/controller/cpp
  $ENV{WEBOTS_HOME}/include/controller/c
)
## Declare a C++ library
# add_library(${PROJECT_NAME}
#   src/${PROJECT_NAME}/my_ros_controller.cpp
# )

## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
# add_executable(${PROJECT_NAME}_node src/my_ros_controller_node.cpp)

## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
## target back to the shorter version for ease of user use
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")

## Add cmake target dependencies of the executable
## same as for the library above
# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

## Specify libraries to link a library or executable target against
# target_link_libraries(${PROJECT_NAME}_node
#   ${catkin_LIBRARIES}
# )

add_executable(my_ros_controller  src/my_ros_controller.cpp)
target_link_libraries(my_ros_controller ${catkin_LIBRARIES} ${LIBRARIES})
add_dependencies(my_ros_controller ${PROJECT_NAME}_generate_messages_cpp)
add_executable(personalservice_call  src/personalservice_call.cpp)
target_link_libraries(personalservice_call ${catkin_LIBRARIES} ${LIBRARIES})
add_dependencies(personalservice_call ${PROJECT_NAME}_generate_messages_cpp)
#############
## 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
# catkin_install_python(PROGRAMS
#   scripts/my_python_script
#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )

## Mark executables for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
# install(TARGETS ${PROJECT_NAME}_node
#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )

## Mark libraries for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
# install(TARGETS ${PROJECT_NAME}
#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
#   RUNTIME DESTINATION ${CATKIN_GLOBAL_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_my_ros_controller.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)

以上代码中controller.msg与controllers.srv是我自定义的文件，若无自定义文件无需加入，若有自定义文件，请以自定义的文件名加入，message_generation是因为我加入了自定义的数据文件后才加入的，如果无自定义文件也无需加入。
下一步是package文件的配置

<?xml version="1.0"?>
<package format="2">
  <name>my_ros_controller</name>
  <version>0.0.0</version>
  <description>The my_ros_controller package</description>

  <!-- One maintainer tag required, multiple allowed, one person per tag -->
  <!-- Example:  -->
  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
  <maintainer email="hubery@todo.todo">hubery</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 multiple are allowed, one per tag -->
  <!-- Optional attribute type can be: website, bugtracker, or repository -->
  <!-- Example: -->
  <!-- <url type="website">http://wiki.ros.org/my_ros_controller</url> -->


  <!-- Author tags are optional, multiple are allowed, one per tag -->
  <!-- Authors do not have to be maintainers, 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 depend as a shortcut for packages that are both build and exec dependencies -->
  <!--   <depend>roscpp</depend> -->
  <!--   Note that this is equivalent to the following: -->
  <!--   <build_depend>roscpp</build_depend> -->
  <!--   <exec_depend>roscpp</exec_depend> -->
  <!-- Use build_depend for packages you need at compile time: -->
  <!--   <build_depend>message_generation</build_depend> -->
  <!-- Use build_export_depend for packages you need in order to build against this package: -->
  <!--   <build_export_depend>message_generation</build_export_depend> -->
  <!-- Use buildtool_depend for build tool packages: -->
  <!--   <buildtool_depend>catkin</buildtool_depend> -->
  <!-- Use exec_depend for packages you need at runtime: -->
  <!--   <exec_depend>message_runtime</exec_depend> -->
  <!-- Use test_depend for packages you need only for testing: -->
  <!--   <test_depend>gtest</test_depend> -->
  <!-- Use doc_depend for packages you need only for building documentation: -->
  <!--   <doc_depend>doxygen</doc_depend> -->
  <buildtool_depend>catkin</buildtool_depend>
  <build_depend>message_generation</build_depend>
  <build_depend>roscpp</build_depend>
  <build_depend>rospy</build_depend>
  <build_depend>sensor_msgs</build_depend>
  <build_depend>std_msgs</build_depend>
  <build_export_depend>roscpp</build_export_depend>
  <build_export_depend>rospy</build_export_depend>
  <build_export_depend>sensor_msgs</build_export_depend>
  <build_export_depend>std_msgs</build_export_depend>
 <exec_depend>message_generation</exec_depend>
  <exec_depend>message_runtime</exec_depend>
  <exec_depend>roscpp</exec_depend>
  <exec_depend>rospy</exec_depend>
  <exec_depend>sensor_msgs</exec_depend>
  <exec_depend>std_msgs</exec_depend>


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

  </export>
</package>

2、自定义代码实现

完成功能包的配置后，我们就能开始自定义控制器的实现了，实现如下：

#include <webots/Robot.hpp>
#include <webots/Motor.hpp>
#include <webots/RangeFinder.hpp>
#include <webots/PositionSensor.hpp>
#include <ros/ros.h>
#include "my_ros_controller/controllers.h"
using namespace webots;
Motor* wheel[7];
#define TIME_STEP 8    //时钟，设置为8，记得webots中的world_info节点里的time_step也要改成8
#define robot_unique_name "/Xrobot/" 
Robot *robot ;

bool jointPPCallback(my_ros_controller::controllers::Request &req , my_ros_controller::controllers::Response &res){
    wheel[0]->setPosition(req.joint_position1) ;
    wheel[1]->setPosition(req.joint_position2) ;
    wheel[2]->setPosition(req.joint_position3) ;
    wheel[3]->setPosition(req.joint_position4) ;
    wheel[4]->setPosition(req.joint_position5) ;
    robot->step(8000)    ;

    ROS_INFO("The destination is %f,%f,%f,%f,%f",req.joint_position1,req.joint_position2,req.joint_position3,req.joint_position4,req.joint_position5);



    
    return true ;
}

bool jointTRCallback(my_ros_controller::controllers::Request &req , my_ros_controller::controllers::Response &res){
    wheel[0]->setPosition(req.joint_position1) ;
    wheel[1]->setPosition(req.joint_position2) ;
    wheel[2]->setPosition(req.joint_position3) ;
    wheel[3]->setPosition(req.joint_position4) ;
    wheel[4]->setPosition(req.joint_position5) ;
    wheel[5]->setPosition(req.joint_position6) ;
    wheel[6]->setPosition(req.joint_position7) ;
    robot->step(80) ;

    ROS_INFO("The destination is %f,%f,%f,%f,%f",req.joint_position1,req.joint_position2,req.joint_position3,req.joint_position4,req.joint_position5);



    
    return true ;
}


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

    robot = new Robot();
    //ROS节点初始化
    ros::init(argc,argv,"my_ros_controller1");
    //创建节点句柄
    ros::NodeHandle n;
   // get the time step of the current world.
    int timeStep = (int)robot->getBasicTimeStep();


    wheel[0] = robot->getMotor("joint1");
    wheel[1] = robot->getMotor("joint2");
    wheel[2] = robot->getMotor("joint3");
    wheel[3] = robot->getMotor("joint4");
    wheel[4] = robot->getMotor("joint5");
    wheel[5] = robot->getMotor("joint6");
    wheel[6] = robot->getMotor("joint7"); 


    ros::ServiceServer jointPPcontrol = n.advertiseService("/jointPP_control",jointPPCallback);
    ros::ServiceServer jointTRcontrol = n.advertiseService("/jointTR_control",jointTRCallback);

    wheel[0]->setPosition(0.1);
    wheel[1]->setPosition(0.1);
    wheel[2]->setPosition(0.1);
    wheel[3]->setPosition(0.1);
    wheel[4]->setPosition(0.1);
    ROS_INFO("Ready to show person information.");
    robot->step(7000) ;


    ros::spin();
/*     while( robot->step(timeStep) != -1 ){
    wheel[0]->setPosition(1.0);
    wheel[1]->setPosition(1.0);
    wheel[2]->setPosition(1.0);
    wheel[3]->setPosition(1.0);
    wheel[4]->setPosition(1.0);

    ROS_INFO("Ready to show person information.");





    }  */


    delete robot;
    return 0;



}

如图，控制器运行的关键点是robot->step( ) 这一代码，因为ros环境和webots的时间是不匹配的，robot->step( n )，n是你对应的世界里的worldinfo下的basictimestep的倍数，n可理解为你给控制器执行你目标动作需要的时间，因此可通过对n的修改实现将ros与webots时间的匹配。

3、控制器接入webots

方式一：在webots中robot的controller内接入

打开目标功能包所在工作空间的devel/lib文件夹，选择目标功能包对应的文件夹，复制粘贴到运行的webots世界所对应的功能包内的controller文件夹内，然后再打开webots，就可以找到这个新的控制器了。
！！！！注意此处的功能包名与对应的可执行文件名保持一致，否则即便接入webots也无法运行！！！！

方式二：直接在工作空间rosrun对应的可执行文件

此时直接rosrun一般会遇到报错，libController.so的编译存在问题
解决方法：

sudo find / -name libController.so

之后复制运行的得到的路径，再打开/etc/ld.so.cof.d文件夹内的libc.conf文件，再在其中粘贴上刚刚复制的路径。
！！！！注意我们运行得到的路径大致如：/usr/local/webots/lib/controller/libController.so，此时我们应删去最后的/libController.so，再粘贴入文件！！！！
！！！！如果发现无法修改该文件，可通过vscode打开该文件再修改！！！！
最后再执行以下指令完成配置