沉默杀手HomeArchivesTagsCategories
编程技巧
|C++C++ 基础|
Word count: 1k|Reading time: 4 min

对浮点数进行四舍五入

1
2
3
4
int getEstimate(float a)
{
	return int(a + 0.5);
}

double 只取小数点后两位

1
2
double d = 0.2500000500001;
double b = floor(d * 10000.000f + 0.5) / 10000.000f;

回调函数的降频功能

这是从gmapping里学的，cartographer里也有，但是更复杂了

1
2
3
4
5
6
7
8
9
unsigned int laser_count_ = 0;
int throttle_scans_ = 5;  // 自定义的值
SlamGMapping::laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
{
  laser_count_++;
  if ((laser_count_ % throttle_scans_) != 0)  // 判断是否降频
    return;
  // ... 主要代码 ...
}

在终端显示带颜色和风格的文字

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
// color指定颜色BLACK, RED, GREEN, YELLOW, BLUE, WHITE, option指定文字风格BOLD, REGULAR, UNDERLINE.
const std::string RESET = "\033[0m";
const std::string BLACK = "0m";
const std::string RED = "1m";
const std::string GREEN = "2m";
const std::string YELLOW = "3m";
const std::string BLUE = "4m";
const std::string WHITE = "7m";

const std::string BOLD = "\033[1;3";
const std::string REGULAR = "\033[0;3";
const std::string UNDERLINE = "\033[4;3";
const std::string BACKGROUND = "\033[4";

std::string colouredString(std::string str, std::string colour, std::string option)
{
  double time_now = ros::Time::now().toSec();
  std::string time_string = std::to_string(time_now);
  return "[" + time_string + "]: " + option + colour + str + RESET;
}

生成极值

C++11中，std::numeric_limits为模板类，在库编译平台提供基础算术类型的极值等属性信息，取代传统C语言，所采用的预处理常数。比较常用的使用是对于给定的基础类型用来判断在当前系统上的最大值、最小值。需包含头文件。

std::numeric_limits<double>::max();的结果是一个很大的数字

数学函数

  • hypot()用来求三角形的斜边长，其原型为：double hypot(double x, double y);，需要#include <stdio.h>

  • fabs函数是求绝对值的函数，函数原型是extern float fabs(float x)，需要#include <math.h>

对double/float数据，一定要使用fabs函数。如果用了abs，就会出现bug，因为返回也是int

反正切函数求角度

atan2返回给定的 X 及 Y 坐标值的反正切值。反正切的角度值等于 X 轴与通过原点和给定坐标点 (Y坐标, X坐标) 的直线之间的夹角。结果以弧度表示并介于-pi到pi之间（不包括-pi）。

而atan(a/b)的取值范围介于-pi/2到pi/2之间，不包括±pi/2

生成27个三维向量，每个可取-1,0,1，一共27种组合

1
2
3
4
5
6
for (i = 0; i < 3 * 3 * 3; i++)
{
	int a = (i / 9) - 1;
    int b = ((i % 9) / 3) - 1;
    int c = ((i % 9) % 3) - 1;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
int killNode(std::string nodeName)
{
  pid_t status;
  string cmd = "rosnode kill "+nodeName;
  status = system(cmd.data());
  if (-1 == status)
  {
    return -1;  // system func error
  }
  else
  {
    if (WIFEXITED(status))    //返回一个非零值, 正常退出
    {
      ROS_INFO("child process exit done: %d", WEXITSTATUS(status) );
      return WEXITSTATUS(status);   // rosnode kill done, exit status value
    }
    else
    {
      ROS_WARN("child process exit abnormally");
      return -3;
    }
  }
}

string getCmdResult(const string &strCmd)
{
  char buf[10240] = {0};
  FILE *pf = NULL;
  if( (pf = popen(strCmd.c_str(), "r")) == NULL )
  {
    return "";
  }
  string strResult;
  while(fgets(buf, sizeof buf, pf))
  {
    strResult += buf;
  }
  pclose(pf);
 
  unsigned int iSize =  strResult.size();
  if(iSize > 0 && strResult[iSize - 1] == '\n')  // linux
  {
    strResult = strResult.substr(0, iSize - 1);
  }
  return strResult;
}

// ros::Time 转为年月日的string
std::string stampToString(const ros::Time& stamp, const std::string format="%Y-%m-%d %H:%M:%S")
{
  const int output_size = 100;
  char output[output_size];
  std::time_t raw_time = static_cast<time_t>(stamp.sec);
  struct tm* timeinfo = localtime(&raw_time);
  std::strftime(output, output_size, format.c_str(), timeinfo);
  std::stringstream ss; 
  ss << std::setw(9) << std::setfill('0') << stamp.nsec;  
  const size_t fractional_second_digits = 4;
  return std::string(output) + "." + ss.str().substr(0, fractional_second_digits);
}

tf2_ros::Buffer buff;
tf2_ros::TransformListener listener(buff);

geometry_msgs::PoseStamped  point_in, point_out;
point_in.header.frame_id = "laser_link";
point_in.header.stamp = ros::Time(0);
point_in.pose =    //赋值
// buff跟上面的使用一样， 返回的point_out是point_in在 parent坐标系 的坐标
point_out = buff.transform(point_in, point_out, "map", ros::Duration(0.3) );

获得Linux命令的结果

比如输入ls，返回当前目录的文件名

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
string getCmdResult(const string &strCmd)
{
  char buf[10240] = {0};
  FILE *pf = NULL;
  if( (pf = popen(strCmd.c_str(), "r")) == NULL )
  {
    return "";
  }
  string strResult;
  while(fgets(buf, sizeof buf, pf))
  {
    strResult += buf;
  }
  pclose(pf);
 
  unsigned int iSize =  strResult.size();
  if(iSize > 0 && strResult[iSize - 1] == '\n')  // linux
  {
    strResult = strResult.substr(0, iSize - 1);
  }
  return strResult;
}