diff --git a/drake_ros_examples/examples/iiwa_manipulator/iiwa_manipulator.cpp b/drake_ros_examples/examples/iiwa_manipulator/iiwa_manipulator.cpp index df11a760..4b78edc0 100644 --- a/drake_ros_examples/examples/iiwa_manipulator/iiwa_manipulator.cpp +++ b/drake_ros_examples/examples/iiwa_manipulator/iiwa_manipulator.cpp @@ -12,42 +12,37 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include +#include +#include + #include +#include #include #include #include #include #include - - -#include - #include #include #include -#include -#include -#include - using drake_ros_core::DrakeRos; using drake_ros_core::RosInterfaceSystem; using drake_ros_viz::RvizVisualizer; +using drake::examples::manipulation_station::ManipulationStation; using drake::systems::Adder; using drake::systems::ConstantVectorSource; -using drake::systems::Sine; using drake::systems::Simulator; -using drake::examples::manipulation_station::ManipulationStation; - +using drake::systems::Sine; -int main() -{ +int main() { drake::systems::DiagramBuilder builder; drake_ros_core::init(); auto ros_interface_system = builder.AddSystem( - std::make_unique("iiwa_manipulator_node")); + std::make_unique("iiwa_manipulator_node")); auto manipulation_station = builder.AddSystem(); manipulation_station->SetupClutterClearingStation(); @@ -55,64 +50,61 @@ int main() // Make the base joint swing sinusoidally. auto constant_term = builder.AddSystem( - drake::VectorX::Zero(manipulation_station->num_iiwa_joints())); + drake::VectorX::Zero(manipulation_station->num_iiwa_joints())); drake::VectorX amplitudes = - drake::VectorX::Zero(manipulation_station->num_iiwa_joints()); + drake::VectorX::Zero(manipulation_station->num_iiwa_joints()); amplitudes[0] = M_PI / 4.; // == 45 degrees - const drake::VectorX frequencies = - drake::VectorX::Constant(manipulation_station->num_iiwa_joints(), 1.); // Hz + const drake::VectorX frequencies = drake::VectorX::Constant( + manipulation_station->num_iiwa_joints(), 1.); // Hz const drake::VectorX phases = - drake::VectorX::Zero(manipulation_station->num_iiwa_joints()); + drake::VectorX::Zero(manipulation_station->num_iiwa_joints()); auto variable_term = builder.AddSystem(amplitudes, frequencies, phases); auto joint_trajectory_generator = - builder.AddSystem(2, manipulation_station->num_iiwa_joints()); - - builder.Connect( - constant_term->get_output_port(), - joint_trajectory_generator->get_input_port(0)); - builder.Connect( - variable_term->get_output_port(0), - joint_trajectory_generator->get_input_port(1)); - builder.Connect( - joint_trajectory_generator->get_output_port(), - manipulation_station->GetInputPort("iiwa_position")); + builder.AddSystem(2, manipulation_station->num_iiwa_joints()); + + builder.Connect(constant_term->get_output_port(), + joint_trajectory_generator->get_input_port(0)); + builder.Connect(variable_term->get_output_port(0), + joint_trajectory_generator->get_input_port(1)); + builder.Connect(joint_trajectory_generator->get_output_port(), + manipulation_station->GetInputPort("iiwa_position")); auto rviz_visualizer = builder.AddSystem( - ros_interface_system->get_ros_interface()); + ros_interface_system->get_ros_interface()); rviz_visualizer->RegisterMultibodyPlant( - &manipulation_station->get_multibody_plant()); + &manipulation_station->get_multibody_plant()); - builder.Connect( - manipulation_station->GetOutputPort("query_object"), - rviz_visualizer->get_graph_query_port()); + builder.Connect(manipulation_station->GetOutputPort("query_object"), + rviz_visualizer->get_graph_query_port()); auto diagram = builder.Build(); auto context = diagram->CreateDefaultContext(); - auto simulator = std::make_unique>(*diagram, std::move(context)); + auto simulator = + std::make_unique>(*diagram, std::move(context)); simulator->set_target_realtime_rate(1.0); simulator->Initialize(); - auto & simulator_context = simulator->get_mutable_context(); + auto& simulator_context = simulator->get_mutable_context(); - auto & manipulation_station_context = - diagram->GetMutableSubsystemContext(*manipulation_station, &simulator_context); + auto& manipulation_station_context = diagram->GetMutableSubsystemContext( + *manipulation_station, &simulator_context); - auto & constant_term_context = - diagram->GetMutableSubsystemContext(*constant_term, &simulator_context); + auto& constant_term_context = + diagram->GetMutableSubsystemContext(*constant_term, &simulator_context); // Fix gripper joints' position. manipulation_station->GetInputPort("wsg_position") - .FixValue(&manipulation_station_context, 0.); + .FixValue(&manipulation_station_context, 0.); // Use default positions for every joint but the base joint. - drake::systems::BasicVector & constants = - constant_term->get_mutable_source_value(&constant_term_context); + drake::systems::BasicVector& constants = + constant_term->get_mutable_source_value(&constant_term_context); constants.set_value( - manipulation_station->GetIiwaPosition(manipulation_station_context)); + manipulation_station->GetIiwaPosition(manipulation_station_context)); constants.get_mutable_value()[0] = -M_PI / 4.; while (true) { diff --git a/drake_ros_examples/examples/rs_flip_flop/rs_flip_flop.cpp b/drake_ros_examples/examples/rs_flip_flop/rs_flip_flop.cpp index 8b957b9d..1a1decbe 100644 --- a/drake_ros_examples/examples/rs_flip_flop/rs_flip_flop.cpp +++ b/drake_ros_examples/examples/rs_flip_flop/rs_flip_flop.cpp @@ -11,44 +11,40 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. + +#include +#include + #include #include #include - #include #include #include #include - #include -#include -#include - using drake_ros_core::DrakeRos; using drake_ros_core::RosInterfaceSystem; using drake_ros_core::RosPublisherSystem; using drake_ros_core::RosSubscriberSystem; -class NorGate : public drake::systems::LeafSystem -{ -public: - NorGate() - { - DeclareAbstractInputPort("A", *drake::AbstractValue::Make(std_msgs::msg::Bool())); - DeclareAbstractInputPort("B", *drake::AbstractValue::Make(std_msgs::msg::Bool())); +class NorGate : public drake::systems::LeafSystem { + public: + NorGate() { + DeclareAbstractInputPort( + "A", *drake::AbstractValue::Make(std_msgs::msg::Bool())); + DeclareAbstractInputPort( + "B", *drake::AbstractValue::Make(std_msgs::msg::Bool())); DeclareAbstractOutputPort("Q", &NorGate::calc_output_value); } virtual ~NorGate() = default; -private: - void - calc_output_value( - const drake::systems::Context & context, - std_msgs::msg::Bool * output) const - { + private: + void calc_output_value(const drake::systems::Context& context, + std_msgs::msg::Bool* output) const { const bool a = GetInputPort("A").Eval(context).data; const bool b = GetInputPort("B").Eval(context).data; output->data = !(a || b); @@ -57,48 +53,46 @@ class NorGate : public drake::systems::LeafSystem // Delay's input port by one timestep to avoid algebraic loop error // Inspired by Simulink's Memory block -template -class Memory : public drake::systems::LeafSystem -{ -public: - explicit Memory(const T & initial_value) - { +template +class Memory : public drake::systems::LeafSystem { + public: + explicit Memory(const T& initial_value) { DeclareAbstractInputPort("value", *drake::AbstractValue::Make(T())); // State for value DeclareAbstractState(drake::Value(initial_value)); // Output depends only on the previous state - DeclareAbstractOutputPort("value", &Memory::calc_output_value, {all_state_ticket()}); - - DeclarePerStepEvent( - drake::systems::UnrestrictedUpdateEvent( - [this]( - const drake::systems::Context & context, - const drake::systems::UnrestrictedUpdateEvent &, - drake::systems::State * state) { + DeclareAbstractOutputPort("value", &Memory::calc_output_value, + {all_state_ticket()}); + + DeclarePerStepEvent(drake::systems::UnrestrictedUpdateEvent( + [this](const drake::systems::Context& context, + const drake::systems::UnrestrictedUpdateEvent&, + drake::systems::State* state) { // Copy input value to state - drake::systems::AbstractValues & abstract_state = state->get_mutable_abstract_state(); + drake::systems::AbstractValues& abstract_state = + state->get_mutable_abstract_state(); abstract_state.get_mutable_value(0).SetFrom( - get_input_port().Eval(context)); + get_input_port().Eval(context)); })); } virtual ~Memory() = default; -private: - void - calc_output_value(const drake::systems::Context & context, T * output) const - { + private: + void calc_output_value(const drake::systems::Context& context, + T* output) const { *output = context.get_abstract_state().get_value(0).get_value(); } }; -int main() -{ +int main() { // NOR gate RS flip flop example - // Input topics /S and /R are active high (true is logic 1 and false is logic 0) - // Output topics /Q and /Q_not are active low (true is logic 0 and false is logic 1) + // Input topics /S and /R are active high (true is logic 1 and false is logic + // 0) + // Output topics /Q and /Q_not are active low (true is logic 0 and false is + // logic 1) // Input/Output table // S: false R: false | Q: no change Q_not: no change @@ -112,42 +106,50 @@ int main() drake_ros_core::init(); auto sys_ros_interface = builder.AddSystem( std::make_unique("rs_flip_flop_node")); - auto sys_pub_Q = builder.AddSystem( - RosPublisherSystem::Make( - "Q", qos, sys_ros_interface->get_ros_interface())); - auto sys_pub_Q_not = builder.AddSystem( - RosPublisherSystem::Make( - "Q_not", qos, sys_ros_interface->get_ros_interface())); - auto sys_sub_S = builder.AddSystem( - RosSubscriberSystem::Make( - "S", qos, sys_ros_interface->get_ros_interface())); - auto sys_sub_R = builder.AddSystem( - RosSubscriberSystem::Make( - "R", qos, sys_ros_interface->get_ros_interface())); + auto sys_pub_Q = + builder.AddSystem(RosPublisherSystem::Make( + "Q", qos, sys_ros_interface->get_ros_interface())); + auto sys_pub_Q_not = + builder.AddSystem(RosPublisherSystem::Make( + "Q_not", qos, sys_ros_interface->get_ros_interface())); + auto sys_sub_S = + builder.AddSystem(RosSubscriberSystem::Make( + "S", qos, sys_ros_interface->get_ros_interface())); + auto sys_sub_R = + builder.AddSystem(RosSubscriberSystem::Make( + "R", qos, sys_ros_interface->get_ros_interface())); auto sys_nor_gate_1 = builder.AddSystem(); auto sys_nor_gate_2 = builder.AddSystem(); - auto sys_memory = builder.AddSystem>(std_msgs::msg::Bool()); + auto sys_memory = + builder.AddSystem>(std_msgs::msg::Bool()); - builder.Connect(sys_nor_gate_1->GetOutputPort("Q"), sys_memory->get_input_port(0)); + builder.Connect(sys_nor_gate_1->GetOutputPort("Q"), + sys_memory->get_input_port(0)); - builder.Connect(sys_sub_S->get_output_port(0), sys_nor_gate_1->GetInputPort("A")); - builder.Connect(sys_nor_gate_2->GetOutputPort("Q"), sys_nor_gate_1->GetInputPort("B")); + builder.Connect(sys_sub_S->get_output_port(0), + sys_nor_gate_1->GetInputPort("A")); + builder.Connect(sys_nor_gate_2->GetOutputPort("Q"), + sys_nor_gate_1->GetInputPort("B")); - builder.Connect(sys_memory->get_output_port(0), sys_nor_gate_2->GetInputPort("A")); - builder.Connect(sys_sub_R->get_output_port(0), sys_nor_gate_2->GetInputPort("B")); + builder.Connect(sys_memory->get_output_port(0), + sys_nor_gate_2->GetInputPort("A")); + builder.Connect(sys_sub_R->get_output_port(0), + sys_nor_gate_2->GetInputPort("B")); - builder.Connect(sys_nor_gate_1->GetOutputPort("Q"), sys_pub_Q->get_input_port(0)); - builder.Connect(sys_nor_gate_2->GetOutputPort("Q"), sys_pub_Q_not->get_input_port(0)); + builder.Connect(sys_nor_gate_1->GetOutputPort("Q"), + sys_pub_Q->get_input_port(0)); + builder.Connect(sys_nor_gate_2->GetOutputPort("Q"), + sys_pub_Q_not->get_input_port(0)); auto diagram = builder.Build(); auto context = diagram->CreateDefaultContext(); - auto simulator = - std::make_unique>(*diagram, std::move(context)); + auto simulator = std::make_unique>( + *diagram, std::move(context)); simulator->set_target_realtime_rate(1.0); simulator->Initialize(); - auto & simulator_context = simulator->get_mutable_context(); + auto& simulator_context = simulator->get_mutable_context(); while (true) { simulator->AdvanceTo(simulator_context.get_time() + 0.1);