内容要点:
练习与测试:
使用在习题2和习题3中实现的节点,并且新添加一个服务功能用于开启或停止机器人。此功能可以用作急停。
提示与流程:
在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行驶,图左上数值逐渐变小。
可选:
评分标准:
可选(附加分):
--5份习题完结--前4份习题说明链接如下:--
习题1:https://cloud.tencent.com/developer/article/1387009
习题2:https://cloud.tencent.com/developer/article/1387001
习题3:https://blog.csdn.net/zhangrelay/article/details/79956801
习题4:https://cloud.tencent.com/developer/article/1387031
课程资料全部文档:https://blog.csdn.net/zhangrelay/article/details/69382096
--End--