diff --git a/src/ros2/TEMPLATE_ros_config.vhd b/src/ros2/TEMPLATE_ros_config.vhd index 2af3a85..377d2b3 100644 --- a/src/ros2/TEMPLATE_ros_config.vhd +++ b/src/ros2/TEMPLATE_ros_config.vhd @@ -72,7 +72,24 @@ package ros_config is -- TODO constant ROS_ACTIONS : ROS_ACTION_ARRAY_TYPE(0 to NUM_ACTIONS-1) := ( others => ( - node_id => 0 + node_id => 0, + ACTIONNAME => gen_user_string("Action1"), + GOAL_RQ_TYPENAME => gen_user_string("Type4_SendGoal_Request"), + GOAL_RR_TYPENAME => gen_user_string("Type4_SendGoal_Response"), + RESULT_RQ_TYPENAME => gen_user_string("Type4_GetResult_Request"), + RESULT_RR_TYPENAME => gen_user_string("Type4_GetResult_Response"), + FEEDBACK_TYPENAME => gen_user_string("Type4_FeedbackMessage"), + GOAL_QOS => ROS_QOS_PROFILE_SERVICES_DEFAULT, + RESULT_QOS => ROS_QOS_PROFILE_SERVICES_DEFAULT, + FEEDBACK_QOS => ROS_QOS_PROFILE_DEFAULT, + CANCEL_QOS => ROS_QOS_PROFILE_SERVICES_DEFAULT, + MAX_GOAL_RQ_SIZE => 10, + MAX_GOAL_RR_SIZE => 10, + MAX_RESULT_RQ_SIZE => 10, + MAX_RESULT_RR_SIZE => 10, + MAX_FEEDBACK_SIZE => 10, + is_client => FALSE, + enable_feedback => TRUE ) ); diff --git a/src/ros2/Tests/Level_1/L1_Fibonacci_ros_action_test1.vhd b/src/ros2/Tests/Level_1/L1_Fibonacci_ros_action_test1.vhd index 60c96b7..f9832e2 100644 --- a/src/ros2/Tests/Level_1/L1_Fibonacci_ros_action_test1.vhd +++ b/src/ros2/Tests/Level_1/L1_Fibonacci_ros_action_test1.vhd @@ -54,7 +54,7 @@ architecture testbench of L1_Fibonacci_ros_action_test1 is signal last_word_gr, last_word_gw, last_word_rr, last_word_rw, last_word_cr, last_word_cw, last_word_f : std_logic := '0'; signal data_gr, data_gw, data_rr, data_rw, data_cr, data_cw, data_f : std_logic_vector(WORD_WIDTH-1 downto 0) := (others => '0'); - signal start_c, start_s, ack_c, ack_s, done_c, done_s : std_logic := '0'; + signal start_c, start_s, ack_c, ack_s, done_c, done_s, data_available_c_c, data_available_g_c, data_available_r_c, data_available_f_c : std_logic := '0'; signal opcode_c, opcode_s : ROS_ACTION_OPCODE_TYPE := NOP; signal return_code_c, return_code_s : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := (others => '0'); -- ###GENERATED START### @@ -222,6 +222,10 @@ begin ack => ack_c, sequence_id => sequence_id_c, service_info => service_info_c, + data_available_g => data_available_g_c, + data_available_r => data_available_r_c, + data_available_c => data_available_c_c, + data_available_f => data_available_f_c, done => done_c, return_code => return_code_c, goal_id => goal_id_c, diff --git a/src/ros2/Tests/Level_1/L1_Fibonacci_ros_action_test2.vhd b/src/ros2/Tests/Level_1/L1_Fibonacci_ros_action_test2.vhd index 5542802..cae3bd8 100644 --- a/src/ros2/Tests/Level_1/L1_Fibonacci_ros_action_test2.vhd +++ b/src/ros2/Tests/Level_1/L1_Fibonacci_ros_action_test2.vhd @@ -54,7 +54,7 @@ architecture testbench of L1_Fibonacci_ros_action_test2 is signal last_word_gr, last_word_gw, last_word_rr, last_word_rw, last_word_cr, last_word_cw, last_word_f : std_logic := '0'; signal data_gr, data_gw, data_rr, data_rw, data_cr, data_cw, data_f : std_logic_vector(WORD_WIDTH-1 downto 0) := (others => '0'); - signal start_c, start_s, ack_c, ack_s, done_c, done_s : std_logic := '0'; + signal start_c, start_s, ack_c, ack_s, done_c, done_s, data_available_c_c, data_available_g_c, data_available_r_c, data_available_f_c : std_logic := '0'; signal opcode_c, opcode_s : ROS_ACTION_OPCODE_TYPE := NOP; signal return_code_c, return_code_s : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := (others => '0'); -- ###GENERATED START### @@ -225,6 +225,10 @@ begin ack => ack_c, sequence_id => sequence_id_c, service_info => service_info_c, + data_available_g => data_available_g_c, + data_available_r => data_available_r_c, + data_available_c => data_available_c_c, + data_available_f => data_available_f_c, done => done_c, return_code => return_code_c, goal_id => goal_id_c, diff --git a/src/ros2/Tests/Level_2/L2_Testbench_ROS_Lib2_config.vhd b/src/ros2/Tests/Level_2/L2_Testbench_ROS_Lib2_config.vhd index 6bb3293..78d0640 100644 --- a/src/ros2/Tests/Level_2/L2_Testbench_ROS_Lib2_config.vhd +++ b/src/ros2/Tests/Level_2/L2_Testbench_ROS_Lib2_config.vhd @@ -72,7 +72,24 @@ package ros_config is constant ROS_ACTIONS : ROS_ACTION_ARRAY_TYPE(0 to NUM_ACTIONS-1) := ( others => ( - node_id => 0 + node_id => 0, + ACTIONNAME => gen_user_string(""), + GOAL_RQ_TYPENAME => gen_user_string(""), + GOAL_RR_TYPENAME => gen_user_string(""), + RESULT_RQ_TYPENAME => gen_user_string(""), + RESULT_RR_TYPENAME => gen_user_string(""), + FEEDBACK_TYPENAME => gen_user_string(""), + GOAL_QOS => ROS_QOS_PROFILE_SERVICES_DEFAULT, + RESULT_QOS => ROS_QOS_PROFILE_SERVICES_DEFAULT, + FEEDBACK_QOS => ROS_QOS_PROFILE_DEFAULT, + CANCEL_QOS => ROS_QOS_PROFILE_SERVICES_DEFAULT, + MAX_GOAL_RQ_SIZE => 0, + MAX_GOAL_RR_SIZE => 0, + MAX_RESULT_RQ_SIZE => 0, + MAX_RESULT_RR_SIZE => 0, + MAX_FEEDBACK_SIZE => 0, + is_client => FALSE, + enable_feedback => TRUE ) ); diff --git a/src/ros2/Tests/Level_2/L2_Testbench_ROS_Lib3_config.vhd b/src/ros2/Tests/Level_2/L2_Testbench_ROS_Lib3_config.vhd index 5a9d8e5..56cdbb9 100644 --- a/src/ros2/Tests/Level_2/L2_Testbench_ROS_Lib3_config.vhd +++ b/src/ros2/Tests/Level_2/L2_Testbench_ROS_Lib3_config.vhd @@ -72,7 +72,24 @@ package ros_config is constant ROS_ACTIONS : ROS_ACTION_ARRAY_TYPE(0 to NUM_ACTIONS-1) := ( others => ( - node_id => 0 + node_id => 0, + ACTIONNAME => gen_user_string(""), + GOAL_RQ_TYPENAME => gen_user_string(""), + GOAL_RR_TYPENAME => gen_user_string(""), + RESULT_RQ_TYPENAME => gen_user_string(""), + RESULT_RR_TYPENAME => gen_user_string(""), + FEEDBACK_TYPENAME => gen_user_string(""), + GOAL_QOS => ROS_QOS_PROFILE_SERVICES_DEFAULT, + RESULT_QOS => ROS_QOS_PROFILE_SERVICES_DEFAULT, + FEEDBACK_QOS => ROS_QOS_PROFILE_DEFAULT, + CANCEL_QOS => ROS_QOS_PROFILE_SERVICES_DEFAULT, + MAX_GOAL_RQ_SIZE => 0, + MAX_GOAL_RR_SIZE => 0, + MAX_RESULT_RQ_SIZE => 0, + MAX_RESULT_RR_SIZE => 0, + MAX_FEEDBACK_SIZE => 0, + is_client => FALSE, + enable_feedback => TRUE ) ); diff --git a/src/ros2/Tests/ros_testbench.pro b/src/ros2/Tests/ros_testbench.pro index 6fb2bbf..ffd3e20 100644 --- a/src/ros2/Tests/ros_testbench.pro +++ b/src/ros2/Tests/ros_testbench.pro @@ -5,6 +5,10 @@ library Testbench_ROS_Lib2 analyze ../../math_pkg.vhd analyze ../../rtps_package.vhd analyze ../ros_package.vhd +analyze ../rcl_interfaces/action_msgs/GoalInfo_package.vhd +analyze ../rcl_interfaces/action_msgs/GoalStatus_package.vhd +analyze ../rcl_interfaces/action_msgs/GoalStatusArray_package.vhd +analyze ../rcl_interfaces/action_msgs/CancelGoal_package.vhd analyze ../example_interfaces/AddTwoInts_package.vhd analyze Level_2/L2_Testbench_ROS_Lib2_config.vhd analyze ../dds_user_config.vhd @@ -43,6 +47,10 @@ library Testbench_ROS_Lib3 analyze ../../math_pkg.vhd analyze ../../rtps_package.vhd analyze ../ros_package.vhd +analyze ../rcl_interfaces/action_msgs/GoalInfo_package.vhd +analyze ../rcl_interfaces/action_msgs/GoalStatus_package.vhd +analyze ../rcl_interfaces/action_msgs/GoalStatusArray_package.vhd +analyze ../rcl_interfaces/action_msgs/CancelGoal_package.vhd analyze ../example_interfaces/AddTwoInts_package.vhd analyze Level_2/L2_Testbench_ROS_Lib3_config.vhd analyze ../dds_user_config.vhd diff --git a/src/ros2/dds_user_config.vhd b/src/ros2/dds_user_config.vhd index 09ce264..dd69a94 100644 --- a/src/ros2/dds_user_config.vhd +++ b/src/ros2/dds_user_config.vhd @@ -8,6 +8,8 @@ use ieee.numeric_std.all; use work.rtps_package.all; use work.ros_package.all; use work.ros_config.all; +use work.CancelGoal_package; +use work.GoalStatusArray_package; package user_config is @@ -25,11 +27,11 @@ package user_config is -- Unicast IPv4 Address used by all RTPS Entities [Default 192.168.0.128] constant DEFAULT_IPv4_ADDRESS : std_logic_vector(IPv4_ADDRESS_WIDTH-1 downto 0) := ROS_ADDRESS; -- Number of RTPS Writer Endpoints - constant NUM_WRITERS : natural := NUM_PUBS + NUM_SERVICES + 1; + constant NUM_WRITERS : natural := gen_num_writers(ROS_PUBLICATIONS,ROS_SERVICES,ROS_ACTIONS); -- Deferred to Package Body -- Number of RTPS Reader Endpoints - constant NUM_READERS : natural := NUM_SUBS + NUM_SERVICES; + constant NUM_READERS : natural := gen_num_readers(ROS_SUBSCRIPTIONS,ROS_SERVICES,ROS_ACTIONS); -- Deferred to Package Body -- Number of RTPS Endpoints (Do not modify) - constant NUM_ENDPOINTS : natural := NUM_READERS+NUM_WRITERS; + constant NUM_ENDPOINTS : natural := NUM_READERS + NUM_WRITERS; -- Deferred to Package Body -- PB Value of Default Port Generation (see DDSI-RTPS 2.3 Section 9.6.1) constant PORT_CONFIG_PB : natural := 7400; -- DG Value of Default Port Generation (see DDSI-RTPS 2.3 Section 9.6.1) @@ -82,10 +84,10 @@ package body user_config is end if; end function; - constant PARTICIPANT_ANNOUNCEMENT_PERIOD : DURATION_TYPE := duration_mux(gen_duration(50 us), gen_duration(30 sec), SIMULATION_TIMING); + constant PARTICIPANT_ANNOUNCEMENT_PERIOD : DURATION_TYPE := duration_mux(gen_duration(500 us), gen_duration(30 sec), SIMULATION_TIMING); constant PARTICIPANT_LEASE_DURATION : DURATION_TYPE := duration_mux(DEFAULT_PARTICIPANT_LEASE_DURATION, DEFAULT_PARTICIPANT_LEASE_DURATION, SIMULATION_TIMING); constant DURATION_DELTA : DURATION_TYPE := duration_mux(gen_duration(50 ns), gen_duration(100 ms), SIMULATION_TIMING); - constant PARTICIPANT_HEARTBEAT_PERIOD : DURATION_TYPE := duration_mux(gen_duration(40 us), gen_duration(1 sec), SIMULATION_TIMING); + constant PARTICIPANT_HEARTBEAT_PERIOD : DURATION_TYPE := duration_mux(gen_duration(100 us), gen_duration(1 sec), SIMULATION_TIMING); constant PARTICIPANT_HEARTBEAT_RESPONSE_DELAY : DURATION_TYPE := duration_mux(gen_duration(500 ns), gen_duration(500 ms), SIMULATION_TIMING); constant PARTICIPANT_HEARTBEAT_SUPPRESSION_DELAY : DURATION_TYPE := duration_mux(gen_duration(100 ns), gen_duration(0 ms), SIMULATION_TIMING); constant PARTICIPANT_ACKNACK_RESPONSE_DELAY : DURATION_TYPE := duration_mux(gen_duration(500 ns), gen_duration(200 ms), SIMULATION_TIMING); @@ -94,7 +96,7 @@ package body user_config is procedure set_sim_timing (ref : inout CONFIG_TYPE; sel : in boolean) is begin if (sel) then - ref.HEARTBEAT_PERIOD := gen_duration(50 us); + ref.HEARTBEAT_PERIOD := gen_duration(100 us); ref.HEARTBEAT_RESPONSE_DELAY := gen_duration(500 ns); ref.HEARTBEAT_SUPPRESSION_DELAY := gen_duration(100 ns); ref.ACKNACK_RESPONSE_DELAY := gen_duration(500 ns); @@ -144,6 +146,62 @@ package body user_config is end if; index := index + 1; end loop; + -- ACTION SUBS + for i in 0 to ROS_ACTIONS'length-1 loop + tmp := gen_fqn(ROS_NODES(ROS_ACTIONS(i).node_id).namespace, ROS_NODES(ROS_ACTIONS(i).node_id).name, ROS_ACTIONS(i).ACTIONNAME); + -- GOAL SERVICE + ret(index) := DEFAULT_READER_CONFIG; + set_from_qos_profile(ROS_ACTIONS(i).GOAL_QOS, ret(index)); + set_sim_timing(ret(index),SIMULATION_TIMING); + if (ROS_ACTIONS(i).is_client) then + ret(index).TOPICNAME := concat("rr",concat(tmp,"/_action/send_goalReply")); + ret(index).MAX_PAYLOAD_SIZE := ROS_ACTIONS(i).MAX_GOAL_RR_SIZE; + ret(index).TYPENAME := ROS_ACTIONS(i).GOAL_RR_TYPENAME; + else + ret(index).TOPICNAME := concat("rq",concat(tmp,"/_action/send_goalRequest")); + ret(index).MAX_PAYLOAD_SIZE := ROS_ACTIONS(i).MAX_GOAL_RQ_SIZE; + ret(index).TYPENAME := ROS_ACTIONS(i).GOAL_RQ_TYPENAME; + end if; + index := index + 1; + -- RESULT SERVICE + ret(index) := DEFAULT_READER_CONFIG; + set_from_qos_profile(ROS_ACTIONS(i).RESULT_QOS, ret(index)); + set_sim_timing(ret(index),SIMULATION_TIMING); + if (ROS_ACTIONS(i).is_client) then + ret(index).TOPICNAME := concat("rr",concat(tmp,"/_action/get_resultReply")); + ret(index).MAX_PAYLOAD_SIZE := ROS_ACTIONS(i).MAX_RESULT_RR_SIZE; + ret(index).TYPENAME := ROS_ACTIONS(i).RESULT_RR_TYPENAME; + else + ret(index).TOPICNAME := concat("rq",concat(tmp,"/_action/get_resultRequest")); + ret(index).MAX_PAYLOAD_SIZE := ROS_ACTIONS(i).MAX_RESULT_RQ_SIZE; + ret(index).TYPENAME := ROS_ACTIONS(i).RESULT_RQ_TYPENAME; + end if; + index := index + 1; + -- CANCEL SERVICE + ret(index) := DEFAULT_READER_CONFIG; + set_from_qos_profile(ROS_ACTIONS(i).CANCEL_QOS, ret(index)); + set_sim_timing(ret(index),SIMULATION_TIMING); + if (ROS_ACTIONS(i).is_client) then + ret(index).TOPICNAME := concat("rr",concat(tmp,"/_action/cancel_goalReply")); + ret(index).MAX_PAYLOAD_SIZE := CancelGoal_package.RR_MAX_CANCELGOAL_SIZE; + ret(index).TYPENAME := gen_user_string("action_msgs::srv::dds_::CancelGoal_Reply_"); + else + ret(index).TOPICNAME := concat("rq",concat(tmp,"/_action/cancel_goalRequest")); + ret(index).MAX_PAYLOAD_SIZE := CancelGoal_package.RQ_MAX_CANCELGOAL_SIZE; + ret(index).TYPENAME := gen_user_string("action_msgs::srv::dds_::CancelGoal_Request_"); + end if; + index := index + 1; + -- FEEDBACK TOPIC + if (ROS_ACTIONS(i).enable_feedback and ROS_ACTIONS(i).is_client) then + ret(index) := DEFAULT_READER_CONFIG; + set_from_qos_profile(ROS_ACTIONS(i).FEEDBACK_QOS, ret(index)); + set_sim_timing(ret(index),SIMULATION_TIMING); + ret(index).TOPICNAME := concat("rt",concat(tmp,"/_action/feedback")); + ret(index).MAX_PAYLOAD_SIZE := ROS_ACTIONS(i).MAX_FEEDBACK_SIZE; + ret(index).TYPENAME := ROS_ACTIONS(i).FEEDBACK_TYPENAME; + index := index + 1; + end if; + end loop; -- PUBLICATIONS for i in 0 to ROS_PUBLICATIONS'length-1 loop ret(index) := DEFAULT_WRITER_CONFIG; @@ -176,6 +234,72 @@ package body user_config is end if; index := index + 1; end loop; + -- ACTION PUBS + for i in 0 to ROS_ACTIONS'length-1 loop + tmp := gen_fqn(ROS_NODES(ROS_ACTIONS(i).node_id).namespace, ROS_NODES(ROS_ACTIONS(i).node_id).name, ROS_ACTIONS(i).ACTIONNAME); + -- GOAL SERVICE + ret(index) := DEFAULT_WRITER_CONFIG; + set_from_qos_profile(ROS_ACTIONS(i).GOAL_QOS, ret(index)); + set_sim_timing(ret(index),SIMULATION_TIMING); + if (ROS_ACTIONS(i).is_client) then + ret(index).TOPICNAME := concat("rq",concat(tmp,"/_action/send_goalRequest")); + ret(index).MAX_PAYLOAD_SIZE := ROS_ACTIONS(i).MAX_GOAL_RQ_SIZE; + ret(index).TYPENAME := ROS_ACTIONS(i).GOAL_RQ_TYPENAME; + else + ret(index).TOPICNAME := concat("rr",concat(tmp,"/_action/send_goalReply")); + ret(index).MAX_PAYLOAD_SIZE := ROS_ACTIONS(i).MAX_GOAL_RR_SIZE; + ret(index).TYPENAME := ROS_ACTIONS(i).GOAL_RR_TYPENAME; + end if; + index := index + 1; + -- RESULT SERVICE + ret(index) := DEFAULT_WRITER_CONFIG; + set_from_qos_profile(ROS_ACTIONS(i).RESULT_QOS, ret(index)); + set_sim_timing(ret(index),SIMULATION_TIMING); + if (ROS_ACTIONS(i).is_client) then + ret(index).TOPICNAME := concat("rq",concat(tmp,"/_action/get_resultRequest")); + ret(index).MAX_PAYLOAD_SIZE := ROS_ACTIONS(i).MAX_RESULT_RQ_SIZE; + ret(index).TYPENAME := ROS_ACTIONS(i).RESULT_RQ_TYPENAME; + else + ret(index).TOPICNAME := concat("rr",concat(tmp,"/_action/get_resultReply")); + ret(index).MAX_PAYLOAD_SIZE := ROS_ACTIONS(i).MAX_RESULT_RR_SIZE; + ret(index).TYPENAME := ROS_ACTIONS(i).RESULT_RR_TYPENAME; + end if; + index := index + 1; + -- CANCEL SERVICE + ret(index) := DEFAULT_WRITER_CONFIG; + set_from_qos_profile(ROS_ACTIONS(i).CANCEL_QOS, ret(index)); + set_sim_timing(ret(index),SIMULATION_TIMING); + if (ROS_ACTIONS(i).is_client) then + ret(index).TOPICNAME := concat("rq",concat(tmp,"/_action/cancel_goalRequest")); + ret(index).MAX_PAYLOAD_SIZE := CancelGoal_package.RQ_MAX_CANCELGOAL_SIZE; + ret(index).TYPENAME := gen_user_string("action_msgs::srv::dds_::CancelGoal_Request_"); + else + ret(index).TOPICNAME := concat("rr",concat(tmp,"/_action/cancel_goalReply")); + ret(index).MAX_PAYLOAD_SIZE := CancelGoal_package.RR_MAX_CANCELGOAL_SIZE; + ret(index).TYPENAME := gen_user_string("action_msgs::srv::dds_::CancelGoal_Reply_"); + end if; + index := index + 1; + -- FEEDBACK TOPIC + if (ROS_ACTIONS(i).enable_feedback and (not ROS_ACTIONS(i).is_client)) then + ret(index) := DEFAULT_WRITER_CONFIG; + set_from_qos_profile(ROS_ACTIONS(i).FEEDBACK_QOS, ret(index)); + set_sim_timing(ret(index),SIMULATION_TIMING); + ret(index).TOPICNAME := concat("rt",concat(tmp,"/_action/feedback")); + ret(index).MAX_PAYLOAD_SIZE := ROS_ACTIONS(i).MAX_FEEDBACK_SIZE; + ret(index).TYPENAME := ROS_ACTIONS(i).FEEDBACK_TYPENAME; + index := index + 1; + end if; + -- STATUS TOPIC + if (not ROS_ACTIONS(i).is_client) then + ret(index) := DEFAULT_WRITER_CONFIG; + set_from_qos_profile(ROS_QOS_PROFILE_ACTION_STATUS_DEFAULT, ret(index)); + set_sim_timing(ret(index),SIMULATION_TIMING); + ret(index).TOPICNAME := concat("rt",concat(tmp,"/_action/status")); + ret(index).MAX_PAYLOAD_SIZE := GoalStatusArray_package.MAX_GOALSTATUSARRAY_SIZE; + ret(index).TYPENAME := gen_user_string("action_msgs::msg::dds_::GoalStatusArray_"); + index := index + 1; + end if; + end loop; -- ROS DISCOVERY PUB ret(index) := DEFAULT_WRITER_CONFIG; set_sim_timing(ret(index),SIMULATION_TIMING); diff --git a/src/ros2/example_interfaces/Fibonacci_ros_action_client.vhd b/src/ros2/example_interfaces/Fibonacci_ros_action_client.vhd index 13ae11a..48e4595 100644 --- a/src/ros2/example_interfaces/Fibonacci_ros_action_client.vhd +++ b/src/ros2/example_interfaces/Fibonacci_ros_action_client.vhd @@ -174,6 +174,10 @@ entity Fibonacci_ros_action_client is ack : out std_logic; sequence_id : out std_logic_vector(ROS_SEQUENCE_ID_WIDTH-1 downto 0); service_info : out SERVICE_INFO_TYPE; + data_available_g : out std_logic; + data_available_r : out std_logic; + data_available_c : out std_logic; + data_available_f : out std_logic; done : out std_logic; return_code : out std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0); -- GOAL SIGNALS @@ -229,7 +233,7 @@ architecture arch of Fibonacci_ros_action_client is --*****SIGNAL DECLARATION***** signal stage, stage_next : STAGE_TYPE; signal cnt, cnt_next : natural range 0 to 5; - signal g_start_user, r_start_user, c_start_user, g_ack_user, r_ack_user, c_ack_user, g_data_available_user, r_data_available_user, c_data_available_user, g_taken_user, r_taken_user, c_taken_user, g_done_user, r_done_user, c_done_user : std_logic; + signal g_start_user, r_start_user, c_start_user, g_ack_user, r_ack_user, c_ack_user, g_taken_user, r_taken_user, c_taken_user, g_done_user, r_done_user, c_done_user : std_logic; signal g_opcode_user, r_opcode_user, c_opcode_user : ROS_SERVICE_OPCODE_TYPE; signal g_service_info_user, r_service_info_user, c_service_info_user : SERVICE_INFO_TYPE; signal g_sequence_id_user, r_sequence_id_user, c_sequence_id_user : std_logic_vector(ROS_SEQUENCE_ID_WIDTH-1 downto 0); @@ -298,7 +302,7 @@ begin return_code_user => g_return_code_user, service_info_user => g_service_info_user, sequence_id_user => g_sequence_id_user, - data_available_user => g_data_available_user, + data_available_user => data_available_g, taken_user => g_taken_user, goal_id => goal_id, -- ###GENERATED START### @@ -362,7 +366,7 @@ begin return_code_user => r_return_code_user, service_info_user => r_service_info_user, sequence_id_user => r_sequence_id_user, - data_available_user => r_data_available_user, + data_available_user => data_available_r, taken_user => r_taken_user, goal_id => result_goal_id, status => result_status, @@ -429,7 +433,7 @@ begin return_code_user => c_return_code_user, service_info_user => c_service_info_user, sequence_id_user => c_sequence_id_user, - data_available_user => c_data_available_user, + data_available_user => data_available_c, taken_user => c_taken_user, goal_info_goal_id => cancel_goal_id, goal_info_stamp => std_logic_vector(to_unsigned(cancel_stamp)), @@ -476,6 +480,7 @@ begin return_code_user => f_return_code_user, message_info_user => f_message_info_user, taken_user => f_taken_user, + data_available_user => data_available_f, -- ###GENERATED START### goal_id => feedback_goal_id, seq_len => feedback_seq_len, @@ -498,6 +503,12 @@ begin f_get_data_dds <= '0'; f_ready_in_dds <= '0'; f_sample_info_ack_dds <= '0'; + f_ack_user <= '0'; + f_done_user <= '0'; + f_return_code_user <= ROS_RET_OK; + data_available_f <= '0'; + f_message_info_user <= EMPTY_MESSAGE_INFO; + f_taken_user <= '0'; feedback_goal_id <= (others => '0'); -- ###GENERATED START### feedback_seq_len <= (others => '0'); diff --git a/src/ros2/ros2.txt b/src/ros2/ros2.txt index 683623c..2f37b36 100644 --- a/src/ros2/ros2.txt +++ b/src/ros2/ros2.txt @@ -464,6 +464,15 @@ TOPIC: '/_action/feedback' TOPIC: '/_action/status' rcl_interfaces/action_msgs/GoalStatusArray.msg +action_tutorials_interfaces::action::dds_::Fibonacci_SendGoal_Response_ +action_tutorials_interfaces::action::dds_::Fibonacci_GetResult_Response_ +action_tutorials_interfaces::action::dds_::Fibonacci_FeedbackMessage_ +action_msgs::srv::dds_::CancelGoal_Response_ +action_msgs::msg::dds_::GoalStatusArray_ + +action_tutorials_interfaces::action::dds_::Fibonacci_SendGoal_Request_ +action_tutorials_interfaces::action::dds_::Fibonacci_GetResult_Request_ +action_msgs::srv::dds_::CancelGoal_Request_ PARAMETER ========= @@ -481,4 +490,5 @@ TOPIC: 'ParameterEventDescriptor' MANAGED NODE ============ http://design.ros2.org/articles/node_lifecycle.html -REF: https://github.com/ros2/rclcpp.git [/rlcpp_lifecycle] \ No newline at end of file +REF: https://github.com/ros2/rclcpp.git [/rlcpp_lifecycle] + diff --git a/src/ros2/ros_package.vhd b/src/ros2/ros_package.vhd index bd8075e..294af50 100644 --- a/src/ros2/ros_package.vhd +++ b/src/ros2/ros_package.vhd @@ -139,6 +139,17 @@ package ros_package is AVOID_ROS_NAMESPACE_CONVENTION => FALSE ); + constant ROS_QOS_PROFILE_ACTION_STATUS_DEFAULT : ROS_QOS_PROFILE_TYPE := ( + HISTORY_QOS => KEEP_LAST_HISTORY_QOS, + HISTORY_DEPTH => std_logic_vector(to_unsigned(1, CDR_LONG_WIDTH)), + RELIABILITY_QOS => RELIABLE_RELIABILITY_QOS, + DURABILITY_QOS => TRANSIENT_LOCAL_DURABILITY_QOS, + DEADLINE_QOS => DEFAULT_DEADLINE_QOS, + LIFESPAN_QOS => DEFAULT_LIFESPAN_QOS, + LIVELINESS_QOS => DEFAULT_LIVELINESS_QOS, + LEASE_DURATION => DEFAULT_LEASE_DURATION, + AVOID_ROS_NAMESPACE_CONVENTION => FALSE + ); type ROS_NODE_TYPE is record @@ -172,8 +183,24 @@ package ros_package is end record; type ROS_ACTION_TYPE is record - node_id : natural; - -- TODO + node_id : natural; + ACTIONNAME : USER_STRING_TYPE; + GOAL_RQ_TYPENAME : USER_STRING_TYPE; + GOAL_RR_TYPENAME : USER_STRING_TYPE; + RESULT_RQ_TYPENAME : USER_STRING_TYPE; + RESULT_RR_TYPENAME : USER_STRING_TYPE; + FEEDBACK_TYPENAME : USER_STRING_TYPE; + GOAL_QOS : ROS_QOS_PROFILE_TYPE; + RESULT_QOS : ROS_QOS_PROFILE_TYPE; + FEEDBACK_QOS : ROS_QOS_PROFILE_TYPE; + CANCEL_QOS : ROS_QOS_PROFILE_TYPE; + MAX_GOAL_RQ_SIZE : natural; + MAX_GOAL_RR_SIZE : natural; + MAX_RESULT_RQ_SIZE : natural; + MAX_RESULT_RR_SIZE : natural; + MAX_FEEDBACK_SIZE : natural; + is_client : boolean; + enable_feedback : boolean; end record; type REQUEST_ID_TYPE is record @@ -288,6 +315,8 @@ package ros_package is function get_num_subs(nodes : ROS_NODE_ARRAY_TYPE) return natural; function get_num_services(nodes : ROS_NODE_ARRAY_TYPE) return natural; function get_num_actions(nodes : ROS_NODE_ARRAY_TYPE) return natural; + function gen_num_readers(subs : ROS_TOPIC_ARRAY_TYPE; services : ROS_SERVICE_ARRAY_TYPE; actions : ROS_ACTION_ARRAY_TYPE) return natural; + function gen_num_writers(pubs : ROS_TOPIC_ARRAY_TYPE; services : ROS_SERVICE_ARRAY_TYPE; actions : ROS_ACTION_ARRAY_TYPE) return natural; function get_domain_id(nodes : ROS_NODE_ARRAY_TYPE) return natural; function gen_fqn(ns : string; node : string; name : string) return USER_STRING_TYPE; procedure check_node_mapping(nodes : in ROS_NODE_ARRAY_TYPE; pubs : in ROS_TOPIC_ARRAY_TYPE; subs : in ROS_TOPIC_ARRAY_TYPE; services : in ROS_SERVICE_ARRAY_TYPE; actions : in ROS_ACTION_ARRAY_TYPE); @@ -336,6 +365,125 @@ package ros_package is type SERVICE_INTERFACE_ARRAY_TYPE is array (natural range <>) of SERVICE_INTERFACE_TYPE; + type ACTION_INTERFACE_TYPE is record + g_start_r : std_logic; + g_ack_r : std_logic; + g_opcode_r : DDS_READER_OPCODE_TYPE; + g_instance_state_r : std_logic_vector(INSTANCE_STATE_KIND_WIDTH-1 downto 0); + g_view_state_r : std_logic_vector(VIEW_STATE_KIND_WIDTH-1 downto 0); + g_sample_state_r : std_logic_vector(SAMPLE_STATE_KIND_WIDTH-1 downto 0); + g_instance_handle_r : INSTANCE_HANDLE_TYPE; + g_max_samples_r : std_logic_vector(MAX_SAMPLES_WIDTH-1 downto 0); + g_get_data_r : std_logic; + g_done_r : std_logic; + g_return_code_r : std_logic_vector(RETURN_CODE_WIDTH-1 downto 0); + g_valid_in_r : std_logic; + g_ready_in_r : std_logic; + g_data_in_r : std_logic_vector(WORD_WIDTH-1 downto 0); + g_last_word_in_r : std_logic; + g_sample_info_r : SAMPLE_INFO_TYPE; + g_sample_info_valid_r : std_logic; + g_sample_info_ack_r : std_logic; + g_eoc_r : std_logic; + g_status_r : std_logic_vector(STATUS_KIND_WIDTH-1 downto 0); + g_start_w : std_logic; + g_ack_w : std_logic; + g_opcode_w : DDS_WRITER_OPCODE_TYPE; + g_instance_handle_out_w : INSTANCE_HANDLE_TYPE; + g_source_ts_w : TIME_TYPE; + g_max_wait_w : DURATION_TYPE; + g_done_w : std_logic; + g_return_code_w : std_logic_vector(RETURN_CODE_WIDTH-1 downto 0); + g_instance_handle_in_w : INSTANCE_HANDLE_TYPE; + g_valid_out_w : std_logic; + g_ready_out_w : std_logic; + g_data_out_w : std_logic_vector(WORD_WIDTH-1 downto 0); + g_last_word_out_w : std_logic; + g_valid_in_w : std_logic; + g_ready_in_w : std_logic; + g_data_in_w : std_logic_vector(WORD_WIDTH-1 downto 0); + g_last_word_in_w : std_logic; + g_status_w : std_logic_vector(STATUS_KIND_WIDTH-1 downto 0); + r_start_r : std_logic; + r_ack_r : std_logic; + r_opcode_r : DDS_READER_OPCODE_TYPE; + r_instance_state_r : std_logic_vector(INSTANCE_STATE_KIND_WIDTH-1 downto 0); + r_view_state_r : std_logic_vector(VIEW_STATE_KIND_WIDTH-1 downto 0); + r_sample_state_r : std_logic_vector(SAMPLE_STATE_KIND_WIDTH-1 downto 0); + r_instance_handle_r : INSTANCE_HANDLE_TYPE; + r_max_samples_r : std_logic_vector(MAX_SAMPLES_WIDTH-1 downto 0); + r_get_data_r : std_logic; + r_return_code_r : std_logic_vector(RETURN_CODE_WIDTH-1 downto 0); + r_done_r : std_logic; + r_valid_in_r : std_logic; + r_ready_in_r : std_logic; + r_data_in_r : std_logic_vector(WORD_WIDTH-1 downto 0); + r_last_word_in_r : std_logic; + r_sample_info_r : SAMPLE_INFO_TYPE; + r_sample_info_valid_r : std_logic; + r_sample_info_ack_r : std_logic; + r_eoc_r : std_logic; + r_status_r : std_logic_vector(STATUS_KIND_WIDTH-1 downto 0); + r_start_w : std_logic; + r_ack_w : std_logic; + r_opcode_w : DDS_WRITER_OPCODE_TYPE; + r_instance_handle_out_w : INSTANCE_HANDLE_TYPE; + r_source_ts_w : TIME_TYPE; + r_max_wait_w : DURATION_TYPE; + r_done_w : std_logic; + r_return_code_w : std_logic_vector(RETURN_CODE_WIDTH-1 downto 0); + r_instance_handle_in_w : INSTANCE_HANDLE_TYPE; + r_valid_out_w : std_logic; + r_ready_out_w : std_logic; + r_data_out_w : std_logic_vector(WORD_WIDTH-1 downto 0); + r_last_word_out_w : std_logic; + r_valid_in_w : std_logic; + r_ready_in_w : std_logic; + r_data_in_w : std_logic_vector(WORD_WIDTH-1 downto 0); + r_last_word_in_w : std_logic; + r_status_w : std_logic_vector(STATUS_KIND_WIDTH-1 downto 0); + c_start_r : std_logic; + c_ack_r : std_logic; + c_opcode_r : DDS_READER_OPCODE_TYPE; + c_instance_state_r : std_logic_vector(INSTANCE_STATE_KIND_WIDTH-1 downto 0); + c_view_state_r : std_logic_vector(VIEW_STATE_KIND_WIDTH-1 downto 0); + c_sample_state_r : std_logic_vector(SAMPLE_STATE_KIND_WIDTH-1 downto 0); + c_instance_handle_r : INSTANCE_HANDLE_TYPE; + c_max_samples_r : std_logic_vector(MAX_SAMPLES_WIDTH-1 downto 0); + c_get_data_r : std_logic; + c_done_r : std_logic; + c_return_code_r : std_logic_vector(RETURN_CODE_WIDTH-1 downto 0); + c_valid_in_r : std_logic; + c_ready_in_r : std_logic; + c_data_in_r : std_logic_vector(WORD_WIDTH-1 downto 0); + c_last_word_in_r : std_logic; + c_sample_info_r : SAMPLE_INFO_TYPE; + c_sample_info_valid_r : std_logic; + c_sample_info_ack_r : std_logic; + c_eoc_r : std_logic; + c_status_r : std_logic_vector(STATUS_KIND_WIDTH-1 downto 0); + c_start_w : std_logic; + c_ack_w : std_logic; + c_opcode_w : DDS_WRITER_OPCODE_TYPE; + c_instance_handle_out_w : INSTANCE_HANDLE_TYPE; + c_source_ts_w : TIME_TYPE; + c_max_wait_w : DURATION_TYPE; + c_done_w : std_logic; + c_return_code_w : std_logic_vector(RETURN_CODE_WIDTH-1 downto 0); + c_instance_handle_in_w : INSTANCE_HANDLE_TYPE; + c_valid_out_w : std_logic; + c_ready_out_w : std_logic; + c_data_out_w : std_logic_vector(WORD_WIDTH-1 downto 0); + c_last_word_out_w : std_logic; + c_valid_in_w : std_logic; + c_ready_in_w : std_logic; + c_data_in_w : std_logic_vector(WORD_WIDTH-1 downto 0); + c_last_word_in_w : std_logic; + c_status_w : std_logic_vector(STATUS_KIND_WIDTH-1 downto 0); + end record; + + type ACTION_INTERFACE_ARRAY_TYPE is array (natural range <>) of ACTION_INTERFACE_TYPE; + function ">" (L,R: ROS_TIME_TYPE) return boolean; function "<" (L,R: ROS_TIME_TYPE) return boolean; function ">=" (L,R: ROS_TIME_TYPE) return boolean; @@ -463,6 +611,38 @@ package body ros_package is return ret; end function; + function gen_num_readers(subs : ROS_TOPIC_ARRAY_TYPE; services : ROS_SERVICE_ARRAY_TYPE; actions : ROS_ACTION_ARRAY_TYPE) return natural is + variable ret : natural := 0; + begin + ret := subs'length + services'length + (actions'length * 3); + for i in 0 to actions'length-1 loop + -- FEEDBACK SUBSCRIBER + if (actions(i).is_client and actions(i).enable_feedback) then + ret := ret + 1; + end if; + end loop; + return ret; + end function; + + function gen_num_writers(pubs : ROS_TOPIC_ARRAY_TYPE; services : ROS_SERVICE_ARRAY_TYPE; actions : ROS_ACTION_ARRAY_TYPE) return natural is + variable ret : natural := 0; + begin + ret := pubs'length + services'length + (actions'length * 3); + for i in 0 to actions'length-1 loop + -- FEEDBACK PUBLISHER + if ((not actions(i).is_client) and actions(i).enable_feedback) then + ret := ret + 1; + end if; + -- STATUS PUBLISHER + if (not actions(i).is_client) then + ret := ret + 1; + end if; + end loop; + -- ROS DISCOVERY PUBLISHER + ret := ret + 1; + return ret; + end function; + function is_numeric_char (char : character) return boolean is variable ret : boolean; begin diff --git a/syn/syn_ros_service_config.vhd b/syn/syn_ros_service_config.vhd index 866c887..f660ed5 100644 --- a/syn/syn_ros_service_config.vhd +++ b/syn/syn_ros_service_config.vhd @@ -72,7 +72,24 @@ package ros_config is constant ROS_ACTIONS : ROS_ACTION_ARRAY_TYPE(0 to NUM_ACTIONS-1) := ( others => ( - node_id => 0 + node_id => 0, + ACTIONNAME => gen_user_string(""), + GOAL_RQ_TYPENAME => gen_user_string(""), + GOAL_RR_TYPENAME => gen_user_string(""), + RESULT_RQ_TYPENAME => gen_user_string(""), + RESULT_RR_TYPENAME => gen_user_string(""), + FEEDBACK_TYPENAME => gen_user_string(""), + GOAL_QOS => ROS_QOS_PROFILE_SERVICES_DEFAULT, + RESULT_QOS => ROS_QOS_PROFILE_SERVICES_DEFAULT, + FEEDBACK_QOS => ROS_QOS_PROFILE_DEFAULT, + CANCEL_QOS => ROS_QOS_PROFILE_SERVICES_DEFAULT, + MAX_GOAL_RQ_SIZE => 0, + MAX_GOAL_RR_SIZE => 0, + MAX_RESULT_RQ_SIZE => 0, + MAX_RESULT_RR_SIZE => 0, + MAX_FEEDBACK_SIZE => 0, + is_client => FALSE, + enable_feedback => TRUE ) );