20210612, 调度流程 取车流程 完整版 debug

dispatch/dispatch_process.cpp

@@ -273,10 +273,18 @@ void Dispatch_process::Main()
 	{
 		std::this_thread::sleep_for(std::chrono::microseconds(1));
 		std::this_thread::sleep_for(std::chrono::milliseconds(1));
-//		std::this_thread::sleep_for(std::chrono::seconds(1));
+#ifdef TIME_TEST
+		std::this_thread::sleep_for(std::chrono::seconds(1));
+#endif
 //		std::cout << " huli test :::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::: " << " m_dispatch_process_status = " << m_dispatch_process_status << std::endl;
 
-		switch ( (Dispatch_process_status)m_dispatch_process_status )
+		std::shared_ptr<Dispatch_device_base> following_carrier = Dispatch_manager::get_instance_references().m_carrier_map[2];
+		Error_manager err =  following_carrier->check_task_level(Dispatch_device_base::Dispatch_task_level::DISPATCH_TASK_TWO_LEVEL);
+LOG(INFO) << " err =  "<<err.to_string()<< "   " << this;
+
+
+
+	switch ( (Dispatch_process_status)m_dispatch_process_status )
 		{
 			case DISPATCH_PROCESS_CREATED://流程创建,
 			{
@@ -902,7 +910,7 @@ Error_manager Dispatch_process::dispatch_control_motion_pickup()
 				{
 					//机器人移到42号口
 					float t_x = tp_dispatch_coordinates->m_carrier_coordinates_map[42].x;
-					catcher_move_x(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_avoid_catcher, tp_avoid_catcher_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_catcher_4th_floor_z);
+					catcher_move_x(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_avoid_catcher, tp_avoid_catcher_task, tp_dispatch_coordinates, t_x);
 					m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1);
 				}
 				//else 无限等待
@@ -917,7 +925,7 @@ Error_manager Dispatch_process::dispatch_control_motion_pickup()
 				{
 					//机器人移到34号口
 					float t_x = tp_dispatch_coordinates->m_carrier_coordinates_map[34].x;
-					catcher_move_x(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_avoid_catcher, tp_avoid_catcher_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_catcher_4th_floor_z);
+					catcher_move_x(get_dispatch_control_command_key(m_dispatch_carrier_node), tp_avoid_catcher, tp_avoid_catcher_task, tp_dispatch_coordinates, t_x);
 					m_dispatch_carrier_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_carrier_node.m_dispatch_control_status+1);
 				}
 				//else 无限等待
@@ -1509,25 +1517,26 @@ Error_manager Dispatch_process::dispatch_control_motion_pickup()
 
 	std::cout << " huli test :::: " << " ======================================================================================== = " << 1 << std::endl;
 	std::cout << " huli test :::: " << " m_dispatch_carrier_node.m_dispatch_control_status = " << m_dispatch_carrier_node.m_dispatch_control_status << std::endl;
-	if ( tp_main_carrier != NULL )
+	if ( m_dispatch_carrier_node.mp_main_carrier.get() != NULL )
 	{
-		std::cout << " huli test :::: " << " tp_main_carrier->get_device_id() = " << tp_main_carrier->get_device_id() << std::endl;
+		std::cout << " huli test :::: " << " tp_main_carrier->get_device_id() = " << m_dispatch_carrier_node.mp_main_carrier->get_device_id() << std::endl;
 	}
 	else
 	{
 		std::cout << " huli test :::: " << " tp_main_carrier->get_device_id() = " << -8888 << std::endl;
 	}
-	if ( tp_avoid_catcher != NULL )
+	if ( m_dispatch_carrier_node.mp_avoid_catcher.get() != NULL )
 	{
-		std::cout << " huli test :::: " << " tp_avoid_catcher->get_device_id() = " << tp_avoid_catcher->get_device_id() << std::endl;
+		std::cout << " huli test :::: " << " tp_avoid_catcher->get_device_id() = " << m_dispatch_carrier_node.mp_avoid_catcher->get_device_id() << std::endl;
 	}
 	else
 	{
 		std::cout << " huli test :::: " << " tp_avoid_catcher->get_device_id() = " << -8888 << std::endl;
 	}
-//	char cr;
-//	std::cin >> cr ;
-
+#ifdef PROCESS_TEST
+	char cr;
+	std::cin >> cr ;
+#endif
 
 
 	Error_manager t_catcher_error;
@@ -1955,7 +1964,7 @@ Error_manager Dispatch_process::dispatch_control_motion_pickup()
 				m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+1);
 			}
 			else if(m_dispatch_carrier_node.m_dispatch_control_status == Dispatch_process::Dispatch_control_status::DISPATCH_CARRIER_PICKUP_54 &&
-					Common_data::approximate_difference(tp_main_carrier->m_actual_x, tp_main_catcher->m_actual_x, tp_dispatch_coordinates->m_separated_distance_x) == false )
+					(Common_data::approximate_difference(tp_main_carrier->m_actual_x, tp_main_catcher->m_actual_x, tp_dispatch_coordinates->m_separated_distance_x) == false) )
 			{
 				m_dispatch_catcher_node.m_dispatch_control_status = (Dispatch_process::Dispatch_control_status)(m_dispatch_catcher_node.m_dispatch_control_status+1);
 			}
