Diff: main.cpp

Revision:

3:cf06189a7511

Parent:

1:b20c9bcfb254

Child:

4:40167450cdf0

--- a/main.cpp	Mon Nov 02 11:29:15 2020 +0000
+++ b/main.cpp	Wed Nov 11 07:34:11 2020 +0000
@@ -4,12 +4,15 @@
 #include "PID.h"
 #include "define.h"
 
+#include "Quaternion.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 <geometry_msgs/PoseStamped.h>
 #include <std_msgs/Empty.h>
 #include <std_srvs/Trigger.h>
 #include <std_srvs/Empty.h>
@@ -36,12 +39,11 @@
     moveStart(cmd_pose2d.x, cmd_pose2d.y, cmd_pose2d.theta, &is_stop);
 }
 
-/*
-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 goal_callback(const geometry_msgs::PoseStamped& goal){
+    //robot_pose_controll(cmd_pose2d.x, cmd_pose2d.y, cmd_pose2d.theta);
+    Quaternion tmp_q(goal.pose.orientation.w, goal.pose.orientation.x, goal.pose.orientation.y, goal.pose.orientation.z);
+    moveStart(goal.pose.position.x, goal.pose.position.y, tmp_q.getEulerAngles().z, &is_stop);
+}
 
 void move_stop_service(const std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) {
   res.success = true;
@@ -56,21 +58,18 @@
 
 //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::ServiceClient<std_srvs::Trigger::Request, std_srvs::Trigger::Response> stop_call_client("stop_call");
 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);
+//ros::Subscriber<geometry_msgs::Pose2D> cmd_pose_sub("wheel/cmd_pose2d", &cmd_pose2d_callback);
+ros::Subscriber<geometry_msgs::PoseStamped> goal_sub("/move_base_simple/goal", &goal_callback);
 
 int main() {
+    BusOut LEDs(LED1, LED2, LED3, LED4);
     coastAllMotor();
     mecanum_node.initNode();
     mecanum_node.subscribe(cmd_vel_sub);
-    mecanum_node.subscribe(cmd_pose_sub);
-    //mecanum_node.advertiseService(start_service);
+    mecanum_node.subscribe(goal_sub);
     mecanum_node.advertiseService(stop_service);
     
-    std_srvs::Trigger::Request req;
-    std_srvs::Trigger::Response res;
-    
     nav_msgs::Odometry odom;
     ros::Publisher odom_pub("wheel/odom", &odom);
     mecanum_node.advertise(odom_pub);
@@ -88,13 +87,16 @@
         mecanum_node.spinOnce();
         current_time = mecanum_node.now();
         
+        wait_ms(500);
+        
         if(is_stop)
         {
-            stop_call_client.call(req, res);
-            is_stop = false;
+            LEDs = 0x0F;
         }
-        
-        wait_ms(500);
+        else
+        {
+            LEDs = LEDs+1;
+        }
         
         //since all odometry is 6DOF we'll need a quaternion created from yaw
         geometry_msgs::Quaternion odom_quat = tf::createQuaternionFromYaw(my_odometry.theta);