amikhalev/uaspire_poc
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Use timer instead of rate

Browse Source
This commit is contained in:
Alex Mikhalev 2019-09-25 19:34:08 -07:00
parent e2afc2875e
commit 24a7c07695
1 changed files with 11 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
src/uaspire_controls/main.cc
View File

@ -1,6 +1,7 @@
#include <functional> #include <functional>
#include <rclcpp/rclcpp.hpp> #include <rclcpp/rclcpp.hpp>
#include <rclcpp/timer.hpp>
#include <std_msgs/msg/string.hpp> #include <std_msgs/msg/string.hpp>
#include <uaspire_msgs/msg/command.hpp> #include <uaspire_msgs/msg/command.hpp>
#include <uaspire_msgs/msg/sensors.hpp> #include <uaspire_msgs/msg/sensors.hpp>
@ -15,14 +16,13 @@ class ControlsNode : public ros::Node
private: private:
ros::Subscription<UaspireCommand>::SharedPtr _command_subscription; ros::Subscription<UaspireCommand>::SharedPtr _command_subscription;
ros::Publisher<UaspireSensors>::SharedPtr _sensors_publisher; ros::Publisher<UaspireSensors>::SharedPtr _sensors_publisher;
ros::WallRate _rate; ros::TimerBase::SharedPtr _timer;
public: public:
RCLCPP_SHARED_PTR_DEFINITIONS(ControlsNode) RCLCPP_SHARED_PTR_DEFINITIONS(ControlsNode)
ControlsNode() ControlsNode()
: Node("uaspire_controls"), : Node("uaspire_controls")
_rate(1.0)
{ {
_command_subscription = create_subscription<UaspireCommand>( _command_subscription = create_subscription<UaspireCommand>(
"uaspire/commands", "uaspire/commands",
@ -33,6 +33,8 @@ public:
"uaspire/sensors", "uaspire/sensors",
ros::QoS(10) ros::QoS(10)
); );
_timer = create_wall_timer(std::chrono::milliseconds(1000),
std::bind(&ControlsNode::on_loop, this));
} }
void on_message(const UaspireCommand::SharedPtr cmd) void on_message(const UaspireCommand::SharedPtr cmd)
@ -40,15 +42,11 @@ public:
RCLCPP_INFO(get_logger(), "Command received: %s", cmd->data.c_str()); RCLCPP_INFO(get_logger(), "Command received: %s", cmd->data.c_str());
} }
void loop() { void on_loop() {
while (ros::ok()) { UaspireSensors sensors;
UaspireSensors sensors; sensors.position = ros::Clock(RCL_STEADY_TIME).now().seconds();
sensors.position = ros::Clock(RCL_STEADY_TIME).now().seconds(); _sensors_publisher->publish(sensors);
_sensors_publisher->publish(sensors); RCLCPP_INFO(get_logger(), "loop");
RCLCPP_INFO(get_logger(), "loop");
ros::spin_some(shared_from_this());
_rate.sleep();
}
} }
}; };
@ -56,7 +54,7 @@ int main(int argc, char** argv) {
ros::init(argc, argv); ros::init(argc, argv);
{ {
auto node = ControlsNode::make_shared(); auto node = ControlsNode::make_shared();
node->loop(); ros::spin(node);
} }
ros::shutdown(); ros::shutdown();
return 0; return 0;