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@5:17ede03f1fa5, 2021-02-23 (annotated)

Committer:
sgrsn
Date:
Tue Feb 23 10:22:26 2021 +0000
Revision:
5:17ede03f1fa5
Parent:
4:40167450cdf0 
Lateset commit;

Who changed what in which revision?

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