greek64/rtps-fpga
2
1
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Add ROS Action glue logic

Browse Source
This commit is contained in:
John Ring 2022-03-08 13:24:58 +01:00
parent bd1f803906
commit 7292cedeb5
11 changed files with 428 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
src/ros2/TEMPLATE_ros_config.vhd
View File

@ -72,7 +72,24 @@ package ros_config is
-- TODO -- TODO
constant ROS_ACTIONS : ROS_ACTION_ARRAY_TYPE(0 to NUM_ACTIONS-1) := ( constant ROS_ACTIONS : ROS_ACTION_ARRAY_TYPE(0 to NUM_ACTIONS-1) := (
others => ( 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
) )
); );

6
src/ros2/Tests/Level_1/L1_Fibonacci_ros_action_test1.vhd
View File

@ -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 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 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 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'); signal return_code_c, return_code_s : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := (others => '0');
-- ###GENERATED START### -- ###GENERATED START###
@ -222,6 +222,10 @@ begin
ack => ack_c, ack => ack_c,
sequence_id => sequence_id_c, sequence_id => sequence_id_c,
service_info => service_info_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, done => done_c,
return_code => return_code_c, return_code => return_code_c,
goal_id => goal_id_c, goal_id => goal_id_c,

6
src/ros2/Tests/Level_1/L1_Fibonacci_ros_action_test2.vhd
View File

@ -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 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 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 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'); signal return_code_c, return_code_s : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := (others => '0');
-- ###GENERATED START### -- ###GENERATED START###
@ -225,6 +225,10 @@ begin
ack => ack_c, ack => ack_c,
sequence_id => sequence_id_c, sequence_id => sequence_id_c,
service_info => service_info_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, done => done_c,
return_code => return_code_c, return_code => return_code_c,
goal_id => goal_id_c, goal_id => goal_id_c,

19
src/ros2/Tests/Level_2/L2_Testbench_ROS_Lib2_config.vhd
View File

@ -72,7 +72,24 @@ package ros_config is
constant ROS_ACTIONS : ROS_ACTION_ARRAY_TYPE(0 to NUM_ACTIONS-1) := ( constant ROS_ACTIONS : ROS_ACTION_ARRAY_TYPE(0 to NUM_ACTIONS-1) := (
others => ( 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
) )
); );

19
src/ros2/Tests/Level_2/L2_Testbench_ROS_Lib3_config.vhd
View File

@ -72,7 +72,24 @@ package ros_config is
constant ROS_ACTIONS : ROS_ACTION_ARRAY_TYPE(0 to NUM_ACTIONS-1) := ( constant ROS_ACTIONS : ROS_ACTION_ARRAY_TYPE(0 to NUM_ACTIONS-1) := (
others => ( 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
) )
); );

8
src/ros2/Tests/ros_testbench.pro
View File

@ -5,6 +5,10 @@ library Testbench_ROS_Lib2
analyze ../../math_pkg.vhd analyze ../../math_pkg.vhd
analyze ../../rtps_package.vhd analyze ../../rtps_package.vhd
analyze ../ros_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 ../example_interfaces/AddTwoInts_package.vhd
analyze Level_2/L2_Testbench_ROS_Lib2_config.vhd analyze Level_2/L2_Testbench_ROS_Lib2_config.vhd
analyze ../dds_user_config.vhd analyze ../dds_user_config.vhd
@ -43,6 +47,10 @@ library Testbench_ROS_Lib3
analyze ../../math_pkg.vhd analyze ../../math_pkg.vhd
analyze ../../rtps_package.vhd analyze ../../rtps_package.vhd
analyze ../ros_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 ../example_interfaces/AddTwoInts_package.vhd
analyze Level_2/L2_Testbench_ROS_Lib3_config.vhd analyze Level_2/L2_Testbench_ROS_Lib3_config.vhd
analyze ../dds_user_config.vhd analyze ../dds_user_config.vhd

136
src/ros2/dds_user_config.vhd
View File

@ -8,6 +8,8 @@ use ieee.numeric_std.all;
use work.rtps_package.all; use work.rtps_package.all;
use work.ros_package.all; use work.ros_package.all;
use work.ros_config.all; use work.ros_config.all;
use work.CancelGoal_package;
use work.GoalStatusArray_package;
package user_config is 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] -- 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; constant DEFAULT_IPv4_ADDRESS : std_logic_vector(IPv4_ADDRESS_WIDTH-1 downto 0) := ROS_ADDRESS;
-- Number of RTPS Writer Endpoints -- 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 -- 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) -- 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) -- PB Value of Default Port Generation (see DDSI-RTPS 2.3 Section 9.6.1)
constant PORT_CONFIG_PB : natural := 7400; constant PORT_CONFIG_PB : natural := 7400;
-- DG Value of Default Port Generation (see DDSI-RTPS 2.3 Section 9.6.1) -- 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 if;
end function; 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 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 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_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_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); 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 procedure set_sim_timing (ref : inout CONFIG_TYPE; sel : in boolean) is
begin begin
if (sel) then 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_RESPONSE_DELAY := gen_duration(500 ns);
ref.HEARTBEAT_SUPPRESSION_DELAY := gen_duration(100 ns); ref.HEARTBEAT_SUPPRESSION_DELAY := gen_duration(100 ns);
ref.ACKNACK_RESPONSE_DELAY := gen_duration(500 ns); ref.ACKNACK_RESPONSE_DELAY := gen_duration(500 ns);
@ -144,6 +146,62 @@ package body user_config is
end if; end if;
index := index + 1; index := index + 1;
end loop; 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 -- PUBLICATIONS
for i in 0 to ROS_PUBLICATIONS'length-1 loop for i in 0 to ROS_PUBLICATIONS'length-1 loop
ret(index) := DEFAULT_WRITER_CONFIG; ret(index) := DEFAULT_WRITER_CONFIG;
@ -176,6 +234,72 @@ package body user_config is
end if; end if;
index := index + 1; index := index + 1;
end loop; 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 -- ROS DISCOVERY PUB
ret(index) := DEFAULT_WRITER_CONFIG; ret(index) := DEFAULT_WRITER_CONFIG;
set_sim_timing(ret(index),SIMULATION_TIMING); set_sim_timing(ret(index),SIMULATION_TIMING);

