hidaka sato / Mbed 2 deprecated WRS2020_mecanum_node

WRS2020用にメカナム台車をROS化

Dependencies:   mbed PID2 JY901 QuaternionMath ros_lib_melodic i2cmaster WRS2020_mecanum_node

Dependents:   WRS2020_mecanum_node

main.cpp@0:c177452db984, 2020-11-02 (annotated)

Committer:
sgrsn
Date:
Mon Nov 02 09:00:16 2020 +0000
Revision:
0:c177452db984
Child:
1:b20c9bcfb254
First commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sgrsn 0:c177452db984 1 #include "mbed.h"
sgrsn 0:c177452db984 2 #include <string>
sgrsn 0:c177452db984 3 #include "JY901.h"
sgrsn 0:c177452db984 4 #include "PID.h"
sgrsn 0:c177452db984 5 #include "define.h"
sgrsn 0:c177452db984 6
sgrsn 0:c177452db984 7 #include <ros.h>
sgrsn 0:c177452db984 8 #include <geometry_msgs/Twist.h>
sgrsn 0:c177452db984 9 #include <tf/transform_broadcaster.h>
sgrsn 0:c177452db984 10 #include <tf/tf.h>
sgrsn 0:c177452db984 11 #include <nav_msgs/Odometry.h>
sgrsn 0:c177452db984 12 #include <geometry_msgs/Pose2D.h>
sgrsn 0:c177452db984 13 #include <std_msgs/Empty.h>
sgrsn 0:c177452db984 14 #include <std_srvs/Trigger.h>
sgrsn 0:c177452db984 15 #include <std_srvs/Empty.h>
sgrsn 0:c177452db984 16
sgrsn 0:c177452db984 17 #include "robot_operator.h"
sgrsn 0:c177452db984 18 #include "device.h"
sgrsn 0:c177452db984 19
sgrsn 0:c177452db984 20 // To do
sgrsn 0:c177452db984 21 // robot_vel_controll 速度がそのまま周波数指令になっているため単位がおかしい
sgrsn 0:c177452db984 22 // あまり大きい値の指令を出すとmbed実行エラーが起きるLED1点滅
sgrsn 0:c177452db984 23
sgrsn 0:c177452db984 24 // (my_odomで返すvel_xは差分を取るより周波数から計算した方がいいか。多分良い)
sgrsn 0:c177452db984 25 //
sgrsn 0:c177452db984 26
sgrsn 0:c177452db984 27 ros::NodeHandle mecanum_node;
sgrsn 0:c177452db984 28
sgrsn 0:c177452db984 29 void cmd_vel_callback(const geometry_msgs::Twist& cmd_vel){
sgrsn 0:c177452db984 30 moveStop();
sgrsn 0:c177452db984 31 robot_vel_controll(cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.angular.z);
sgrsn 0:c177452db984 32 }
sgrsn 0:c177452db984 33
sgrsn 0:c177452db984 34 void cmd_pose2d_callback(const geometry_msgs::Pose2D& cmd_pose2d){
sgrsn 0:c177452db984 35 //robot_pose_controll(cmd_pose2d.x, cmd_pose2d.y, cmd_pose2d.theta);
sgrsn 0:c177452db984 36 moveStart(cmd_pose2d.x, cmd_pose2d.y, cmd_pose2d.theta);
sgrsn 0:c177452db984 37 }
sgrsn 0:c177452db984 38
sgrsn 0:c177452db984 39 /*
sgrsn 0:c177452db984 40 void move_start_service(const std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) {
sgrsn 0:c177452db984 41 res.success = true;
sgrsn 0:c177452db984 42 res.message = "Moving...";
sgrsn 0:c177452db984 43 moveStart(cmd_pose2d.x, cmd_pose2d.y, cmd_pose2d.theta);
sgrsn 0:c177452db984 44 }*/
sgrsn 0:c177452db984 45
sgrsn 0:c177452db984 46 void move_stop_service(const std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) {
sgrsn 0:c177452db984 47 res.success = true;
sgrsn 0:c177452db984 48 res.message = "Stop";
sgrsn 0:c177452db984 49 moveStop();
sgrsn 0:c177452db984 50 }
sgrsn 0:c177452db984 51
sgrsn 0:c177452db984 52 void odom_update()
sgrsn 0:c177452db984 53 {
sgrsn 0:c177452db984 54 my_odometry.update();
sgrsn 0:c177452db984 55 }
sgrsn 0:c177452db984 56
sgrsn 0:c177452db984 57 //ros::ServiceServer<geometry_msgs::Pose2D,std_msgs::Empty> start_service("wheel/move_start", &move_start_service);
sgrsn 0:c177452db984 58 ros::ServiceServer<std_srvs::Trigger::Request, std_srvs::Trigger::Response> stop_service("wheel/move_stop", &move_stop_service);
sgrsn 0:c177452db984 59 ros::Subscriber<geometry_msgs::Twist> cmd_vel_sub("wheel/cmd_vel", &cmd_vel_callback);
sgrsn 0:c177452db984 60 ros::Subscriber<geometry_msgs::Pose2D> cmd_pose_sub("wheel/cmd_pose2d", &cmd_pose2d_callback);
sgrsn 0:c177452db984 61
sgrsn 0:c177452db984 62 int main() {
sgrsn 0:c177452db984 63 coastAllMotor();
sgrsn 0:c177452db984 64 mecanum_node.initNode();
sgrsn 0:c177452db984 65 mecanum_node.subscribe(cmd_vel_sub);
sgrsn 0:c177452db984 66 mecanum_node.subscribe(cmd_pose_sub);
sgrsn 0:c177452db984 67 //mecanum_node.advertiseService(start_service);
sgrsn 0:c177452db984 68 mecanum_node.advertiseService(stop_service);
sgrsn 0:c177452db984 69
sgrsn 0:c177452db984 70 nav_msgs::Odometry odom;
sgrsn 0:c177452db984 71 ros::Publisher odom_pub("wheel/odom", &odom);
sgrsn 0:c177452db984 72 mecanum_node.advertise(odom_pub);
sgrsn 0:c177452db984 73 tf::TransformBroadcaster odom_broadcaster;
sgrsn 0:c177452db984 74 odom_broadcaster.init(mecanum_node);
sgrsn 0:c177452db984 75
sgrsn 0:c177452db984 76 ros::Time current_time, last_time;
sgrsn 0:c177452db984 77 current_time = mecanum_node.now();
sgrsn 0:c177452db984 78 last_time = mecanum_node.now();
sgrsn 0:c177452db984 79
sgrsn 0:c177452db984 80 Ticker ticker_odom;
sgrsn 0:c177452db984 81 ticker_odom.attach(&odom_update, 0.010);
sgrsn 0:c177452db984 82
sgrsn 0:c177452db984 83 while (1) {
sgrsn 0:c177452db984 84 mecanum_node.spinOnce();
sgrsn 0:c177452db984 85 current_time = mecanum_node.now();
sgrsn 0:c177452db984 86
sgrsn 0:c177452db984 87 wait_ms(500);
sgrsn 0:c177452db984 88
sgrsn 0:c177452db984 89 //since all odometry is 6DOF we'll need a quaternion created from yaw
sgrsn 0:c177452db984 90 geometry_msgs::Quaternion odom_quat = tf::createQuaternionFromYaw(my_odometry.theta);
sgrsn 0:c177452db984 91
sgrsn 0:c177452db984 92 //first, we'll publish the transform over tf
sgrsn 0:c177452db984 93 geometry_msgs::TransformStamped odom_trans;
sgrsn 0:c177452db984 94 odom_trans.header.stamp = current_time;
sgrsn 0:c177452db984 95 odom_trans.header.frame_id = "odom";
sgrsn 0:c177452db984 96 odom_trans.child_frame_id = "base_link";
sgrsn 0:c177452db984 97
sgrsn 0:c177452db984 98 odom_trans.transform.translation.x = my_odometry.x;
sgrsn 0:c177452db984 99 odom_trans.transform.translation.y = my_odometry.y;
sgrsn 0:c177452db984 100 odom_trans.transform.translation.z = 0.0;
sgrsn 0:c177452db984 101 odom_trans.transform.rotation = odom_quat;
sgrsn 0:c177452db984 102
sgrsn 0:c177452db984 103 //send the transform
sgrsn 0:c177452db984 104 odom_broadcaster.sendTransform(odom_trans);
sgrsn 0:c177452db984 105
sgrsn 0:c177452db984 106 //next, we'll publish the odometry message over ROS
sgrsn 0:c177452db984 107 nav_msgs::Odometry odom;
sgrsn 0:c177452db984 108 odom.header.stamp = current_time;
sgrsn 0:c177452db984 109 odom.header.frame_id = "odom";
sgrsn 0:c177452db984 110
sgrsn 0:c177452db984 111 //set the position
sgrsn 0:c177452db984 112 odom.pose.pose.position.x = my_odometry.x;
sgrsn 0:c177452db984 113 odom.pose.pose.position.y = my_odometry.y;
sgrsn 0:c177452db984 114 odom.pose.pose.position.z = 0.0;
sgrsn 0:c177452db984 115 odom.pose.pose.orientation = odom_quat;
sgrsn 0:c177452db984 116
sgrsn 0:c177452db984 117 //set the velocity
sgrsn 0:c177452db984 118 odom.child_frame_id = "base_link";
sgrsn 0:c177452db984 119 odom.twist.twist.linear.x = my_odometry.vel_x;
sgrsn 0:c177452db984 120 odom.twist.twist.linear.y = my_odometry.vel_y;
sgrsn 0:c177452db984 121 odom.twist.twist.angular.z = my_odometry.vel_theta;
sgrsn 0:c177452db984 122
sgrsn 0:c177452db984 123 //publish the message
sgrsn 0:c177452db984 124 odom_pub.publish(&odom);
sgrsn 0:c177452db984 125 }
sgrsn 0:c177452db984 126 }
sgrsn 0:c177452db984 127
sgrsn 0:c177452db984 128