// 渐变色函数
vector<int> getColor(int value){
vector<int> startColor{0,255,0};
vector<int> endColor{255,255,0};
if(value >= 255)
return endColor;
else if(value <= 0)
return startColor;
int r_gap=endColor[0]-startColor[0];
int g_gap=endColor[1]-startColor[1];
int b_gap=endColor[2]-startColor[2];
// int nSteps = max(abs(r), max(abs(g), abs(b)));
// if (nSteps < 1) nSteps = 1;
int nSteps = 255;
// Calculate the step size for each color
float rStep=r_gap/(float)nSteps;
float gStep=g_gap/(float)nSteps;
float bStep=b_gap/(float)nSteps;
// Reset the colors to the starting position
float rStart=startColor[0];
float gStart=startColor[1];
float bStart=startColor[2];
// float step = (value - 255)/255;
int step = value;
int r = rStart + r_gap * value / 255;
int g = gStart + g_gap * value / 255;
int b = bStart + b_gap * value / 255;
float a = value / 255;
cout << r << " " << g << " " << b << endl;
return vector<int>{r,g,b,a};
// return vector<int>{(int)(rStart+rStep*step+0.5),(int)(gStart+gStep*step+0.5),(int)(bStart+bStep*step+0.5)};
}
后来发现在rviz中只有绿黄两种颜色,应该是rviz不支持渐变色。
解决方案:
1.用点云pointcloud
sensor_msgs::PointCloud cloud;
int num_points = cost_map.size[0]*cost_map.size[1]*cost_map.size[2];
cloud.header.stamp = ros::Time::now();
cloud.header.frame_id = "map";//填充 PointCloud 消息的头:frame 和 timestamp.
cloud.points.resize(num_points);//设置点云的数量.
//增加信道 "intensity" 并设置其大小,使与点云数量相匹配.
cloud.channels.resize(1);
cloud.channels[0].name = "intensities";
cloud.channels[0].values.resize(num_points);
//使用虚拟数据填充 PointCloud 消息.同时,使用虚拟数据填充 intensity 信道.
int i = 0;
for (int row = 0; row < cost_map.size[0]; ++row)
for (int col = 0; col < cost_map.size[1]; ++col)
for (int hei = 0;hei < cost_map.size[2]; ++ hei){
cloud.points[i].x = camera_pose.position.x + (row - cam_posid[0]) * resolution;
cloud.points[i].y = camera_pose.position.y + (col - cam_posid[1]) * resolution;
cloud.points[i].z = camera_pose.position.z + (hei - cam_posid[2]) * resolution;
int cur_cost = cost_map.at<float>(row, col, hei);
cloud.channels[0].values[i] = cur_cost;
i++;
}
costcloud_pub.publish(cloud);
2.渐变色中加入更多颜色变化,value值的变化表现在marker.color.alpha上