1+ #include < chrono>
2+ #include < functional>
3+ #include < memory>
4+ #include < string>
5+
6+ #include " rclcpp/rclcpp.hpp"
7+ #include " std_msgs/msg/float64.hpp"
8+ #include < stdio.h>
9+ #include < unistd.h>
10+ #include < termios.h>
11+
12+ #include < map>
13+ using namespace std ::chrono_literals;
14+
15+ /* This example creates a subclass of Node and uses std::bind() to register a
16+ * member function as a callback from the timer. */
17+
18+
19+ int getch (void )
20+ {
21+ int ch;
22+ struct termios oldt;
23+ struct termios newt;
24+
25+ // Store old settings, and copy to new settings
26+ tcgetattr (STDIN_FILENO, &oldt);
27+ newt = oldt;
28+
29+ // Make required changes and apply the settings
30+ newt.c_lflag &= ~(ICANON | ECHO);
31+ newt.c_iflag |= IGNBRK;
32+ newt.c_iflag &= ~(INLCR | ICRNL | IXON | IXOFF);
33+ newt.c_lflag &= ~(ICANON | ECHO | ECHOK | ECHOE | ECHONL | ISIG | IEXTEN);
34+ newt.c_cc [VMIN] = 1 ;
35+ newt.c_cc [VTIME] = 0 ;
36+ tcsetattr (fileno (stdin), TCSANOW, &newt);
37+
38+ // Get the current character
39+ ch = getchar ();
40+
41+ // Reapply old settings
42+ tcsetattr (STDIN_FILENO, TCSANOW, &oldt);
43+
44+ return ch;
45+ }
46+
47+ int main (int argc, char * argv[])
48+ {
49+ const char * msg = R"(
50+ Reading from the keyboard!
51+ ---------------------------
52+ Thrust Key:
53+ w
54+
55+ s
56+
57+ Lower Blades:
58+
59+ a d
60+
61+
62+ Upper Blades:
63+ u
64+
65+ j
66+
67+ t : up (+z)
68+ b : down (-z)
69+ q/e : reverse thrust
70+ w/s : increase/decrease Thrust Key by 10
71+ a/d : increase/decrease Lower Blade angle by 1
72+ u/j : increase/decrease Upper Blade angle by 1
73+ NOTE : Increasing or Decreasing will take affect live on the moving robot.
74+ Consider Stopping the robot before changing it.
75+ CTRL-C to quit
76+ )"
77+ rclcpp::init (argc, argv);
78+ auto node = rclcpp::Node::make_shared (" uuv_teleop"
79+ std::cout<<msg<<std::endl;
80+ auto publisher_ver = node->create_publisher <std_msgs::msg::Float64 >(" /lower_blade" 10 );
81+ auto publisher_hoz = node->create_publisher <std_msgs::msg::Float64 >(" /upper_blade" 10 );
82+ auto publisher_thrust = node->create_publisher <std_msgs::msg::Float64 >(" /thrust" 10 );
83+ auto message_ver = std_msgs::msg::Float64 ();
84+ auto message_hoz = std_msgs::msg::Float64 ();
85+ auto message_thrust = std_msgs::msg::Float64 ();
86+ message_ver.data = 0 ;
87+ message_hoz.data = 0 ;
88+ message_thrust.data = 0 ;
89+
90+ while (rclcpp::ok ())
91+ {
92+ char key = getch ();
93+
94+ if (key == ' A' ' a'
95+ if (message_ver.data <= -1.0 ){
96+ message_ver.data = message_ver.data ;
97+ std::cout<<" Lower Blade Angle:- " data <<std::endl;
98+ }
99+ else {
100+ message_ver.data = message_ver.data - 1 ;
101+ publisher_ver->publish (message_ver);
102+ std::cout<<" Lower Blade Angle:- " data <<std::endl;
103+ }
104+ }
105+ if (key == ' D' ' d'
106+ if (message_ver.data >= 1.0 ){
107+ message_ver.data = message_ver.data ;
108+ std::cout<<" Lower Blade Angle:- " data <<std::endl;
109+ }
110+ else {
111+ message_ver.data = message_ver.data + 1 ;
112+ publisher_ver->publish (message_ver);
113+ std::cout<<" Lower Blade Angle:- " data <<std::endl;
114+ }
115+ }
116+ if (key == ' u' ' U'
117+ if (message_hoz.data <= -1.0 ){
118+ message_hoz.data = message_hoz.data ;
119+ std::cout<<" Upper Blade Angle:- " data <<std::endl;
120+ }
121+ else {
122+ message_hoz.data = message_hoz.data - 1 ;
123+ publisher_hoz->publish (message_hoz);
124+ std::cout<<" Upper Blade Angle:- " data <<std::endl;
125+ }
126+ }
127+ if (key == ' j' ' J'
128+ if (message_hoz.data >= 1.0 ){
129+ message_hoz.data = message_hoz.data ;
130+ std::cout<<" Upper Blade Angle:- " data <<std::endl;
131+ }
132+ else {
133+ message_hoz.data = message_hoz.data + 1 ;
134+ publisher_hoz->publish (message_hoz);
135+ std::cout<<" Upper Blade Angle:- " data <<std::endl;
136+ }
137+ }
138+ if (key == ' q' ' Q'
139+ if (message_thrust.data <=0 ){
140+ message_thrust.data = message_thrust.data ;
141+ publisher_thrust->publish (message_thrust);
142+ std::cout<<" Thrust Value:- " data <<std::endl;
143+ }
144+ else {
145+ message_thrust.data = - message_thrust.data ;
146+ publisher_thrust->publish (message_thrust);
147+ std::cout<<" Thrust Value:- " data <<std::endl;
148+ }
149+
150+ }
151+ if (key == ' e' ' E'
152+ if (message_thrust.data >=0 ){
153+ message_thrust.data = message_thrust.data ;
154+ publisher_thrust->publish (message_thrust);
155+ std::cout<<" Thrust Value:- " data <<std::endl;
156+ }
157+ else {
158+ message_thrust.data = - message_thrust.data ;
159+ publisher_thrust->publish (message_thrust);
160+ std::cout<<" Thrust Value:- " data <<std::endl;
161+ }
162+
163+ }
164+ if (key == ' w' ' W'
165+ message_thrust.data = message_thrust.data + 10 ;
166+ publisher_thrust->publish (message_thrust);
167+ std::cout<<" Thrust Value (UP):- " data <<std::endl;
168+ }
169+
170+ if (key == ' s' ' S'
171+ message_thrust.data = message_thrust.data - 10 ;
172+ publisher_thrust->publish (message_thrust);
173+ std::cout<<" Thrust Value (DOWN):- " data <<std::endl;
174+ }
175+
176+ else if (key == ' \x03 '
177+ break ;
178+ }
179+ }
180+
181+
182+ rclcpp::shutdown ();
183+ return 0 ;
184+ }
0 commit comments