[go: up one dir, main page]
More Web Proxy on the site http://driver.im/
Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

This is trial impl #320

Draft
wants to merge 3 commits into
base: main
Choose a base branch
from
Draft

This is trial impl #320

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
936dfe3
Add measurement
pojiro Apr 24, 2024
3fcaa26
Change build_get_fun_fragments/3 of msg_c.ex and msg_c.eex
pojiro Apr 26, 2024
a91b29a
Add publish/take to each messages
pojiro Apr 26, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 5 additions & 1 deletion lib/mix/tasks/rclex/gen/msgs.ex
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -163,6 +163,8 @@ defmodule Mix.Tasks.Rclex.Gen.Msgs do
{"#{function_prefix}_destroy!", 1, nif_#{function_prefix}_destroy, REGULAR_NIF},
{"#{function_prefix}_set!", 2, nif_#{function_prefix}_set, REGULAR_NIF},
{"#{function_prefix}_get!", 1, nif_#{function_prefix}_get, REGULAR_NIF},
{"#{function_prefix}_publish!", 2, nif_#{function_prefix}_publish, REGULAR_NIF},
{"#{function_prefix}_take!", 1, nif_#{function_prefix}_take, REGULAR_NIF},
"""
end)
end
Expand All @@ -186,7 +188,9 @@ defmodule Mix.Tasks.Rclex.Gen.Msgs do
{"create!", ""},
{"destroy!", "_msg"},
{"set!", "_msg, _data"},
{"get!", "_msg"}
{"get!", "_msg"},
{"publish!", "_publisher, _data"},
{"take!", "_subscription"}
]

msg_funcs =
Expand Down
147 changes: 101 additions & 46 deletions lib/rclex/generators/msg_c.ex
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -87,8 +87,10 @@ defmodule Rclex.Generators.MsgC do
"""
int #{var}_arity;
const ERL_NIF_TERM *#{var}_tuple;
if (!enif_get_tuple(env, #{term}, &#{var}_arity, &#{var}_tuple))
return enif_make_badarg(env);
if (!enif_get_tuple(env, #{term}, &#{var}_arity, &#{var}_tuple)) {
term = enif_make_badarg(env);
goto after;
}

#{binary}
"""
Expand Down Expand Up @@ -123,24 +125,33 @@ defmodule Rclex.Generators.MsgC do

"""
unsigned int #{var}_length;
if (!enif_get_list_length(env, #{term}, &#{var}_length))
return enif_make_badarg(env);
if (!enif_get_list_length(env, #{term}, &#{var}_length)) {
term = enif_make_badarg(env);
goto after;
}

#{sequence} *#{var} = #{sequence}__create(#{var}_length);
if (#{var} == NULL) return raise(env, __FILE__, __LINE__);
if (#{var} == NULL) {
term = raise(env, __FILE__, __LINE__);
goto after;
}
message_p->#{mbr} = *#{var};

unsigned int #{var}_i;
ERL_NIF_TERM #{var}_left, #{var}_head, #{var}_tail;
for (#{var}_i = 0, #{var}_left = #{term}; #{var}_i < #{var}_length; ++#{var}_i, #{var}_left = #{var}_tail)
{
if (!enif_get_list_cell(env, #{var}_left, &#{var}_head, &#{var}_tail))
return enif_make_badarg(env);
if (!enif_get_list_cell(env, #{var}_left, &#{var}_head, &#{var}_tail)){
term = enif_make_badarg(env);
goto after;
}

int #{var}_i_arity;
const ERL_NIF_TERM *#{var}_i_tuple;
if (!enif_get_tuple(env, #{var}_head, &#{var}_i_arity, &#{var}_i_tuple))
return enif_make_badarg(env);
if (!enif_get_tuple(env, #{var}_head, &#{var}_i_arity, &#{var}_i_tuple)) {
term = enif_make_badarg(env);
goto after;
}

#{binary}
}
Expand Down Expand Up @@ -169,20 +180,26 @@ defmodule Rclex.Generators.MsgC do

"""
unsigned int #{var}_length;
if (!enif_get_list_length(env, #{term}, &#{var}_length))
return enif_make_badarg(env);
if (!enif_get_list_length(env, #{term}, &#{var}_length)) {
term = enif_make_badarg(env);
goto after;
}

#{sequence} #{var};
if(!#{sequence}__init(&#{var}, #{var}_length))
return enif_make_badarg(env);
if(!#{sequence}__init(&#{var}, #{var}_length)) {
term = enif_make_badarg(env);
goto after;
}
message_p->#{mbr} = #{var};

unsigned int #{var}_i;
ERL_NIF_TERM #{var}_left, #{var}_head, #{var}_tail;
for (#{var}_i = 0, #{var}_left = #{term}; #{var}_i < #{var}_length; ++#{var}_i, #{var}_left = #{var}_tail)
{
if (!enif_get_list_cell(env, #{var}_left, &#{var}_head, &#{var}_tail))
return enif_make_badarg(env);
if (!enif_get_list_cell(env, #{var}_left, &#{var}_head, &#{var}_tail)) {
term = enif_make_badarg(env);
goto after;
}

#{binary}
}
Expand All @@ -192,76 +209,96 @@ defmodule Rclex.Generators.MsgC do
defp enif_get_builtin("bool", var, mbr, term) do
"""
unsigned int #{var}_length;
if (!enif_get_atom_length(env, #{term}, &#{var}_length, ERL_NIF_LATIN1))
return enif_make_badarg(env);
if (!enif_get_atom_length(env, #{term}, &#{var}_length, ERL_NIF_LATIN1)) {
term = enif_make_badarg(env);
goto after;
}

char #{var}[#{var}_length + 1];
if (enif_get_atom(env, #{term}, #{var}, #{var}_length + 1, ERL_NIF_LATIN1) <= 0)
return enif_make_badarg(env);
{
char #{var}[#{var}_length + 1];
if (enif_get_atom(env, #{term}, #{var}, #{var}_length + 1, ERL_NIF_LATIN1) <= 0) {
term = enif_make_badarg(env);
goto after;
}

message_p->#{mbr} = (strncmp(#{var}, "true", 4) == 0);
message_p->#{mbr} = (strncmp(#{var}, "true", 4) == 0);
}
"""
end

defp enif_get_builtin("int64", var, mbr, term) do
"""
int64_t #{var};
if (!enif_get_int64(env, #{term}, &#{var}))
return enif_make_badarg(env);
if (!enif_get_int64(env, #{term}, &#{var})) {
term = enif_make_badarg(env);
goto after;
}
message_p->#{mbr} = #{var};
"""
end

defp enif_get_builtin("byte", var, mbr, term) do
"""
unsigned int #{var};
if (!enif_get_uint(env, #{term}, &#{var}))
return enif_make_badarg(env);
if (!enif_get_uint(env, #{term}, &#{var})) {
term = enif_make_badarg(env);
goto after;
}
message_p->#{mbr} = (uint8_t)#{var};
"""
end

defp enif_get_builtin("int" <> _, var, mbr, term) do
"""
int #{var};
if (!enif_get_int(env, #{term}, &#{var}))
return enif_make_badarg(env);
if (!enif_get_int(env, #{term}, &#{var})) {
term = enif_make_badarg(env);
goto after;
}
message_p->#{mbr} = #{var};
"""
end

defp enif_get_builtin("uint64", var, mbr, term) do
"""
uint64_t #{var};
if (!enif_get_uint64(env, #{term}, &#{var}))
return enif_make_badarg(env);
if (!enif_get_uint64(env, #{term}, &#{var})) {
term = enif_make_badarg(env);
goto after;
}
message_p->#{mbr} = #{var};
"""
end

defp enif_get_builtin("uint" <> _, var, mbr, term) do
"""
unsigned int #{var};
if (!enif_get_uint(env, #{term}, &#{var}))
return enif_make_badarg(env);
if (!enif_get_uint(env, #{term}, &#{var})) {
term = enif_make_badarg(env);
goto after;
}
message_p->#{mbr} = #{var};
"""
end

defp enif_get_builtin("float64", var, mbr, term) do
"""
double #{var};
if (!enif_get_double(env, #{term}, &#{var}))
return enif_make_badarg(env);
if (!enif_get_double(env, #{term}, &#{var})) {
term = enif_make_badarg(env);
goto after;
}
message_p->#{mbr} = #{var};
"""
end

defp enif_get_builtin("float32", var, mbr, term) do
"""
double #{var};
if (!enif_get_double(env, #{term}, &#{var}))
return enif_make_badarg(env);
if (!enif_get_double(env, #{term}, &#{var})) {
term = enif_make_badarg(env);
goto after;
}
message_p->#{mbr} = (float)#{var};
"""
end
Expand All @@ -270,28 +307,46 @@ defmodule Rclex.Generators.MsgC do
"""
unsigned int #{var}_length;
#if (ERL_NIF_MAJOR_VERSION == 2 && ERL_NIF_MINOR_VERSION >= 17) // OTP-26 and later
if (!enif_get_string_length(env, #{term}, &#{var}_length, ERL_NIF_LATIN1))
return enif_make_badarg(env);
if (!enif_get_string_length(env, #{term}, &#{var}_length, ERL_NIF_LATIN1)) {
term = enif_make_badarg(env);
goto after;
}
#else
if (!enif_get_list_length(env, #{term}, &#{var}_length))
return enif_make_badarg(env);
if (!enif_get_list_length(env, #{term}, &#{var}_length)) {
term = enif_make_badarg(env);
goto after;
}
#endif

char #{var}[#{var}_length + 1];
if (enif_get_string(env, #{term}, #{var}, #{var}_length + 1, ERL_NIF_LATIN1) <= 0)
return enif_make_badarg(env);
{
char #{var}[#{var}_length + 1];
if (enif_get_string(env, #{term}, #{var}, #{var}_length + 1, ERL_NIF_LATIN1) <= 0) {
term = enif_make_badarg(env);
goto after;
}

if (!rosidl_runtime_c__String__assign(&(message_p->#{mbr}), #{var}))
return raise(env, __FILE__, __LINE__);
if (!rosidl_runtime_c__String__assign(&(message_p->#{mbr}), #{var})) {
term = raise(env, __FILE__, __LINE__);
goto after;
}
}
"""
end

def get_fun_fragments(ros2_message_type, ros2_message_type_map) do
build_get_fun_fragments(%Acc{type: {:msg_type, ros2_message_type}}, ros2_message_type_map)
fragments =
build_get_fun_fragments(%Acc{type: {:msg_type, ros2_message_type}}, ros2_message_type_map)
|> format()

"""
{
#{fragments}
}
"""
|> format()
end

def build_get_fun_fragments(acc, lhs \\ "return", ros2_message_type_map) do
def build_get_fun_fragments(acc, lhs \\ "term =", ros2_message_type_map) do
{binary, accs} = enif_make(acc.type, acc, ros2_message_type_map)

array_accs =
Expand Down
56 changes: 19 additions & 37 deletions lib/rclex/subscription.ex
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -73,26 +73,16 @@ defmodule Rclex.Subscription do
def handle_info(:take, state) do
case Nif.rcl_wait_subscription!(state.callback_resource, 1000, state.subscription) do
:ok ->
message = apply(state.message_type, :create!, [])

try do
case Nif.rcl_take!(state.subscription, message) do
:ok ->
message_struct = apply(state.message_type, :get!, [message])

{:ok, _pid} =
Task.Supervisor.start_child(
{:via, PartitionSupervisor, {Rclex.TaskSupervisors, self()}},
fn -> state.callback.(message_struct) end
)

:subscription_take_failed ->
Logger.debug(
"#{__MODULE__}: take failed but no error occurred in the middleware"
case apply(state.message_type, :take!, [state.subscription]) do
:subscription_take_failed ->
Logger.debug("#{__MODULE__}: take failed but no error occurred in the middleware")

message_struct ->
{:ok, _pid} =
Task.Supervisor.start_child(
{:via, PartitionSupervisor, {Rclex.TaskSupervisors, self()}},
fn -> state.callback.(message_struct) end
)
end
after
:ok = apply(state.message_type, :destroy!, [message])
end

:timeout ->
Expand Down Expand Up @@ -121,24 +111,16 @@ defmodule Rclex.Subscription do

def handle_info({:new_message, number_of_events}, state) when number_of_events > 0 do
for _ <- 1..number_of_events do
message = apply(state.message_type, :create!, [])

try do
case Nif.rcl_take!(state.subscription, message) do
:ok ->
message_struct = apply(state.message_type, :get!, [message])

{:ok, _pid} =
Task.Supervisor.start_child(
{:via, PartitionSupervisor, {Rclex.TaskSupervisors, self()}},
fn -> state.callback.(message_struct) end
)

:subscription_take_failed ->
Logger.debug("#{__MODULE__}: take failed but no error occurred in the middleware")
end
after
:ok = apply(state.message_type, :destroy!, [message])
case apply(state.message_type, :take!, [state.subscription]) do
:subscription_take_failed ->
Logger.debug("#{__MODULE__}: take failed but no error occurred in the middleware")

message_struct ->
{:ok, _pid} =
Task.Supervisor.start_child(
{:via, PartitionSupervisor, {Rclex.TaskSupervisors, self()}},
fn -> state.callback.(message_struct) end
)
end
end

Expand Down
Loading
Loading