@@ -2084,17 +2093,17 @@ Error_manager Dispatch_process::dispatch_control_motion_pickup()
 
 	std::cout << " huli test :::: " << " ----------------------------------------------------------------------------------------- = " << 2 << std::endl;
 	std::cout << " huli test :::: " << " m_dispatch_catcher_node.m_dispatch_control_status = " << m_dispatch_catcher_node.m_dispatch_control_status << std::endl;
-	if ( tp_main_catcher != NULL )
+	if ( m_dispatch_catcher_node.mp_main_catcher.get() != NULL )
 	{
-		std::cout << " huli test :::: " << " tp_main_catcher->get_device_id() = " << tp_main_catcher->get_device_id() << std::endl;
+		std::cout << " huli test :::: " << " tp_main_catcher->get_device_id() = " << m_dispatch_catcher_node.mp_main_catcher->get_device_id() << std::endl;
 	}
 	else
 	{
 		std::cout << " huli test :::: " << " tp_main_catcher->get_device_id() = " << -8888 << std::endl;
 	}
-	if ( tp_following_carrier != NULL )
+	if ( m_dispatch_catcher_node.mp_following_carrier.get() != NULL )
 	{
-		std::cout << " huli test :::: " << " tp_following_carrier->get_device_id() = " << tp_following_carrier->get_device_id() << std::endl;
+		std::cout << " huli test :::: " << " tp_following_carrier->get_device_id() = " << m_dispatch_catcher_node.mp_following_carrier->get_device_id() << std::endl;
 	}
 	else
 	{
@@ -2104,14 +2113,17 @@ Error_manager Dispatch_process::dispatch_control_motion_pickup()
 	std::cout << " huli test :::: " << " +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ = " << 3 << std::endl;
 	std::cout << " huli test :::: " << " m_parkspace_information_optimal.parkingspace_index_id = " << m_parkspace_information_optimal.parkingspace_index_id << std::endl;
 	std::cout << " huli test :::: " << " m_dispatch_destination = " << m_dispatch_destination << std::endl;
-	
-//	char ch;
-//	std::cin >> ch ;
 
+#ifdef PROCESS_TEST
+	char ch;
+	std::cin >> ch ;
+#endif
 
 	//结果汇总
 	if ( m_dispatch_carrier_node.m_dispatch_control_status == DISPATCH_CARRIER_PICKUP_END &&
-		 m_dispatch_catcher_node.m_dispatch_control_status == DISPATCH_CATCHER_PICKUP_END)
+		 m_dispatch_catcher_node.m_dispatch_control_status == DISPATCH_CATCHER_PICKUP_END &&
+		 m_dispatch_carrier_node.m_error == Error_code::SUCCESS &&
+		 m_dispatch_catcher_node.m_error == Error_code::SUCCESS)
 	{
 		return Error_code::SUCCESS;
 	}
@@ -2294,7 +2306,7 @@ Error_manager Dispatch_process::connect_dispatch_catcher(std::shared_ptr<Dispatc
 			tp_catcher_task->m_request_wheelbase = Dispatch_coordinates::get_instance_references().m_default_wheelbase;
 			tp_catcher_task->m_request_clamp_motion = (Catcher_task::Clamp_motion)tp_catcher->m_actual_clamp_motion1;
 
-			t_error = tp_catcher->execute_task(m_dispatch_catcher_node.mp_main_catcher_task, dispatch_task_level);
+			t_error = tp_catcher->execute_task(p_catcher_task, dispatch_task_level);
 		}
 	}
 	else

dispatch/dispatch_process.h

@@ -39,6 +39,11 @@
 #include "../tool/point3D_tool.h"
 #include <atomic>
 
+//#define MAIN_TEST 1
+//#define PROCESS_TEST 1
+#define TIME_TEST 1
+
+
 //调度流程 存车
 class Dispatch_process : public tq::BaseTask
 {

main.cpp

@@ -92,7 +92,7 @@ int main(int argc,char* argv[])
 	FLAGS_stop_logging_if_full_disk = true;
 
 
-#define MAIN_TEST 1
+//#define MAIN_TEST 1
 #ifdef MAIN_TEST
 	t_error = Dispatch_coordinates::get_instance_references().dispatch_coordinates_init();
 	std::cout << " huli test :::: " << " Dispatch_coordinates::get_instance_references().dispatch_coordinates_init() = " << t_error.to_string() << std::endl;
@@ -180,7 +180,7 @@ int main(int argc,char* argv[])
 	Catcher_task * tp_catcher_task1 = (Catcher_task *)tp_task_Base1.get();
 	tp_catcher_task1->task_init(NULL,std::chrono::milliseconds(15000));
 	tp_catcher_task1->m_request_key = "x23456789012345678901234567890r1";
-	tp_catcher_task1->m_request_x = 24855;
+	tp_catcher_task1->m_request_x = 17955;
 	tp_catcher_task1->m_request_y = 3000;
 	tp_catcher_task1->m_request_b = 90;
 	tp_catcher_task1->m_request_z = 5410;
@@ -463,7 +463,7 @@ int main(int argc,char* argv[])
 	Carrier_task * tp_carrier_task22 = (Carrier_task *)tp_task_Base22.get();
 	tp_carrier_task22->task_init(NULL,std::chrono::milliseconds(15000));
 	tp_carrier_task22->m_request_key = "x23456789012345678901234567890c2";
-	tp_carrier_task22->m_request_x = 24855;
+	tp_carrier_task22->m_request_x = 17955;
 //	tp_carrier_task22->m_request_x = 4700;
 	tp_carrier_task22->m_request_y = 3000;
 	tp_carrier_task22->m_request_z = 5125;