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

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@0:7fb71f2b3259, 2022-03-19 (annotated)

Committer:
smoritaemb
Date:
Sat Mar 19 23:21:06 2022 +0900
Revision:
0:7fb71f2b3259
Child:
1:596a3e446612
Initial commit.

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 0:7fb71f2b3259 59 while (1)
smoritaemb 0:7fb71f2b3259 60 {
smoritaemb 0:7fb71f2b3259 61 linear.x = COEFF_LIN * (inputA0.read() - initialA);
smoritaemb 0:7fb71f2b3259 62 angular.z = COEFF_ANG * (inputA1.read() - initialA);
smoritaemb 0:7fb71f2b3259 63 twist.linear = linear;
smoritaemb 0:7fb71f2b3259 64 twist.angular = angular;
smoritaemb 0:7fb71f2b3259 65 pub.publish(twist);
smoritaemb 0:7fb71f2b3259 66 osDelay(100);
smoritaemb 0:7fb71f2b3259 67 }
smoritaemb 0:7fb71f2b3259 68
smoritaemb 0:7fb71f2b3259 69 mros2::spin();
smoritaemb 0:7fb71f2b3259 70 return 0;
smoritaemb 0:7fb71f2b3259 71 }