Remove collision subscription.

MACH1-ROBOT · May 9, 2023 · 7d5d5f4 · 7d5d5f4
1 parent 434df13
commit 7d5d5f4
Showing 1 changed file with 11 additions and 32 deletions.
diff --git a/src/vel_switch.cpp b/src/vel_switch.cpp
@@ -1,9 +1,6 @@
-#include <algorithm>
 #include <geometry_msgs/Twist.h>
-#include <mach1_msgs/Collision.h>
-#include <mach1_msgs/VelCmd.h>
+#include <mach1_msgs/VelStatus.h>
 #include <map>
-#include <memory>
 #include <mutex>
 #include <ros/ros.h>
 
@@ -12,29 +9,25 @@ class VelSwitch
   public:
     enum class NavMode
     {
-        MODE_0,
         MODE_AUTO,
         MODE_JOY,
         MODE_KEY,
     };
     VelSwitch();
     void joystick_vel_callback(const geometry_msgs::Twist::ConstPtr &vel);
     void keyboard_vel_callback(const geometry_msgs::Twist::ConstPtr &vel);
-    void rear_collision_callback(const mach1_msgs::Collision::ConstPtr &data);
     void loop();
 
   private:
     ros::NodeHandle nh_;
     ros::Publisher vel_status_pub;
     ros::Subscriber joystick_vel_sub;
     ros::Subscriber keyboard_vel_sub;
-    ros::Subscriber rear_collision_sub;
-    ros::Rate loop_rate{20};
+    ros::Rate loop_rate{10};
     std::mutex vel_msg_mutex;
-    mach1_msgs::VelCmd vel_msg;
+    mach1_msgs::VelStatus vel_msg;
 
     std::map<NavMode, bool> navModes = {
-        {NavMode::MODE_0, false},
         {NavMode::MODE_AUTO, false},
         {NavMode::MODE_JOY, false},
         {NavMode::MODE_KEY, false},
@@ -43,12 +36,10 @@ class VelSwitch
 
 VelSwitch::VelSwitch()
 {
-    vel_status_pub = nh_.advertise<mach1_msgs::VelCmd>("/vel_status", 1);
+    vel_status_pub = nh_.advertise<mach1_msgs::VelStatus>("/vel_status", 1);
     joystick_vel_sub = nh_.subscribe("/joy_cmd_vel", 10, &VelSwitch::joystick_vel_callback, this);
     keyboard_vel_sub =
         nh_.subscribe("/keyboard_cmd_vel", 10, &VelSwitch::keyboard_vel_callback, this);
-    rear_collision_sub =
-        nh_.subscribe("/rear_collision", 10, &VelSwitch::rear_collision_callback, this);
 };
 
 void VelSwitch::joystick_vel_callback(const geometry_msgs::Twist::ConstPtr &vel)
@@ -67,29 +58,13 @@ void VelSwitch::keyboard_vel_callback(const geometry_msgs::Twist::ConstPtr &vel)
     navModes[NavMode::MODE_KEY] = true;
 };   // release the lock when lock_guard goes out of scope
 
-void VelSwitch::rear_collision_callback(const mach1_msgs::Collision::ConstPtr &data)
-{
-    if (data->distance < data->min_collision_dist)
-    {
-        std::lock_guard<std::mutex> lock(vel_msg_mutex);
-        vel_msg.twist_msg.linear.x = 0;
-        vel_msg.twist_msg.angular.z = 0;
-        navModes[NavMode::MODE_0] = true;
-    }   // release the lock when lock_guard goes out of scope
-};
-
 void VelSwitch::loop()
 {
     while (ros::ok())
     {
         {
             std::lock_guard<std::mutex> lock(vel_msg_mutex);
-            if (navModes[NavMode::MODE_0])
-            {
-                vel_msg.nav_type = "OFF";
-                navModes[NavMode::MODE_0] = false;
-            }
-            else if (navModes[NavMode::MODE_JOY])
+            if (navModes[NavMode::MODE_JOY])
             {
                 vel_msg.nav_type = "JOYSTICK";
                 navModes[NavMode::MODE_JOY] = false;
@@ -106,9 +81,13 @@ void VelSwitch::loop()
             }
             else
             {
-                vel_msg.nav_type = "NONE";
+                vel_msg.twist_msg.angular.z = 0;
+                vel_msg.twist_msg.linear.x = 0;
                 vel_msg.teleop_enabled = false;
+                vel_msg.nav_type = "NONE";
             }
+            vel_status_pub.publish(vel_msg);
+
         }   // release the lock when lock_guard goes out of scope
         loop_rate.sleep();
         ros::spinOnce();
@@ -117,7 +96,7 @@ void VelSwitch::loop()
 
 int main(int argc, char **argv)
 {
-    ros::init(argc, argv, "vel_mux_node");
+    ros::init(argc, argv, "vel_switch_node");
     VelSwitch VelSwitch;
     VelSwitch.loop();
     return 0;