2 #include "std_msgs/String.h"
3 #include "std_msgs/Float64.h"
10 #include <controller_manager/controller_manager.h>
17 int main(
int argc,
char **argv)
21 ros::init(argc, argv,
"Mia_hw_node");
26 if (ros::param::has(
"~Mia_fs_"))
28 ros::param::get(
"~Mia_fs_", fs_rate);
33 ros::param::set(
"~Mia_fs_", fs_rate);
37 ros::AsyncSpinner spinner(1);
44 bool init_success = mia_hw.
init(nh,nh);
48 ROS_ERROR_NAMED(
"Mia_hw_node",
"Error initializing mia_hw_interface.\n");
52 controller_manager::ControllerManager cm(&mia_hw,nh);
56 ROS_INFO_NAMED(
"Mia_hw_node",
"Mia_hw_interface started");
61 mia_hw.
read (ros::Time::now(), rate.expectedCycleTime());
62 cm.update (ros::Time::now(), rate.expectedCycleTime());
63 mia_hw.
write(ros::Time::now(), rate.expectedCycleTime());