WRS2020用にメカナム台車をROS化
Dependencies: mbed PID2 JY901 QuaternionMath ros_lib_melodic i2cmaster WRS2020_mecanum_node
Dependents: WRS2020_mecanum_node
main.cpp@4:40167450cdf0, 2020-11-12 (annotated)
- Committer:
- sgrsn
- Date:
- Thu Nov 12 07:36:18 2020 +0000
- Revision:
- 4:40167450cdf0
- Parent:
- 3:cf06189a7511
Fix quaternion, Switch X and Y axis
Who changed what in which revision?
User | Revision | Line number | New 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 |