S Morita / Mbed OS example-mbed-mros2-mturtle-teleop

Imported mturtle_teleop_joy demo below for mros2 on Mbed. https://raw.githubusercontent.com/mROS-base/mros2-mbed/main/workspace/mturtle_teleop/app.cpp

Dependencies:   mbed-mros2

Please refer to the README below.

https://github.com/mROS-base/mros2-mbed/blob/main/workspace/mturtle_teleop/README.md

main.cpp@1:596a3e446612, 2022-03-19 (annotated)

Committer:
smoritaemb
Date:
Sat Mar 19 16:03:15 2022 +0000
Revision:
1:596a3e446612
Parent:
0:7fb71f2b3259
Child:
2:b7bba9a5f4a5
Added simple console mode.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
smoritaemb 0:7fb71f2b3259 1 /* mros2 example
smoritaemb 0:7fb71f2b3259 2 * Copyright (c) 2021 smorita_emb
smoritaemb 0:7fb71f2b3259 3 *
smoritaemb 0:7fb71f2b3259 4 * Licensed under the Apache License, Version 2.0 (the "License");
smoritaemb 0:7fb71f2b3259 5 * you may not use this file except in compliance with the License.
smoritaemb 0:7fb71f2b3259 6 * You may obtain a copy of the License at
smoritaemb 0:7fb71f2b3259 7 *
smoritaemb 0:7fb71f2b3259 8 * http://www.apache.org/licenses/LICENSE-2.0
smoritaemb 0:7fb71f2b3259 9 *
smoritaemb 0:7fb71f2b3259 10 * Unless required by applicable law or agreed to in writing, software
smoritaemb 0:7fb71f2b3259 11 * distributed under the License is distributed on an "AS IS" BASIS,
smoritaemb 0:7fb71f2b3259 12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
smoritaemb 0:7fb71f2b3259 13 * See the License for the specific language governing permissions and
smoritaemb 0:7fb71f2b3259 14 * limitations under the License.
smoritaemb 0:7fb71f2b3259 15 */
smoritaemb 0:7fb71f2b3259 16
smoritaemb 0:7fb71f2b3259 17 #include "mbed.h"
smoritaemb 0:7fb71f2b3259 18 #include "mros2.h"
smoritaemb 0:7fb71f2b3259 19 #include "geometry_msgs/msg/vector3.hpp"
smoritaemb 0:7fb71f2b3259 20 #include "geometry_msgs/msg/twist.hpp"
smoritaemb 0:7fb71f2b3259 21 #include "EthernetInterface.h"
smoritaemb 0:7fb71f2b3259 22
smoritaemb 0:7fb71f2b3259 23 #define IP_ADDRESS ("192.168.11.2") /* IP address */
smoritaemb 0:7fb71f2b3259 24 #define SUBNET_MASK ("255.255.255.0") /* Subnet mask */
smoritaemb 0:7fb71f2b3259 25 #define DEFAULT_GATEWAY ("192.168.11.1") /* Default gateway */
smoritaemb 0:7fb71f2b3259 26
smoritaemb 0:7fb71f2b3259 27 AnalogIn inputA0(A0);
smoritaemb 0:7fb71f2b3259 28 AnalogIn inputA1(A1);
smoritaemb 0:7fb71f2b3259 29
smoritaemb 0:7fb71f2b3259 30 #define COEFF_LIN 10.0
smoritaemb 0:7fb71f2b3259 31 #define COEFF_ANG 10.0
smoritaemb 0:7fb71f2b3259 32
smoritaemb 0:7fb71f2b3259 33 int main() {
smoritaemb 0:7fb71f2b3259 34 EthernetInterface network;
smoritaemb 0:7fb71f2b3259 35 network.set_dhcp(false);
smoritaemb 0:7fb71f2b3259 36 network.set_network(IP_ADDRESS, SUBNET_MASK, DEFAULT_GATEWAY);
smoritaemb 0:7fb71f2b3259 37 nsapi_size_or_error_t result = network.connect();
smoritaemb 0:7fb71f2b3259 38
smoritaemb 0:7fb71f2b3259 39 printf("mbed mros2 start!\r\n");
smoritaemb 0:7fb71f2b3259 40 printf("app name: mturtle_teleop_joy\r\n");
smoritaemb 0:7fb71f2b3259 41 mros2::init(0, NULL);
smoritaemb 0:7fb71f2b3259 42 MROS2_DEBUG("mROS 2 initialization is completed\r\n");
smoritaemb 0:7fb71f2b3259 43
smoritaemb 0:7fb71f2b3259 44 mros2::Node node = mros2::Node::create_node("mturtle_teleop_joy");
smoritaemb 0:7fb71f2b3259 45 mros2::Publisher pub = node.create_publisher<geometry_msgs::msg::Twist>("turtle1/cmd_vel", 10);
smoritaemb 0:7fb71f2b3259 46 osDelay(100);
smoritaemb 0:7fb71f2b3259 47 MROS2_INFO("ready to pub/sub message\r\n");
smoritaemb 0:7fb71f2b3259 48
smoritaemb 0:7fb71f2b3259 49 geometry_msgs::msg::Vector3 linear;
smoritaemb 0:7fb71f2b3259 50 geometry_msgs::msg::Vector3 angular;
smoritaemb 0:7fb71f2b3259 51 geometry_msgs::msg::Twist twist;
smoritaemb 0:7fb71f2b3259 52 linear.y = 0;
smoritaemb 0:7fb71f2b3259 53 linear.z = 0;
smoritaemb 0:7fb71f2b3259 54 angular.x = 0;
smoritaemb 0:7fb71f2b3259 55 angular.y = 0;
smoritaemb 0:7fb71f2b3259 56
smoritaemb 0:7fb71f2b3259 57 MROS2_INFO("publish Twist msg to turtlesim according to the input from Joystick module");
smoritaemb 0:7fb71f2b3259 58 float initialA = inputA0.read();
smoritaemb 1:596a3e446612 59 pollfh fds[1];
smoritaemb 1:596a3e446612 60 fds[0].fh = mbed::mbed_file_handle(STDIN_FILENO);
smoritaemb 1:596a3e446612 61 fds[0].events = POLLIN;
smoritaemb 1:596a3e446612 62 bool console_mode = false;
smoritaemb 1:596a3e446612 63
smoritaemb 0:7fb71f2b3259 64 while (1)
smoritaemb 1:596a3e446612 65 {
smoritaemb 1:596a3e446612 66 if (console_mode || poll(fds, 1, 0))
smoritaemb 1:596a3e446612 67 {
smoritaemb 1:596a3e446612 68 console_mode = true;
smoritaemb 1:596a3e446612 69 if (poll(fds, 1, 0))
smoritaemb 1:596a3e446612 70 {
smoritaemb 1:596a3e446612 71 char c;
smoritaemb 1:596a3e446612 72 mbed::mbed_file_handle(STDIN_FILENO)->read(&c, 1);
smoritaemb 1:596a3e446612 73 switch (c)
smoritaemb 1:596a3e446612 74 {
smoritaemb 1:596a3e446612 75 case 'w':
smoritaemb 1:596a3e446612 76 linear.x = COEFF_LIN * 5.0;
smoritaemb 1:596a3e446612 77 break;
smoritaemb 1:596a3e446612 78 case 's':
smoritaemb 1:596a3e446612 79 linear.x = 0.0;
smoritaemb 1:596a3e446612 80 angular.z = 0.0;
smoritaemb 1:596a3e446612 81 break;
smoritaemb 1:596a3e446612 82 case 'x':
smoritaemb 1:596a3e446612 83 linear.x = -COEFF_LIN * 5.0;
smoritaemb 1:596a3e446612 84 break;
smoritaemb 1:596a3e446612 85 case 'a':
smoritaemb 1:596a3e446612 86 angular.z = -COEFF_ANG * 5.0;
smoritaemb 1:596a3e446612 87 break;
smoritaemb 1:596a3e446612 88 case 'd':
smoritaemb 1:596a3e446612 89 angular.z = COEFF_ANG * 5.0;
smoritaemb 1:596a3e446612 90 break;
smoritaemb 1:596a3e446612 91 case 'q':
smoritaemb 1:596a3e446612 92 linear.x = 0.0;
smoritaemb 1:596a3e446612 93 angular.z = 0.0;
smoritaemb 1:596a3e446612 94 console_mode = false;
smoritaemb 1:596a3e446612 95 break;
smoritaemb 1:596a3e446612 96 }
smoritaemb 1:596a3e446612 97 }
smoritaemb 1:596a3e446612 98 } else {
smoritaemb 1:596a3e446612 99 linear.x = COEFF_LIN * (inputA0.read() - initialA);
smoritaemb 1:596a3e446612 100 angular.z = COEFF_ANG * (inputA1.read() - initialA);
smoritaemb 1:596a3e446612 101 }
smoritaemb 0:7fb71f2b3259 102 twist.linear = linear;
smoritaemb 0:7fb71f2b3259 103 twist.angular = angular;
smoritaemb 0:7fb71f2b3259 104 pub.publish(twist);
smoritaemb 0:7fb71f2b3259 105 osDelay(100);
smoritaemb 0:7fb71f2b3259 106 }
smoritaemb 0:7fb71f2b3259 107
smoritaemb 0:7fb71f2b3259 108 mros2::spin();
smoritaemb 0:7fb71f2b3259 109 return 0;
smoritaemb 0:7fb71f2b3259 110 }