Add SpringController valve command #51

andermi · 2022-05-27T19:12:39Z

Addresses #44 Valve command

Test with:
ros2 service call /valve_command buoy_msgs/srv/ValveCommand duration_sec:\ 10\

Can watch /sc_record/range_finder in rqt_plot. Value should rise ~0.25 m in 10 sec

Can close the valve with:
ros2 service call /valve_command buoy_msgs/srv/ValveCommand duration_sec:\ 0\

Modeling the valve (need to clean up w/ parameter):

void PolytropicPneumaticSpring::openValve(
  const int dt_nano,
  double & P1, double & V1,
  double & P2, double & V2)
{
  // want piston to drop 1 inch per second
  // so let mass flow from lower chamber to upper
  // results in initial pressure change (P0 -- or P1, P2 with hysteresis) via ideal gas law
  double dt_sec = dt_nano * 1e-9;
  static const double absement{5e-6};  // measure of valve opening cross-section and duration
                                       // units of meter-seconds
  double mass_flow = absement*this->dataPtr->P;  // P in Pascals (kg/(m*s^2)) so, mass_flow in kg/s
  double delta_mass = dt_sec*mass_flow;  // kg of gas per step
  if (this->dataPtr->is_upper)
  {
    this->dataPtr->mass += delta_mass;
    P1 = this->dataPtr->mass * this->dataPtr->R * this->dataPtr->T0 / V1;
    P2 = this->dataPtr->mass * this->dataPtr->R * this->dataPtr->T0 / V2;
  }
  else
  {
    this->dataPtr->mass -= delta_mass;
    P1 = this->dataPtr->mass * this->dataPtr->R * this->dataPtr->T0 / V1;
    P2 = this->dataPtr->mass * this->dataPtr->R * this->dataPtr->T0 / V2;
  }
}

opening for 5 seconds using component:

spring_state_comp->Data().valve_command = true;
spring_state_comp->Data().command_duration = 5s;

lowers piston from 0.72 to 0.85 meters (range_finder value measured from top of upper chamber) in ~5.5 seconds
results in dropping 0.0236 (~1 inch) per second

Todo:

Model valve
Add command service to SpringController

Signed-off-by: Michael Anderson <[email protected]>

…m/osrf/buoy_sim into andermi/springcontroller_commands

Signed-off-by: Michael Anderson <[email protected]>

hamilton8415 · 2022-05-27T23:43:46Z

Looks good, nice way to characterize mass flow as a function of pressure differential abasement. absement is a new one for me: http://wearcam.org/absement/examples.htm This same technique will apply to the pump command as well, different sign and magnitude of course...

It looks like you have test code in there to control the time this valve is active, I'll be watching to see how you integrate the command and timeouts from the ROS2 spring controller code and will copy for similar behaviors in ElectroHydraulicPTO. At the moment I simply respond to ignition transport callouts and maintain a time-out in the plugin.

Signed-off-by: Michael Anderson <[email protected]>

andermi · 2022-05-28T01:18:29Z

integrate the command and timeouts

I made use of the ignition::math::Stopwatch in my SpringState component in the ECM

  ignition::math::Stopwatch command_watch;
  ignition::math::clock::duration command_duration;
  bool valve_command{false};
  bool pump_command{false};

and manage the stopwatch like this:

manageCommandTimer(SpringState & state)
{
  if (state.valve_command || state.pump_command) {
    if (!state.command_watch.Running()) {
      if (state.valve_command) {
        ignmsg << "Valve opening (" << \
          std::chrono::duration_cast<std::chrono::seconds>(state.command_duration).count() << \
          "s)" << std::endl;
      }
      if (state.pump_command) {
        ignmsg << "Pump on (" << \
          std::chrono::duration_cast<std::chrono::seconds>(state.command_duration).count() << \
          "s)" << std::endl;
      }
      state.command_watch.Start(true);
    } else {
      if (state.command_watch.ElapsedRunTime() >= \
        state.command_duration)
      {
        state.command_watch.Stop();

        if (state.valve_command) {
          state.valve_command = false;
          ignmsg << "Valve closed" << std::endl;
        }

        if (state.pump_command) {
          state.pump_command = false;
          ignmsg << "Pump off" << std::endl;
        }
      }
    }
  } else if (!state.valve_command && !state.pump_command) {
    if (state.command_watch.Running()) {
      state.command_watch.Stop();
      ignmsg << "Valve closed / Pump off" << std::endl;
    }
  }
}

