diff --git a/include/t07_ros/Node.h b/include/t07_ros/Node.h index bf8d0f5..52978ac 100644 --- a/include/t07_ros/Node.h +++ b/include/t07_ros/Node.h @@ -73,6 +73,15 @@ class Node : public rclcpp::Node rclcpp::TimerBase::SharedPtr _motor_left_ctrl_loop_timer; void init_motor_left(); void motor_left_ctrl_loop(); + + rclcpp::QoS _motor_right_qos_profile; + rclcpp::SubscriptionOptions _motor_right_sub_options; + rclcpp::Subscription::SharedPtr _motor_right_sub; + quantity _motor_right_target; + cyphal::Publisher _motor_right_pwm_pub; + rclcpp::TimerBase::SharedPtr _motor_right_ctrl_loop_timer; + void init_motor_right(); + void motor_right_ctrl_loop(); }; /************************************************************************************** diff --git a/launch/t07.py b/launch/t07.py index 9a66699..3824de5 100644 --- a/launch/t07.py +++ b/launch/t07.py @@ -17,6 +17,10 @@ def generate_launch_description(): {'motor_left_topic_deadline_ms': 100}, {'motor_left_topic_liveliness_lease_duration': 1000}, {'motor_left_pwm_port_id': 600}, + {'motor_right_topic': '/motor/right/target'}, + {'motor_right_topic_deadline_ms': 100}, + {'motor_right_topic_liveliness_lease_duration': 1000}, + {'motor_right_pwm_port_id': 601}, ] ) ]) diff --git a/src/Node.cpp b/src/Node.cpp index 9310aaf..56ffd54 100644 --- a/src/Node.cpp +++ b/src/Node.cpp @@ -40,6 +40,12 @@ Node::Node() rmw_qos_profile_sensor_data } , _motor_left_target{0. * m/s} +, _motor_right_qos_profile +{ + rclcpp::KeepLast(1), + rmw_qos_profile_sensor_data +} +, _motor_right_target{0. * m/s} { init_cyphal_heartbeat(); init_cyphal_node_info(); @@ -64,6 +70,7 @@ Node::Node() }); init_motor_left(); + init_motor_right(); RCLCPP_INFO(get_logger(), "%s init complete.", get_name()); } @@ -176,6 +183,71 @@ void Node::motor_left_ctrl_loop() } } +void Node::init_motor_right() +{ + declare_parameter("motor_right_topic", "/motor/right/target"); + declare_parameter("motor_right_topic_deadline_ms", 100); + declare_parameter("motor_right_topic_liveliness_lease_duration", 1000); + declare_parameter("motor_right_pwm_port_id", 600); + + auto const motor_right_topic = get_parameter("motor_right_topic").as_string(); + auto const motor_right_topic_deadline = std::chrono::milliseconds(get_parameter("motor_right_topic_deadline_ms").as_int()); + auto const motor_right_topic_liveliness_lease_duration = std::chrono::milliseconds(get_parameter("motor_right_topic_liveliness_lease_duration").as_int()); + + _motor_right_qos_profile.deadline(motor_right_topic_deadline); + _motor_right_qos_profile.liveliness(RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC); + _motor_right_qos_profile.liveliness_lease_duration(motor_right_topic_liveliness_lease_duration); + + _motor_right_sub_options.event_callbacks.deadline_callback = + [this, motor_right_topic](rclcpp::QOSDeadlineRequestedInfo & /* event */) -> void + { + _motor_right_target = 0. * m/s; + RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(),5*1000UL, + "deadline missed for \"%s\", limiting _motor_right_target to %f m/s.", + motor_right_topic.c_str(), _motor_right_target.numerical_value_in(m/s)); + }; + + _motor_right_sub_options.event_callbacks.liveliness_callback = + [this, motor_right_topic](rclcpp::QOSLivelinessChangedInfo & event) -> void + { + if (event.alive_count == 0) + { + _motor_right_target = 0. * m/s; + RCLCPP_ERROR(get_logger(), + "liveliness lost for \"%s\", limiting _motor_right_target to %f m/s", + motor_right_topic.c_str(), _motor_right_target.numerical_value_in(m/s)); + } + }; + + _motor_right_sub = create_subscription( + motor_right_topic, + _motor_right_qos_profile, + [this](std_msgs::msg::Float32::SharedPtr const msg) { _motor_right_target = static_cast(msg->data) * m/s; }, + _motor_right_sub_options + ); + + _motor_right_pwm_pub = _node_hdl.create_publisher(get_parameter("motor_right_pwm_port_id").as_int(), 1*1000*1000UL); + + _motor_right_ctrl_loop_timer = create_wall_timer(CTRL_LOOP_RATE, [this]() { this->motor_right_ctrl_loop(); }); +} + +void Node::motor_right_ctrl_loop() +{ + if (_motor_right_target > 1. * m/s) + _motor_right_target = 1. * m/s; + + int16_t const motor_right_pwm_value = _motor_right_target.numerical_value_in(m/s) * 100; + + uavcan::primitive::scalar::Integer16_1_0 pwm_right_msg; + pwm_right_msg.value = motor_right_pwm_value; + + { + std::lock_guard lock(_node_mtx); + _motor_right_pwm_pub->publish(pwm_right_msg); + _node_hdl.spinSome(); + } +} + /************************************************************************************** * NAMESPACE **************************************************************************************/