cartographer如何使用3个反光柱作为一个landmark

107 篇文章 76 订阅

3个反光柱的摆放规则：摆成一个直角三角形或者直线或其他特征 ，直角边不同即可，可以设置边的长度来作为附加特征。

talk is cheap, show your code.

        if (reflector_combination_mode == 3 && reflectors_.size() == 3)
{
//log(Info, "reflectors_.size() = " + std::to_string(reflectors_.size()));
Reflector_pos item_j, item_k;

for (int i = 0; i < reflectors_.size(); i++)
{
Reflector_pos item_i = reflectors_[i];

//log(Info, "i = " + std::to_string(i));
switch (i)
{
case 0 :
//log(Info, "-------------------------vetex is 0 --------------------------");
item_j = reflectors_[1];
item_k = reflectors_[2];
break;
case 1 :
//log(Info, "-------------------------vetex is 1 --------------------------");
item_j = reflectors_[0];
item_k = reflectors_[2];
break;
case 2 :
//log(Info, "-------------------------vetex is 2 --------------------------");
item_j = reflectors_[0];
item_k = reflectors_[1];
break;
}

double theta_1 = atan2(item_i.center_y - item_j.center_y, item_i.center_x - item_j.center_x);
//log(Info, "theta_1 is " + std::to_string(theta_1));
double theta_2 = atan2(item_i.center_y - item_k.center_y, item_i.center_x - item_k.center_x);
//log(Info, "theta_2 is " + std::to_string(theta_2));

//degree detect error is not less 20
double f_theta = fabs(theta_1 - theta_2);
//log(Info, "f_theta is " + std::to_string(f_theta));
//log(Info, "fabs(f_theta - M_PI/2) = " + std::to_string(fabs(f_theta - M_PI/2)) );
if (fabs(f_theta - M_PI/2) > M_PI/9) continue;

//log(Info, "i = " + std::to_string(i) + " satisfy condition");
//if satisfy condition (less 20) then
double x_ij = fabs(item_i.center_x - item_j.center_x);
double y_ij = fabs(item_i.center_y - item_j.center_y);
double distance_ij = sqrt(pow(x_ij, 2) + pow(y_ij, 2));
//log(Info, "distance_ij is = " + std::to_string(distance_ij));
double x_ik = fabs(item_i.center_x - item_k.center_x);
double y_ik = fabs(item_i.center_y - item_k.center_y);
double distance_ik = sqrt(pow(x_ik, 2) + pow(y_ik, 2));
//log(Info, "distance_ik is = " + std::to_string(distance_ik));

double long_distance = 0;
if (distance_ij > distance_ik)
{
//ij is the axis x
long_distance = distance_ij;
origin_theta = atan2(1, 0) - atan2(item_i.center_y - item_j.center_y, item_i.center_x - item_j.center_x);
//log(Info, "origin_theta = " + std::to_string(origin_theta));
}
else
{
//ik is the axis_x
long_distance = distance_ik;
origin_theta = atan2(1, 0) - atan2(item_i.center_y - item_k.center_y, item_i.center_x - item_k.center_x);
//log(Info, "origin_theta = " + std::to_string(origin_theta));
}

//the long side of right angle is even number;
if(round_int(long_distance) % 2 != 0) return;
//log(Info, "long_distance = " + std::to_string(long_distance));
origin_x = item_i.center_x;
origin_y = item_i.center_y;
//log(Info, "origin_x = " + std::to_string(origin_x));
//log(Info, "origin_y = " + std::to_string(origin_y));
}
}

//主要调试下面这句来约束反光住landmark的构造模式
double distance = sqrt(pow(origin_x, 2) + pow(origin_y, 2));
if (distance < 1.0) return;
//
cartographer_ros_msgs::LandmarkEntry reflector_LandMarkEntry;
reflector_LandMarkEntry.id = "landmark_1";
reflector_LandMarkEntry.tracking_from_landmark_transform.position.x = origin_x;
reflector_LandMarkEntry.tracking_from_landmark_transform.position.y = origin_y;
reflector_LandMarkEntry.tracking_from_landmark_transform.position.z = 0;
tf::Quaternion quat = tf::createQuaternionFromYaw(-1.0 * origin_theta);
reflector_LandMarkEntry.tracking_from_landmark_transform.orientation.x = quat.x();
reflector_LandMarkEntry.tracking_from_landmark_transform.orientation.y = quat.y();
reflector_LandMarkEntry.tracking_from_landmark_transform.orientation.z = quat.z();
reflector_LandMarkEntry.tracking_from_landmark_transform.orientation.w = quat.w();
reflector_LandMarkEntry.translation_weight = 1.0;
reflector_LandMarkEntry.rotation_weight = 1.0;
reflector_LandMarkEntry.type = "reflector_combined";

reflector_LandMarkList.landmarks.push_back(reflector_LandMarkEntry);


<launch>

<param name="/use_sim_time" value="true" />

<param name="robot_description"
textfile="$(find cartographer_ros)/urdf/backpack_2d_laser_test.urdf" /> <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" /> <node pkg="laser_reflector_detect" type="laser_reflector_detect_node" name="laser_reflector_detect_node" output="screen" > <rosparam file="$(find laser_reflector_detect)/param/laser_reflector_detect.yaml"/>
</node>

<node name="rviz" pkg="rviz" type="rviz" required="true"
args="-d $(find cartographer_ros)/configuration_files/demo_2d_okagv_laser_test.rviz" /> <node name="playbag" pkg="rosbag" type="play" args="--clock -r 1$(arg bag_filename)" />

</launch>

• 6
点赞
• 23
收藏
• 打赏
• 8
评论
09-11
02-13 545
03-29 5668
06-28 904
12-05 502
10-08 533
09-04 2926
10-19 604
12-27 2633

“相关推荐”对你有帮助么？

• 非常没帮助
• 没帮助
• 一般
• 有帮助
• 非常有帮助

¥2 ¥4 ¥6 ¥10 ¥20

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、C币套餐、付费专栏及课程。