58 ROS_INFO(
"dynamixel_workbench_wheel : Init OK!");
70 struct termios oldt, newt;
73 tcgetattr( STDIN_FILENO, &oldt );
75 newt.c_lflag &= ~(ICANON | ECHO);
78 tcsetattr( STDIN_FILENO, TCSANOW, &newt );
80 tcsetattr( STDIN_FILENO, TCSANOW, &oldt );
87 struct termios oldt, newt;
91 tcgetattr(STDIN_FILENO, &oldt);
93 newt.c_lflag &= ~(ICANON | ECHO);
94 tcsetattr(STDIN_FILENO, TCSANOW, &newt);
95 oldf = fcntl(STDIN_FILENO, F_GETFL, 0);
96 fcntl(STDIN_FILENO, F_SETFL, oldf | O_NONBLOCK);
100 tcsetattr(STDIN_FILENO, TCSANOW, &oldt);
101 fcntl(STDIN_FILENO, F_SETFL, oldf);
111 int main(
int argc,
char **argv)
114 ros::init(argc, argv,
"dynamixel_workbench_wheel");
115 ROS_INFO(
"Set angular velocity(+-0.2 rad/sec) to your motor!! by using keyboard");
124 dynamixel_workbench_msgs::SetDirection srv;
129 if (dynamixel_wheel.
kbhit())
131 char c = dynamixel_wheel.
getch();
140 srv.request.right_wheel_velocity = 0.2;
141 srv.request.left_wheel_velocity = 0.2;
145 srv.request.right_wheel_velocity = -0.2;
146 srv.request.left_wheel_velocity = -0.2;
150 srv.request.right_wheel_velocity = 0.2;
151 srv.request.left_wheel_velocity = -0.2;
155 srv.request.right_wheel_velocity = -0.2;
156 srv.request.left_wheel_velocity = 0.2;
160 srv.request.right_wheel_velocity = 0.0;
161 srv.request.left_wheel_velocity = 0.0;
165 srv.request.right_wheel_velocity = 0.2;
166 srv.request.left_wheel_velocity = 0.2;
172 ROS_INFO(
"[LEFT_WHEEL_VELOCITY]: %.2f, [RIGHT_WHEEL_VELOCITY]: %.2f", srv.response.left_wheel_velocity, srv.response.right_wheel_velocity);
177 ROS_ERROR(
"Failed to call service /wheel");
ServiceClient serviceClient(const std::string &service_name, bool persistent=false, const M_string &header_values=M_string())
DynamixelWorkbenchWheel()
~DynamixelWorkbenchWheel()
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
bool call(MReq &req, MRes &res)
int main(int argc, char **argv)
bool shutdownDynamixelWorkbenchWheel(void)
bool initDynamixelWorkbenchWheel(void)
ros::ServiceClient wheel_control_client_
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
ROSCPP_DECL void shutdown()
ROSCPP_DECL void spinOnce()