19
src/ros2/example_interfaces/Fibonacci_ros_action_client.vhd
View File

@ -174,6 +174,10 @@ entity Fibonacci_ros_action_client is
ack : out std_logic; ack : out std_logic;
sequence_id : out std_logic_vector(ROS_SEQUENCE_ID_WIDTH-1 downto 0); sequence_id : out std_logic_vector(ROS_SEQUENCE_ID_WIDTH-1 downto 0);
service_info : out SERVICE_INFO_TYPE; 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; done : out std_logic;
return_code : out std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0); return_code : out std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0);
-- GOAL SIGNALS -- GOAL SIGNALS
@ -229,7 +233,7 @@ architecture arch of Fibonacci_ros_action_client is
--*****SIGNAL DECLARATION***** --*****SIGNAL DECLARATION*****
signal stage, stage_next : STAGE_TYPE; signal stage, stage_next : STAGE_TYPE;
signal cnt, cnt_next : natural range 0 to 5; 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_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_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); 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, return_code_user => g_return_code_user,
service_info_user => g_service_info_user, service_info_user => g_service_info_user,
sequence_id_user => g_sequence_id_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, taken_user => g_taken_user,
goal_id => goal_id, goal_id => goal_id,
-- ###GENERATED START### -- ###GENERATED START###
@ -362,7 +366,7 @@ begin
return_code_user => r_return_code_user, return_code_user => r_return_code_user,
service_info_user => r_service_info_user, service_info_user => r_service_info_user,
sequence_id_user => r_sequence_id_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, taken_user => r_taken_user,
goal_id => result_goal_id, goal_id => result_goal_id,
status => result_status, status => result_status,
@ -429,7 +433,7 @@ begin
return_code_user => c_return_code_user, return_code_user => c_return_code_user,
service_info_user => c_service_info_user, service_info_user => c_service_info_user,
sequence_id_user => c_sequence_id_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, taken_user => c_taken_user,
goal_info_goal_id => cancel_goal_id, goal_info_goal_id => cancel_goal_id,
goal_info_stamp => std_logic_vector(to_unsigned(cancel_stamp)), goal_info_stamp => std_logic_vector(to_unsigned(cancel_stamp)),
@ -476,6 +480,7 @@ begin
return_code_user => f_return_code_user, return_code_user => f_return_code_user,
message_info_user => f_message_info_user, message_info_user => f_message_info_user,
taken_user => f_taken_user, taken_user => f_taken_user,
data_available_user => data_available_f,
-- ###GENERATED START### -- ###GENERATED START###
goal_id => feedback_goal_id, goal_id => feedback_goal_id,
seq_len => feedback_seq_len, seq_len => feedback_seq_len,
@ -498,6 +503,12 @@ begin
f_get_data_dds <= '0'; f_get_data_dds <= '0';
f_ready_in_dds <= '0'; f_ready_in_dds <= '0';
f_sample_info_ack_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'); feedback_goal_id <= (others => '0');
-- ###GENERATED START### -- ###GENERATED START###
feedback_seq_len <= (others => '0'); feedback_seq_len <= (others => '0');

12
src/ros2/ros2.txt
View File

@ -464,6 +464,15 @@ TOPIC: '<ACTIONNAME>/_action/feedback'
TOPIC: '<ACTIONNAME>/_action/status' TOPIC: '<ACTIONNAME>/_action/status'
rcl_interfaces/action_msgs/GoalStatusArray.msg 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 PARAMETER
========= =========
@ -481,4 +490,5 @@ TOPIC: 'ParameterEventDescriptor'
MANAGED NODE MANAGED NODE
============ ============
http://design.ros2.org/articles/node_lifecycle.html http://design.ros2.org/articles/node_lifecycle.html
REF: https://github.com/ros2/rclcpp.git [/rlcpp_lifecycle] REF: https://github.com/ros2/rclcpp.git [/rlcpp_lifecycle]

184
src/ros2/ros_package.vhd
View File

@ -139,6 +139,17 @@ package ros_package is
AVOID_ROS_NAMESPACE_CONVENTION => FALSE 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 type ROS_NODE_TYPE is record
@ -172,8 +183,24 @@ package ros_package is
end record; end record;
type ROS_ACTION_TYPE is record type ROS_ACTION_TYPE is record
node_id : natural; node_id : natural;
-- TODO 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; end record;
type REQUEST_ID_TYPE is 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_subs(nodes : ROS_NODE_ARRAY_TYPE) return natural;
function get_num_services(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 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 get_domain_id(nodes : ROS_NODE_ARRAY_TYPE) return natural;
function gen_fqn(ns : string; node : string; name : string) return USER_STRING_TYPE; 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); 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 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; 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; return ret;
end function; 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 function is_numeric_char (char : character) return boolean is
variable ret : boolean; variable ret : boolean;
begin begin

19
syn/syn_ros_service_config.vhd
View File

@ -72,7 +72,24 @@ package ros_config is
constant ROS_ACTIONS : ROS_ACTION_ARRAY_TYPE(0 to NUM_ACTIONS-1) := ( constant ROS_ACTIONS : ROS_ACTION_ARRAY_TYPE(0 to NUM_ACTIONS-1) := (
others => ( 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
) )
); );