| 169 | |
| 170 | |
| 171 | int main(int argc, char **argv) |
| 172 | { |
| 173 | ros::init(argc, argv, "kcf_tracker"); |
| 174 | ros::NodeHandle nh("~"); |
| 175 | image_transport::ImageTransport it(nh); |
| 176 | ros::Rate loop_rate(30); |
| 177 | |
| 178 | // 发布调试消息 |
| 179 | msg_node_name = "/prometheus/message/kcf_tracker"; |
| 180 | message_pub = nh.advertise<prometheus_msgs::Message>(msg_node_name, 10); |
| 181 | |
| 182 | std::string camera_topic, camera_info; |
| 183 | if (nh.getParam("camera_topic", camera_topic)) { |
| 184 | if (local_print) |
| 185 | ROS_INFO("camera_topic is %s", camera_topic.c_str()); |
| 186 | if (message_print) |
| 187 | pub_message(message_pub, prometheus_msgs::Message::NORMAL, msg_node_name, "camera_topic is" + camera_topic); |
| 188 | } else { |
| 189 | if (local_print) |
| 190 | ROS_WARN("didn't find parameter camera_topic"); |
| 191 | if (message_print) |
| 192 | pub_message(message_pub, prometheus_msgs::Message::WARN, msg_node_name, "didn't find parameter camera_topic"); |
| 193 | camera_topic = "/prometheus/camera/rgb/image_raw"; |
| 194 | } |
| 195 | |
| 196 | if (nh.getParam("camera_info", camera_info)) { |
| 197 | if (local_print) |
| 198 | ROS_INFO("camera_info is %s", camera_info.c_str()); |
| 199 | if (message_print) |
| 200 | pub_message(message_pub, prometheus_msgs::Message::NORMAL, msg_node_name, "camera_info is" + camera_info); |
| 201 | } else { |
| 202 | if (local_print) |
| 203 | ROS_WARN("didn't find parameter camera_info"); |
| 204 | if (message_print) |
| 205 | pub_message(message_pub, prometheus_msgs::Message::WARN, msg_node_name, "didn't find parameter camera_info"); |
| 206 | camera_info = "camera_param.yaml"; |
| 207 | } |
| 208 | |
| 209 | bool switch_state = is_suspanded; |
| 210 | // 接收开关话题 |
| 211 | switch_subscriber = nh.subscribe("/prometheus/switch/kcf_tracker", 10, switchCallback); |
| 212 | |
| 213 | // 接收图像的话题 |
| 214 | imageSubscriber_ = it.subscribe(camera_topic.c_str(), 1, cameraCallback); |
| 215 | |
| 216 | // 跟踪结果，xyz |
| 217 | pose_pub = nh.advertise<prometheus_msgs::DetectionInfo>("/prometheus/object_detection/kcf_tracker", 1); |
| 218 | |
| 219 | sensor_msgs::ImagePtr msg_ellipse; |
| 220 | |
| 221 | std::string ros_path = ros::package::getPath("prometheus_detection"); |
| 222 | if (local_print) |
| 223 | cout << "DETECTION_PATH: " << ros_path << endl; |
| 224 | if (message_print) |
| 225 | pub_message(message_pub, prometheus_msgs::Message::NORMAL, msg_node_name, "DETECTION_PATH: " + ros_path); |
| 226 | // 读取参数文档camera_param.yaml中的参数值； |
| 227 | YAML::Node camera_config = YAML::LoadFile(camera_info); |
| 228 | // 相机内部参数 |
nothing calls this directly
no test coverage detected