内容要点:
- ROS服务调用(Service calls)
练习与测试:
使用在习题2和习题3中实现的节点,并且新添加一个服务功能用于开启或停止机器人。此功能可以用作急停。
- 实现此功能参考(第4讲,第8页PPT),此任务中使用 std_srvs/SetBool 服务类型。
- 启动仿真并且调用服务在终端使用 rosservice call 启动或停止机器人。
提示与流程:
在HuskyHighlevelController.hpp中对应的位置添加如下代码:
#include <std_srvs/SetBool.h> bool srvCallback(std_srvs::SetBool::Request &request, std_srvs::SetBool::Response &response); ros::ServiceServer serviceServer_;
HuskyHighlevelController.cpp代码如下所示:
#include "husky_highlevel_controller/HuskyHighlevelController.hpp" namespace husky_highlevel_controller { HuskyHighlevelController::HuskyHighlevelController(ros::NodeHandle& nodeHandle) : nodeHandle_(nodeHandle) { if (!readParameters()) { ROS_ERROR("Could not read parameters."); ros::requestShutdown(); } // subscribers scan_sub_ = nodeHandle_.subscribe(subscriberTopic_, queue_size , &HuskyHighlevelController::scanCallback, this); // publishers cmd_pub_ = nodeHandle_.advertise<geometry_msgs::Twist>("/cmd_vel",10); vis_pub_ = nodeHandle_.advertise<visualization_msgs::Marker>("/visualization_marker",10); // service_server serviceServer_ = nodeHandle_.advertiseService("husky_start_move", &HuskyHighlevelController::srvCallback, this); ROS_INFO("Successfully launched node."); } HuskyHighlevelController::~HuskyHighlevelController() { } bool HuskyHighlevelController::readParameters() { if (!nodeHandle_.getParam("scan_sub_topic", subscriberTopic_)) { ROS_ERROR("Could not find scan_sub_topic parameter!"); return false; } if (!nodeHandle_.getParam("scan_sub_queue_size", queue_size)) { ROS_ERROR("Could not find scan_sub_queue_size parameter!"); return false; } return true; } void HuskyHighlevelController::scanCallback(const sensor_msgs::LaserScan &scan_msg) { float smallest_distance = 1000; // the angle corresponding to the minimum distance //number of the elements in ranges array int arr_size = floor((scan_msg.angle_max-scan_msg.angle_min)/scan_msg.angle_increment); for (int i=0 ; i< arr_size ;i++) { if (scan_msg.ranges[i] < smallest_distance) { smallest_distance = scan_msg.ranges[i]; alpha_pillar = (scan_msg.angle_min + i*scan_msg.angle_increment); } } //Pillar Husky offset pose x_pillar = smallest_distance*cos(alpha_pillar); y_pillar = smallest_distance*sin(alpha_pillar); //cout<<"cout Minimum laser distance(m): "<<smallest_distance<<"\n"; //ROS_INFO_STREAM("ROS_INFO_STREAM Minimum laser distance(m): "<<smallest_distance); //ROS_INFO("Pillar laser distance(m):%lf", smallest_distance); ROS_INFO("Pillar offset angle(rad):%lf", alpha_pillar); ROS_INFO("pillar x distance(m):%lf", x_pillar); ROS_INFO("pillar y distance(m):%lf", y_pillar); //P-Controller to drive husky towards the pillar //propotinal gain float p_gain_vel = 0.1; float p_gain_ang = 0.4; if( start_move && x_pillar>0.2 ) { if (x_pillar <= 0.4 ) { vel_msg_.linear.x = 0; vel_msg_.angular.z = 0; } else { // if(start_move) // { vel_msg_.linear.x = x_pillar * p_gain_vel ; vel_msg_.angular.z = -(y_pillar * p_gain_ang) ; // } // if(start_move==false) // { // vel_msg_.linear.x = 0; // vel_msg_.angular.z = 0; // } } } else { vel_msg_.linear.x = 0; vel_msg_.angular.z = 0; } cmd_pub_.publish(vel_msg_); //RViz Marker marker.header.frame_id = "base_laser"; //base no marker.header.stamp = ros::Time(); marker.ns = "pillar"; marker.id = 0; marker.type = visualization_msgs::Marker::SPHERE; marker.action = visualization_msgs::Marker::ADD; marker.pose.position.x = x_pillar; marker.pose.position.y = y_pillar; marker.scale.x = 0.2; marker.scale.y = 0.2; marker.scale.z = 2.0; marker.color.a = 1.0; // Don't forget to set the alpha! marker.color.r = 0.1; marker.color.g = 0.1; marker.color.b = 0.1; vis_pub_.publish(marker); } bool HuskyHighlevelController::srvCallback(std_srvs::SetBool::Request &request, std_srvs::SetBool::Response &response) { // try{ if (request.data) { start_move = true; } else { start_move = false; } response.success = true; // } // catch(...){ // ROS_WARN("Impossible to execute Start-Move service"); // ros::Duration(1.0).sleep(); // response.success = false; // //continue; // } ROS_INFO("request: %i", request.data ); ROS_INFO("sending back response: [%i]", response.success); return true; } /* void HuskyHighlevelController::pController() { //propotinal gain float p_gain_vel = 0.4; float p_gain_ang = 1; if(start_move) { if (x_pillar <= 0.4 ) { vel_msg_.linear.x = 0; vel_msg_.angular.z =0; } else { vel_msg_.linear.x = x_pillar * p_gain_vel ; vel_msg_.angular.z = -alpha_pillar ; } } else { vel_msg_.linear.x = 0; vel_msg_.angular.z =0; } } */ } /* namespace */
代码仅供参考,实现效果如下图:
Husky停止,图左上数值几乎不变。
Husky行驶,图左上数值逐渐变小。
可选:
- 创建一个独立的节点,使用激光传感器测距,当机器人非常靠近障碍物时停止。
- 创建一个独立的节点,在出现意外或停止服务启动时,急停机器人。使用让rqt_multiplot绘制主题/imu/data的数据,并做分析,同时开发一种检测碰撞的方法。
评分标准:
- 使用服务调用方式,停止Husky。(50%)
- 使用服务调用方式,开启Husky。(50%)
可选(附加分):
- 自动触发急停当机器人非常靠近一个障碍物。(25%)
- 自动触发急停当机器人意外撞到障碍物。(25%)
--5份习题完结--前4份习题说明链接如下:--
习题1:https://blog.csdn.net/ZhangRelay/article/details/79463992
习题2:https://blog.csdn.net/ZhangRelay/article/details/79627591
习题3:https://blog.csdn.net/zhangrelay/article/details/79956801
习题4:https://blog.csdn.net/ZhangRelay/article/details/79968374
课程资料全部文档:https://blog.csdn.net/zhangrelay/article/details/69382096
--End--