机器人操作系统ROS_典型功能代码详解(5)

3//rosaria_teleop_code.cpp

#include \

#include \

#include \ #include #include

#define MAX_VELOCITY_LINEAR 5 #define MAX_VELOCITY_ANGULAR 1

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

ros::init(argc, argv, \); ros::NodeHandle n;

ros::Publisher key_publisher = n.advertise(\, 1000); //该话题名称要与接收节点中的一致, 即对应上RosAria节点的话题名称\

ros::Rate loop_rate(30);

int fwd_velocity = 0; int rw_velocity = 0; int ang_velocity = 0;

double vadd=0.0; double wadd=0.0;

bool dog=true;// true表示机器人往前走 int count = 0; while (ros::ok()) {

geometry_msgs::Twist twist_msg; //在该数据结构中, 可以赋值机器人的角速度与线速度控制量

if (dog==true) //让机器人以三角波式的线速度来运行 { vadd+=0.01; if (vadd>=1) { dog=false; } }

if (dog==false) { vadd-=0.01; if (vadd<=-1) { dog=true; } }

twist_msg.linear.x =vadd;

twist_msg.angular.z = ang_velocity; //对Twist数据结构中的相关成员变量赋值

ROS_INFO(\, vadd, ang_velocity);

key_publisher.publish(twist_msg); //发布Twist数据结构

ros::spinOnce();

loop_rate.sleep(); ++count;

21

}

return 0; }

#CmakeList.txt

cmake_minimum_required(VERSION 2.8.3) project(wmrcontrol)

## 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 roscpp rospy std_msgs )

## 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 \## * add a run_depend tag for \## * In this file (CMakeLists.txt):

## * add \## find_package(catkin REQUIRED COMPONENTS ...)

## * add \## 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

# Message1.msg # Message2.msg # )

## Generate services in the 'srv' folder # add_service_files( # FILES

# Service1.srv

22

# 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 # std_msgs # )

################################### ## 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 wmrcontrol

# CATKIN_DEPENDS roscpp rospy std_msgs # 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} )

## Declare a cpp library # add_library(wmrcontrol

# src/${PROJECT_NAME}/wmrcontrol.cpp # )

## Declare a cpp executable

# add_executable(wmrcontrol_node src/wmrcontrol_node.cpp)

## Add cmake target dependencies of the executable/library

## as an example, message headers may need to be generated before nodes # add_dependencies(wmrcontrol_node wmrcontrol_generate_messages_cpp)

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

############# ## Install ## #############

23

# 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 wmrcontrol wmrcontrol_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 \# PATTERN \# )

## 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_wmrcontrol.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)

##########################test talker and listener # add_executable(talker src/talker.cpp)

# target_link_libraries(talker ${catkin_LIBRARIES})

# add_dependencies(talker wmrcontrol_generate_messages_cpp) # add_executable(listener src/listener.cpp)

# target_link_libraries(listener ${catkin_LIBRARIES})

# add_dependencies(listener wmrcontrol_generate_messages_cpp)

################################ create the node rosaria_teleop_key对于第2个的键盘控制, 加入如下6句话 # find_package(PkgConfig REQUIRED)

# pkg_check_modules ( ncurses++ REQUIRED ncurses++)

# add_executable(rosaria_teleop_key src/rosaria_teleop_key.cpp) # target_link_libraries(rosaria_teleop_key ${catkin_LIBRARIES}) # target_link_libraries(rosaria_teleop_key ${ncurses++_LIBRARIES})

# add_dependencies(rosaria_teleop_key wmrcontrol_generate_messages_cpp)

############################ create the node rosaria_teleop_code

24

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

add_dependencies(rosaria_teleop_code wmrcontrol_generate_messages_cpp)

Images

//image_converter.cpp //包 cv_bridge仅有的一个程序

#include

#include #include

#include #include #include

static const std::string OPENCV_WINDOW = \;

class ImageConverter { ros::NodeHandle nh_; //在类下, 缺省值为private. image_transport::ImageTransport it_; //用来创建publisher或者subscriber image_transport::Subscriber image_sub_; //管理图像的订阅及其回调函数 image_transport::Publisher image_pub_;//管理图像话题的发布

public: ImageConverter(): it_(nh_) { // Subscrive to input video feed and publish output video feed image_sub_ = it_.subscribe(\, 1, &ImageConverter::imageCb, this); //对应ROS的订阅者//关联了回调函数

//\为Kinect输出彩色图像的Topic //关联了回调函数imageCb

//是不是也可以subscribe(\ image_pub_ = it_.advertise(\, 1); //没有指名发布的数据类型?? cv::namedWindow(OPENCV_WINDOW); } ~ImageConverter() { cv::destroyWindow(OPENCV_WINDOW); } void imageCb(const sensor_msgs::ImageConstPtr& msg)//Callback { cv_bridge::CvImagePtr cv_ptr; //定义了名字空间cv_bridge下类CvImagePtr的对象cv_ptr///////// 非指针 try { cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8); //将sensor_msgs::Image数据类型转换为cv::Mat类型///////////返回类型为CvImagePtr//非指针 //是BGR顺序, cvtColor中也是BGR顺序的 //:: :: 两个名字空间 //对象之间可以这样直接赋值??? 看看toCvCopy的源代码??? } catch (cv_bridge::Exception& e) { ROS_ERROR(\, e.what()); return;

25