I changed the implementation of this a bit in d7e4747

andermi · 2022-05-28T01:25:22Z

I added (osrf/mbari_wec_utils#11) a BUSY flag in the command service response to indicate a command is already running so as to not issue a new valve command when we are already running the valve or pump. @hamilton8415 should the behavior instead be to preempt the current command? (e.g. stop the pump if I issue a valve, or extend the valve time if I issue another valve)

Signed-off-by: Michael Anderson <[email protected]>

hamilton8415 · 2022-05-28T01:45:40Z

In both the pump and valve commands, the behavior of the hardware is a new command sets the timeout to the new specified value. So if you send a "valve 10" and then 5 seconds later send another "valve 10", the valve will be open for a total of 15 seconds. Same for the pump.

andermi · 2022-05-28T04:45:47Z

Ok, I'll modify the behavior then.

chapulina

I did a high-level review and didn't look into the math or output behaviour.

Can close the valve with:
ros2 service call /valve_command buoy_msgs/srv/ValveCommand duration_sec:\ 0\

Should I see a message like "Valve closed" printed when I run this command?

buoy_gazebo/src/PolytropicPneumaticSpring/CMakeLists.txt

buoy_gazebo/src/PolytropicPneumaticSpring/PolytropicPneumaticSpring.cpp

buoy_gazebo/src/controllers/SpringController/SpringController.cpp

Signed-off-by: Michael Anderson <[email protected]>

andermi · 2022-05-31T22:15:12Z

Should I see a message like "Valve closed" printed when I run this command?

I changed these to ROS2 console prints now.

In the latest commits, I modified behavior to follow what @hamilton8415 mentioned. Also, cleaned up the command logic, and addressed PR comments.

Behavior when issuing console commands (don't mind the truncated duration errors for printing):

[ign gazebo-1] [INFO] [1654030769.374244685] [spring_controller]: [ROS 2 Spring Control] ValveCommand Received (10s)
[ign gazebo-1] [INFO] [1654030769.374641468] [spring_controller]: Valve open (10s)
[ign gazebo-1] [INFO] [1654030773.070602607] [spring_controller]: [ROS 2 Spring Control] ValveCommand Received (0s)
[ign gazebo-1] [INFO] [1654030773.071846096] [spring_controller]: Valve closed after (3s)
[ign gazebo-1] [INFO] [1654030787.287722168] [spring_controller]: [ROS 2 Spring Control] ValveCommand Received (10s)
[ign gazebo-1] [INFO] [1654030787.288250843] [spring_controller]: Valve open (10s)
[ign gazebo-1] [INFO] [1654030790.001460840] [spring_controller]: [ROS 2 Spring Control] ValveCommand Received (10s)
[ign gazebo-1] [INFO] [1654030790.002160845] [spring_controller]: Valve open (12s)
[ign gazebo-1] [INFO] [1654030792.750108155] [spring_controller]: [ROS 2 Spring Control] ValveCommand Received (10s)
[ign gazebo-1] [INFO] [1654030792.750821277] [spring_controller]: Valve open (15s)
[ign gazebo-1] [INFO] [1654030802.751982084] [spring_controller]: Valve closed after (15s)

Signed-off-by: Michael Anderson <[email protected]>

chapulina · 2022-06-01T00:54:53Z

if you send a "valve 10" and then 5 seconds later send another "valve 10", the valve will be open for a total of 15 seconds

I think real time and sim time are getting mixed somewhere.

I'm trying to run this scenario like this:

Start simulation paused:
ign gazebo mbari_wec.sdf -v 3
Send a command to open for 10s:
ros2 service call /valve_command buoy_msgs/srv/ValveCommand duration_sec:\ 10
Step the simulation for 5 s (5000 iterations)
ign service -s /world/world_demo/control --reqtype ignition.msgs.WorldControl --reptype ignition.msgs.Boolean --timeout 1000 --req "pause: true, step: true, multi_step: 5000"
Send a command to open for 10s:
ros2 service call /valve_command buoy_msgs/srv/ValveCommand duration_sec:\ 10
It printed [INFO] [1654044735.192398374] [spring_controller]: Valve open (54s), I think that's counting real time, not sim time
Step for 5 seconds more:
ign service -s /world/world_demo/control --reqtype ignition.msgs.WorldControl --reptype ignition.msgs.Boolean --timeout 1000 --req "pause: true, step: true, multi_step: 5000"
It printed
```
[Msg] Valve closed after (103s)
[INFO] [1654044794.627076135] [spring_controller]: Valve closed after (103s)
```
But we're at 10 s of simulation, I'd expect the valve not to close until 15 s.

andermi · 2022-06-01T01:33:51Z

I think real time and sim time are getting mixed somewhere.

@chapulina Is this an issue with using Stopwatch?

Would Timer follow sim time?

Hmm, the comments at the top of the file say wall time. Is there a timer that follows sim time?

Signed-off-by: Michael Anderson <[email protected]>

andermi · 2022-06-01T22:40:16Z

Is there a timer that follows sim time?

@chapulina I made my own 🤓

your test should now work with sim time:
$ ros2 launch buoy_gazebo mbari_wec.launch.py
(need launch file for sim clock bridge from gz to ros2)

$ ros2 service call /valve_command buoy_msgs/srv/ValveCommand duration_sec:\ 10
requester: making request: buoy_msgs.srv.ValveCommand_Request(duration_sec=10)

response:
buoy_msgs.srv.ValveCommand_Response(result=buoy_msgs.msg.PBCommandResponse(value=0))

[ROS 2 Spring Control] ValveCommand Received (10s)
Valve open (10s)

step sim ~5sec:

$ ign service -s /world/world_demo/control --reqtype ignition.msgs.WorldControl --reptype ignition.msgs.Boolean --timeout 1000 --req "pause: true, step: true, multi_step: 5000"
data: true

send another command:

$ ros2 service call /valve_command buoy_msgs/srv/ValveCommand duration_sec:\ 10
requester: making request: buoy_msgs.srv.ValveCommand_Request(duration_sec=10)

response:
buoy_msgs.srv.ValveCommand_Response(result=buoy_msgs.msg.PBCommandResponse(value=0))

[ROS 2 Spring Control] ValveCommand Received (10s)
Valve open (15s)

step again ~10sec:

$ ign service -s /world/world_demo/control --reqtype ignition.msgs.WorldControl --reptype ignition.msgs.Boolean --timeout 1000 --req "pause: true, step: true, multi_step: 10000"
data: true

Valve closed after (15s)

Signed-off-by: Michael Anderson <[email protected]>

chapulina

Works for me! Nice ROS-based sim time stopwatch!

buoy_gazebo/src/PolytropicPneumaticSpring/SpringState.hpp

buoy_gazebo/src/buoy_utils/StopwatchSimTime.hpp

chapulina · 2022-06-02T23:15:47Z

buoy_gazebo/src/buoy_utils/StopwatchSimTime.hpp

+///   timeSys.ElapsedRunTime()).count() << " ms\n";
+/// watch.Stop();
+/// ```
+class StopwatchSimTime


Nice! This looks like something that could be generally useful. We should consider moving it somewhere like ros_ign_gazebo.

It should also work with pure ROS2 using /clock published from any source (and use_sim_time param set per node). May want to put it in rclcpp somewhere?

Actually, should work in ROS2 with any time source really. It just takes an instance of an rclcpp::clock from the rclcpp::Node.

Yeah true this is way more generic than I originally thought. I'm not sure what the best place would be, the rclcpp repo already looks very large. I'll ask around

Signed-off-by: Michael Anderson <[email protected]>

andermi added 4 commits May 26, 2022 18:13

adding valve command

9738091

Signed-off-by: Michael Anderson <[email protected]>

Merge branch 'andermi/springcontroller_commands' of https://github.co…

e7b8074

…m/osrf/buoy_sim into andermi/springcontroller_commands

valve modelled to drop piston 1 inch per second; now add command

4698c68

Signed-off-by: Michael Anderson <[email protected]>

cleanup

9cf7f5c

Signed-off-by: Michael Anderson <[email protected]>

andermi requested review from chapulina and hamilton8415 May 27, 2022 19:12

andermi changed the base branch from main to andermi/springcontroller_commands May 27, 2022 19:14

Base automatically changed from andermi/springcontroller_commands to main May 27, 2022 19:27

andermi linked an issue May 27, 2022 that may be closed by this pull request

Command services in ROS2 Spring Controller plugin #44

Closed

6 tasks

andermi added 2 commits May 27, 2022 12:44

colcon test

20720fb

Signed-off-by: Michael Anderson <[email protected]>

Merge branch 'main' into andermi/springcontroller_valve

082d7c5

added valve service to spring controller

9d7b2a7

Signed-off-by: Michael Anderson <[email protected]>

andermi mentioned this pull request May 28, 2022

add busy flag to command response osrf/mbari_wec_utils#11

Merged

get valve close command working

968d553

Signed-off-by: Michael Anderson <[email protected]>

andermi marked this pull request as ready for review May 28, 2022 01:40

chapulina reviewed May 31, 2022

View reviewed changes

andermi added 2 commits May 31, 2022 14:01

modify command behavior to add duration to elapsed on new command

d7e4747

Signed-off-by: Michael Anderson <[email protected]>

address PR comments

a4a41a7

Signed-off-by: Michael Anderson <[email protected]>

andermi added 2 commits May 31, 2022 16:08

use pressure differential in valve mass_flow calc

a61c92c

Signed-off-by: Michael Anderson <[email protected]>

cleanup debug prints

b660fd9

Signed-off-by: Michael Anderson <[email protected]>

andermi requested a review from chapulina May 31, 2022 23:26

andermi added 2 commits June 1, 2022 15:05

make stopwatch use simtime

6b7e093

Signed-off-by: Michael Anderson <[email protected]>

colcon test

e8861e5

Signed-off-by: Michael Anderson <[email protected]>

andermi added 3 commits June 1, 2022 17:35

remove dead code

b9f03cb

Signed-off-by: Michael Anderson <[email protected]>

cleanup

dbdb260

Signed-off-by: Michael Anderson <[email protected]>

whitespace

c3b240b

Signed-off-by: Michael Anderson <[email protected]>

chapulina approved these changes Jun 2, 2022

View reviewed changes

PR tweaks

b748d07

Signed-off-by: Michael Anderson <[email protected]>

andermi merged commit bacb549 into main Jun 6, 2022

andermi deleted the andermi/springcontroller_valve branch June 6, 2022 17:56

chapulina mentioned this pull request Jun 24, 2022

GUI dashboard for the buoy #66

Closed

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Add SpringController valve command #51

Add SpringController valve command #51

andermi commented May 27, 2022 •

edited

Loading

hamilton8415 commented May 27, 2022

andermi commented May 28, 2022 •

edited

Loading

andermi commented May 28, 2022 •

edited

Loading

hamilton8415 commented May 28, 2022

andermi commented May 28, 2022

chapulina left a comment

andermi commented May 31, 2022

chapulina commented Jun 1, 2022

andermi commented Jun 1, 2022 •

edited

Loading

andermi commented Jun 1, 2022

chapulina left a comment

chapulina Jun 2, 2022

andermi Jun 3, 2022

andermi Jun 3, 2022

chapulina Jun 3, 2022

Add SpringController valve command #51

Add SpringController valve command #51

Conversation

andermi commented May 27, 2022 • edited Loading

hamilton8415 commented May 27, 2022

andermi commented May 28, 2022 • edited Loading

andermi commented May 28, 2022 • edited Loading

hamilton8415 commented May 28, 2022

andermi commented May 28, 2022

chapulina left a comment

Choose a reason for hiding this comment

andermi commented May 31, 2022

chapulina commented Jun 1, 2022

andermi commented Jun 1, 2022 • edited Loading

andermi commented Jun 1, 2022

chapulina left a comment

Choose a reason for hiding this comment

chapulina Jun 2, 2022

Choose a reason for hiding this comment

andermi Jun 3, 2022

Choose a reason for hiding this comment

andermi Jun 3, 2022

Choose a reason for hiding this comment

chapulina Jun 3, 2022

Choose a reason for hiding this comment

andermi commented May 27, 2022 •

edited

Loading

andermi commented May 28, 2022 •

edited

Loading

andermi commented May 28, 2022 •

edited

Loading

andermi commented Jun 1, 2022 •

edited

Loading