Diff: main.cpp

Revision:

0:c177452db984

Child:

1:b20c9bcfb254

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Nov 02 09:00:16 2020 +0000
@@ -0,0 +1,128 @@
+#include "mbed.h"
+#include <string> 
+#include "JY901.h"
+#include "PID.h"
+#include "define.h"
+
+#include <ros.h>
+#include <geometry_msgs/Twist.h>
+#include <tf/transform_broadcaster.h>
+#include <tf/tf.h>
+#include <nav_msgs/Odometry.h>
+#include <geometry_msgs/Pose2D.h>
+#include <std_msgs/Empty.h>
+#include <std_srvs/Trigger.h>
+#include <std_srvs/Empty.h>
+
+#include "robot_operator.h"
+#include "device.h"
+
+// To do
+// robot_vel_controll 速度がそのまま周波数指令になっているため単位がおかしい
+// あまり大きい値の指令を出すとmbed実行エラーが起きるLED1点滅
+
+// (my_odomで返すvel_xは差分を取るより周波数から計算した方がいいか。多分良い)
+//
+
+ros::NodeHandle mecanum_node;
+
+void cmd_vel_callback(const geometry_msgs::Twist& cmd_vel){
+    moveStop();
+    robot_vel_controll(cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.angular.z);
+}
+
+void cmd_pose2d_callback(const geometry_msgs::Pose2D& cmd_pose2d){
+    //robot_pose_controll(cmd_pose2d.x, cmd_pose2d.y, cmd_pose2d.theta);
+    moveStart(cmd_pose2d.x, cmd_pose2d.y, cmd_pose2d.theta);
+}
+
+/*
+void move_start_service(const std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) {
+  res.success = true;
+  res.message = "Moving...";
+  moveStart(cmd_pose2d.x, cmd_pose2d.y, cmd_pose2d.theta);
+}*/
+
+void move_stop_service(const std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) {
+  res.success = true;
+  res.message = "Stop";
+  moveStop();
+}
+
+void odom_update()
+{
+    my_odometry.update();
+}
+
+//ros::ServiceServer<geometry_msgs::Pose2D,std_msgs::Empty> start_service("wheel/move_start", &move_start_service);
+ros::ServiceServer<std_srvs::Trigger::Request, std_srvs::Trigger::Response> stop_service("wheel/move_stop", &move_stop_service);
+ros::Subscriber<geometry_msgs::Twist> cmd_vel_sub("wheel/cmd_vel", &cmd_vel_callback);
+ros::Subscriber<geometry_msgs::Pose2D> cmd_pose_sub("wheel/cmd_pose2d", &cmd_pose2d_callback);
+
+int main() {
+    coastAllMotor();
+    mecanum_node.initNode();
+    mecanum_node.subscribe(cmd_vel_sub);
+    mecanum_node.subscribe(cmd_pose_sub);
+    //mecanum_node.advertiseService(start_service);
+    mecanum_node.advertiseService(stop_service);
+    
+    nav_msgs::Odometry odom;
+    ros::Publisher odom_pub("wheel/odom", &odom);
+    mecanum_node.advertise(odom_pub);
+    tf::TransformBroadcaster odom_broadcaster;
+    odom_broadcaster.init(mecanum_node);
+    
+    ros::Time current_time, last_time;
+    current_time = mecanum_node.now();
+    last_time = mecanum_node.now();
+    
+    Ticker ticker_odom;
+    ticker_odom.attach(&odom_update, 0.010);
+    
+    while (1) {
+        mecanum_node.spinOnce();
+        current_time = mecanum_node.now();
+        
+        wait_ms(500);
+        
+        //since all odometry is 6DOF we'll need a quaternion created from yaw
+        geometry_msgs::Quaternion odom_quat = tf::createQuaternionFromYaw(my_odometry.theta);
+    
+        //first, we'll publish the transform over tf
+        geometry_msgs::TransformStamped odom_trans;
+        odom_trans.header.stamp = current_time;
+        odom_trans.header.frame_id = "odom";
+        odom_trans.child_frame_id = "base_link";
+    
+        odom_trans.transform.translation.x = my_odometry.x;
+        odom_trans.transform.translation.y = my_odometry.y;
+        odom_trans.transform.translation.z = 0.0;
+        odom_trans.transform.rotation = odom_quat;
+    
+        //send the transform
+        odom_broadcaster.sendTransform(odom_trans);
+    
+        //next, we'll publish the odometry message over ROS
+        nav_msgs::Odometry odom;
+        odom.header.stamp = current_time;
+        odom.header.frame_id = "odom";
+    
+        //set the position
+        odom.pose.pose.position.x = my_odometry.x;
+        odom.pose.pose.position.y = my_odometry.y;
+        odom.pose.pose.position.z = 0.0;
+        odom.pose.pose.orientation = odom_quat;
+    
+        //set the velocity
+        odom.child_frame_id = "base_link";
+        odom.twist.twist.linear.x = my_odometry.vel_x;
+        odom.twist.twist.linear.y = my_odometry.vel_y;
+        odom.twist.twist.angular.z = my_odometry.vel_theta;
+    
+        //publish the message
+        odom_pub.publish(&odom);
+    }
+}
+
+