diff --git a/Project.toml b/Project.toml index e5ca7a0..f7bdc2b 100644 --- a/Project.toml +++ b/Project.toml @@ -1,7 +1,7 @@ name = "RosSockets" uuid = "f2b1035b-1fed-4502-a057-be66ed18c291" authors = ["Jacob Levy"] -version = "0.2.2" +version = "0.3.0" [deps] JSON = "682c06a0-de6a-54ab-a142-c8b1cf79cde6" diff --git a/README.md b/README.md index 8159340..0e33365 100644 --- a/README.md +++ b/README.md @@ -58,14 +58,15 @@ close_robot_connection(robot_connection) First, ensure the `/state_feedback` node from from [ros_sockets](https://github.com/CLeARoboticsLab/ros_sockets) is running on the target. -Open a listener for feedback data, setting a `port` to listen on which matches that of the node: +Open a connection to the ROS node, setting `ip` and `port` to match that of the node: ```jl +ip = "192.168.88.128" port = 42422 -feedback_connection = open_feedback_connection(port) +feedback_connection = open_feedback_connection(ip, port) ``` -With an open listener, wait for data to arrive from the ROS node. +With the connection open, wait for data to arrive from the ROS node. Execution is blocked while waiting, up to the timeout duration (seconds) provided. If the timeout duration elapses without the arrival of data, a TimeoutError exception is thrown. @@ -76,7 +77,7 @@ state = receive_feedback_data(feedback_connection, timeout) `receive_feedback_data` returns a struct with the following fields: `position`, `orientation`, `linear_vel`, `angular_vel`. -When complete with tasks, be sure to close the listener: +When complete with tasks, be sure to close the connection: ```jl close_feedback_connection(feedback_connection) diff --git a/examples/multi_agent_example.jl b/examples/multi_agent_example.jl index dc0a27e..cac81c5 100644 --- a/examples/multi_agent_example.jl +++ b/examples/multi_agent_example.jl @@ -87,7 +87,7 @@ function run_example() # open feedback and velocity control connections for each agent for agent in agents - agent.feedback_connection = open_feedback_connection(agent.feedback_port) + agent.feedback_connection = open_feedback_connection(IP, agent.feedback_port) agent.robot_connection = open_robot_connection(IP, agent.control_port) end diff --git a/examples/optimal_feedback_control_example.jl b/examples/optimal_feedback_control_example.jl index 74dcf3e..bcc5121 100644 --- a/examples/optimal_feedback_control_example.jl +++ b/examples/optimal_feedback_control_example.jl @@ -28,13 +28,13 @@ function run_example() # initialize a plotter to display trajectory solution real-time display(plot(xlims=(-1,4),ylims=(-1,4),size=(600,600))) - # Open listener for feedback data from the ROS node - feedback_port = 42422 # port to listen for ROS data on + # Open a connection to the ROS feedback node + ip = "192.168.1.135" # ip address of the host of the ROS node + feedback_port = 42422 # port to connect on timeout = 10.0 # maximum seconds to wait for data with receive_feedback_data - feedback_connection = open_feedback_connection(feedback_port) + feedback_connection = open_feedback_connection(ip, feedback_port) # open a connection to the ROS velocity control node - ip = "192.168.1.135" # ip address of the host of the ROS node control_port = 42421 # port to connect on robot_connection = open_robot_connection(ip, control_port) diff --git a/examples/state_feedback_barebones_example.jl b/examples/state_feedback_barebones_example.jl index 816dcf1..dac2083 100644 --- a/examples/state_feedback_barebones_example.jl +++ b/examples/state_feedback_barebones_example.jl @@ -6,11 +6,12 @@ using RosSockets function run_example() - port = 42422 # port to listen for ROS data on + ip = "192.168.1.135" # ip address of the host of the ROS node + port = 42422 # port to connect on timeout = 10.0 # maximum seconds to wait for data with receive_feedback_data # Open listener for feedback data from the ROS node - feedback_connection = open_feedback_connection(port) + feedback_connection = open_feedback_connection(ip, port) # Wait for data to arrive from the ROS node. Execution is blocked while # waiting, up to the timout duration provided. If the timeout duration diff --git a/examples/utils/nlp.jl b/examples/utils/nlp.jl index 747da54..3e1dc6d 100644 --- a/examples/utils/nlp.jl +++ b/examples/utils/nlp.jl @@ -7,9 +7,8 @@ function initialize_model(goal_state, timestep) Q = 1 R = 2 - T = 400 dt = timestep - times = [k*dt for k in 0:T-1] + T = Integer(round(40/dt)) v_max = 0.22 a_max = v_max diff --git a/src/state_feedback.jl b/src/state_feedback.jl index e7a52ff..f551f59 100644 --- a/src/state_feedback.jl +++ b/src/state_feedback.jl @@ -17,22 +17,18 @@ mutable struct FeedbackData end mutable struct FeedbackConnection - socket::Sockets.UDPSocket task::Task + command_channel::Channel{Any} data_channel::Channel{Any} - ready_channel::Channel{Bool} - function FeedbackConnection(port::Integer) - @info "Starting feedback server on port $(port) ..." - socket = UDPSocket() - bind(socket,IPv4(0),port) + function FeedbackConnection(ip::String, port::Integer) + command_channel = Channel(1) data_channel = Channel(1) - ready_channel = Channel{Bool}(1) - put!(ready_channel, false) + @info "Connecting to feedback server at $(ip):$(port) ..." + socket = Sockets.connect(ip, port) task = errormonitor(Threads.@spawn feedback_connection_task(socket, - data_channel, ready_channel)) - feedback_connection = new(socket, task, data_channel, ready_channel) - @info "Feedback server started" + command_channel, data_channel)) + feedback_connection = new(task, command_channel, data_channel) return feedback_connection end end @@ -41,37 +37,31 @@ mutable struct TimeoutError <: Exception end """ - open_feedback_connection(port::Integer) + open_feedback_connection(ip::String, port::Integer) -Open listener for feedback data from the ROS node on the specified port and -return the `FeedbackConnection`. +Open a connection to the ROS node and return the `FeedbackConnection`. +The `ip` must be a string formated as `"123.123.123.123"` """ -function open_feedback_connection(port::Integer) - return FeedbackConnection(port) +function open_feedback_connection(ip::String, port::Integer) + return FeedbackConnection(ip, port) end -# This task continuously reads from the UDP port in order to keep the buffer -# clear. That way, when receive_feedback_data is called, stale data will not be -# returned. -function feedback_connection_task(socket, data_channel, ready_channel) +function feedback_connection_task(socket, command_channel, data_channel) + @info "Feedback connection task spawned" while true - payload = nothing - try - payload = recv(socket) - catch e - # socket was closed, end the task + command = take!(command_channel) + if command === :close + @info "Closing feedback connection ..." break end - - if fetch(ready_channel) - _ = take!(ready_channel) - put!(ready_channel, false) - put!(data_channel, payload) - else - @debug "Feedback data discarded" - end + msg = """{ "action": "get_feedback_data" }\n""" + write(socket, msg) + data = readline(socket) + put!(data_channel, data) end + close(socket) + @info "Feedback connection task completed" end """ @@ -91,11 +81,10 @@ exception. """ function receive_feedback_data(feedback_connection::FeedbackConnection, timeout::Real = 10.0) - _ = take!(feedback_connection.ready_channel) - put!(feedback_connection.ready_channel, true) t = Timer(_ -> timeout_callback(feedback_connection), timeout) feedback_data = nothing try + put!(feedback_connection.command_channel, :read) payload = take!(feedback_connection.data_channel) data = JSON.parse(String(payload)) feedback_data = FeedbackData(data) @@ -117,15 +106,15 @@ function timeout_callback(feedback_connection::FeedbackConnection) end """ -close_feedback_connection(feedback_connection::FeedbackConnection) + close_feedback_connection(feedback_connection::FeedbackConnection) -Close the listener. +Close the connection with the ROS node. """ function close_feedback_connection(feedback_connection::FeedbackConnection) @info "Stopping feedback server ..." - close(feedback_connection.socket) + put!(feedback_connection.command_channel, :close) wait(feedback_connection.task) close(feedback_connection.data_channel) - close(feedback_connection.ready_channel) + close(feedback_connection.command_channel) @info "Feedback server stopped" end diff --git a/src/velocity_control.jl b/src/velocity_control.jl index cfb6e03..7bc8ccc 100644 --- a/src/velocity_control.jl +++ b/src/velocity_control.jl @@ -19,7 +19,7 @@ Open a connection to the ROS node and return the `RobotConnection`. The `ip` must be a string formated as `"123.123.123.123"` """ -function open_robot_connection(ip::String, port::Integer=42421) +function open_robot_connection(ip::String, port::Integer) robot_connection = RobotConnection(ip, port) return robot_connection end @@ -34,7 +34,6 @@ function robot_connection_task(channel, socket) end msg = """{ "controls": $(controls) }\n""" write(socket, msg) - @info "Control commands sent" end close(socket) @info "Robot connection task completed" diff --git a/test/runtests.jl b/test/runtests.jl index 27dbff6..bce274e 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -10,22 +10,31 @@ function test_velocity_control() port = 42450 channel = Channel(1) server = listen(port) - errormonitor(@async begin + t = errormonitor(@async begin sock = accept(server) payload = readline(sock) put!(channel, payload) close(sock) end) + @test !istaskfailed(t) # connect to localhost, send data, then close the connection robot_connection = open_robot_connection(ip, port) + @test !istaskfailed(robot_connection.task) + @test !istaskdone(robot_connection.task) + send_control_commands(robot_connection, commands) + close_robot_connection(robot_connection, stop_robot=false) + @test istaskdone(robot_connection.task) # take the data from the channel and compare it to the sent data received_payload = take!(channel) data = JSON.parse(received_payload) + @test haskey(data, "controls") @test data["controls"] == commands + + close(server) end function test_state_feedback() @@ -36,28 +45,41 @@ function test_state_feedback() data["angular_vel"] = [.4,.5,.6] feedback_data = FeedbackData(data) - ip = ip"127.0.0.1" + # create a server on localhost that listens for a connection, reads a + # command, writes feedback data, and puts the received command on a channel + ip = "127.0.0.1" port = 42450 - timeout = 10.0 - channel = Channel(1) + timeout = 5.0 - # create a task that opens a feedback connection, recieves data, and then - # closes the connection - feedback_connection = open_feedback_connection(port) - task = errormonitor(@async begin - received_feedback_data = receive_feedback_data(feedback_connection, timeout) - put!(channel, received_feedback_data) - close_feedback_connection(feedback_connection) + channel = Channel(1) + server = listen(port) + t = errormonitor(@async begin + sock = accept(server) + payload = readline(sock) + put!(channel, payload) + write(sock,JSON.json(data)*"\n") + close(sock) end) + @test !istaskfailed(t) + + # open a feedback connection, recieve data, and then close the connection + feedback_connection = open_feedback_connection(ip, port) + @test !istaskfailed(feedback_connection.task) + @test !istaskdone(feedback_connection.task) - # send data to the feedback connection and compare the received data to the - # sent data - sleep(1.0) - sock = UDPSocket() - send(sock, ip, port, JSON.json(data)) - received_feedback_data = take!(channel) - wait(task) + received_feedback_data = receive_feedback_data(feedback_connection, timeout) @test received_feedback_data == feedback_data + + close_feedback_connection(feedback_connection) + @test istaskdone(feedback_connection.task) + + # check that the command sent to the feedback server is correct + received_payload = take!(channel) + json_cmd = JSON.parse(received_payload) + @test haskey(json_cmd, "action") + @test json_cmd["action"] == "get_feedback_data" + + close(server) end function Base.:(==)(x::FeedbackData, y::FeedbackData) @@ -67,7 +89,12 @@ function Base.:(==)(x::FeedbackData, y::FeedbackData) x.angular_vel == y.angular_vel end -@testset "RosSockets.jl" begin - test_velocity_control() - test_state_feedback() -end +@testset verbose = true "RosSockets.jl" begin + @testset "Velocity Control" begin + test_velocity_control() + end + + @testset "State Feedback" begin + test_state_feedback() + end +end \ No newline at end of file