From 93e0840dd10996553ea744fcf279bf41fcea0bca Mon Sep 17 00:00:00 2001
From: thirgy4 <thirgy4@googlemail.com>
Date: Wed, 3 Nov 2021 12:49:52 +0000
Subject: [PATCH] Working cpp listener | python script not detected

---
 .../CMakeLists.txt                            |  3 +-
 .../include/objectDet.h                       | 24 +++++++++
 .../launch/keyboard_command.launch            |  4 +-
 .../spot_micro_keyboard_command/objectDet.cpp | 26 ----------
 .../postObjDetect.py                          | 45 +++++++++++++++++
 .../scripts/postObjDetect.py                  | 29 -----------
 .../src/objectDet.cpp                         | 50 +++++++++++++++++++
 7 files changed, 123 insertions(+), 58 deletions(-)
 create mode 100644 RWR/src/spot_micro_keyboard_command/include/objectDet.h
 delete mode 100644 RWR/src/spot_micro_keyboard_command/objectDet.cpp
 create mode 100644 RWR/src/spot_micro_keyboard_command/postObjDetect.py
 delete mode 100644 RWR/src/spot_micro_keyboard_command/scripts/postObjDetect.py
 create mode 100644 RWR/src/spot_micro_keyboard_command/src/objectDet.cpp

diff --git a/RWR/src/spot_micro_keyboard_command/CMakeLists.txt b/RWR/src/spot_micro_keyboard_command/CMakeLists.txt
index 29689dd..367f7c4 100644
--- a/RWR/src/spot_micro_keyboard_command/CMakeLists.txt
+++ b/RWR/src/spot_micro_keyboard_command/CMakeLists.txt
@@ -138,7 +138,8 @@ include_directories(
 ## With catkin_make all packages are built within a single CMake context
 ## The recommended prefix ensures that target names across packages don't collide
 #add_executable(${PROJECT_NAME}_node src/spot_micro_keyboard_command_node.cpp)
-add_executable(move ~/3rdYear/RWR/Robot-With-Reflexes/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_keyboard_command/objectDet.cpp)
+
+add_executable(move src/objectDet.cpp)
 
 add_dependencies(move
   ${${PROJECT_NAME}_EXPORTED_TARGETS}
diff --git a/RWR/src/spot_micro_keyboard_command/include/objectDet.h b/RWR/src/spot_micro_keyboard_command/include/objectDet.h
new file mode 100644
index 0000000..15831a0
--- /dev/null
+++ b/RWR/src/spot_micro_keyboard_command/include/objectDet.h
@@ -0,0 +1,24 @@
+#ifndef OBJECT_DET_H
+#define OBJECT_DET_H
+
+#include <ros/ros.h>
+#include <std_msgs/Bool.h>
+
+class ObjectDet
+{
+    public:
+        ObjectDet(ros::NodeHandle & nh);
+        ~ObjectDet();
+
+    private:
+        bool called;
+        ros::NodeHandle nh_;
+        ros::Publisher walk_cmd;
+        ros::Publisher stand_cmd;
+        ros::Subscriber obj_det;
+        //ros::Subscriber curr_state;
+
+        void ObjectDetectedCallback(const std_msgs::Bool::ConstPtr& det);
+};
+
+#endif //OBJECT_DET_H
\ No newline at end of file
diff --git a/RWR/src/spot_micro_keyboard_command/launch/keyboard_command.launch b/RWR/src/spot_micro_keyboard_command/launch/keyboard_command.launch
index 9442cac..bdf260e 100644
--- a/RWR/src/spot_micro_keyboard_command/launch/keyboard_command.launch
+++ b/RWR/src/spot_micro_keyboard_command/launch/keyboard_command.launch
@@ -14,7 +14,7 @@
     <node name="spot_micro_keyboard_command_node" pkg="spot_micro_keyboard_command" type="spotMicroKeyboardMove.py" output="screen">
     </node>
 
-    <!-- Run the hard left node -->
+    <!-- Run the stand and walk node -->
     <node name="postObjDetMovement" pkg="spot_micro_keyboard_command" type="postObjDetect.py" output="screen">
     </node>
 
@@ -25,7 +25,7 @@
 
     <!-- If run_rviz is true, run the rviz node by including it's launch file -->
     <group if="$(arg run_rviz)">
-        <include file="$(find spot_micro_rviz)/launch/show_model.launch" />
+        <include file="$(find spot_micro_rviz)/launch/show_and_move_model_via_gui.launch" />
     </group>
     
 </launch>
diff --git a/RWR/src/spot_micro_keyboard_command/objectDet.cpp b/RWR/src/spot_micro_keyboard_command/objectDet.cpp
deleted file mode 100644
index d7222ba..0000000
--- a/RWR/src/spot_micro_keyboard_command/objectDet.cpp
+++ /dev/null
@@ -1,26 +0,0 @@
-#include <ros/ros.h>
-#include <geometry_msgs/Twist.h>
-#include <tf/transform_datatypes.h>
-#include <std_msgs/Bool.h>
-
-int main(int argc, char **argv) {
-    ros::init(argc, argv, "move_publisher");
-    ros::NodeHandle nh;
-    ros::Publisher stand_pub = nh.advertise<std_msgs::Bool>("/stand_cmd", 1000);
-    ros::Publisher walk_pub = nh.advertise<std_msgs::Bool>("/walk_cmd",1000); 
-
-    //ros::Rate loop_rate(10);
-    //int count = 0;
-
-    std_msgs::Bool stand_msg;
-    std_msgs::Bool walk_msg;
-    stand_msg.data = true;
-    walk_msg.data = true;
-
-    stand_pub.publish(stand_msg);
-    //walk_pub.publish(walk_msg);
-
-    //ros::spinOnce();
-    
-    return 0;
-}
\ No newline at end of file
diff --git a/RWR/src/spot_micro_keyboard_command/postObjDetect.py b/RWR/src/spot_micro_keyboard_command/postObjDetect.py
new file mode 100644
index 0000000..9fd1402
--- /dev/null
+++ b/RWR/src/spot_micro_keyboard_command/postObjDetect.py
@@ -0,0 +1,45 @@
+#!/usr/bin/python3
+
+import rospy
+import sys, select, termios, tty # For terminal keyboard key press reading
+from std_msgs.msg import Bool 
+from math import pi
+
+called = False
+
+def objCallback(obj_det):
+    #inputArgs = Twist()
+    called = False
+
+    if obj_det == True:
+        called = True
+        stand_msg = Bool()
+        stand_msg.data = True
+        walk_msg = Bool()
+        walk_msg.data = True
+
+        obj_det = rospy.Subscriber('/obj_det', Bool)
+
+        standPub = rospy.Publisher('/stand_cmd',Bool,queue_size=1)
+        walkPub = rospy.Publish('/walk_cmd',Bool, queue_size=1)
+
+        standPub.publish(stand_msg)
+        walkPub.publish(walk_msg)
+
+        rospy.loginfo("issued obj detected - transitioning to walk mode")
+
+    else:
+        if called == True:
+            rospy.loginfo("issued obj detected already")
+        else:
+            rospy.loginfo("No obj called from command line")
+
+    
+def listener():
+    rospy.init_node('obj_det_sub')
+    rospy.Subscriber('/obj_det', Bool, objCallback, queue_size=1)
+    rospy.Spin()
+
+if __name__ == '__main__':
+    listener()
+    #obj_det = rospy.Subscriber('/obj_det', Bool, objCallback, queue_size=1)
diff --git a/RWR/src/spot_micro_keyboard_command/scripts/postObjDetect.py b/RWR/src/spot_micro_keyboard_command/scripts/postObjDetect.py
deleted file mode 100644
index deb6672..0000000
--- a/RWR/src/spot_micro_keyboard_command/scripts/postObjDetect.py
+++ /dev/null
@@ -1,29 +0,0 @@
-#!/usr/bin/python3
-
-import rospy
-import sys, select, termios, tty # For terminal keyboard key press reading
-from std_msgs.msg import Bool 
-from geometry_msgs.msg import Vector3
-from geometry_msgs.msg import Twist
-from math import pi
-
-def stand_walk():
-    inputArgs = Twist()
-
-    stand_msg = Bool()
-    stand_msg.data = True
-    walk_msg = Bool()
-    walk_msg.data = True
-
-    standPub = rospy.Publisher('/stand_cmd',Bool,queue_size=1)
-    walkPub = rospy.Publish('/walk_cmd',Bool, queue_size=1)
-
-    standPub.publish(stand_msg)
-    walkPub.publish(walk_msg)
-
-    rospy.loginfo("issued obj detected - transitioning to walk mode")
-    
-
-
-if __name__ == '__main__':
-    stand_walk()
diff --git a/RWR/src/spot_micro_keyboard_command/src/objectDet.cpp b/RWR/src/spot_micro_keyboard_command/src/objectDet.cpp
new file mode 100644
index 0000000..a6acb17
--- /dev/null
+++ b/RWR/src/spot_micro_keyboard_command/src/objectDet.cpp
@@ -0,0 +1,50 @@
+#include <ros/ros.h>
+#include <geometry_msgs/Twist.h>
+#include <tf/transform_datatypes.h>
+#include <std_msgs/Bool.h>
+
+#include "objectDet.h"
+
+ObjectDet::ObjectDet(ros::NodeHandle & nh) : nh_(nh)
+{
+    called = false; 
+
+    stand_cmd = nh_.advertise<std_msgs::Bool>("/stand_cmd", 1);
+    walk_cmd = nh_.advertise<std_msgs::Bool>("/walk_cmd", 1);
+
+    obj_det = nh_.subscribe("/obj_det", 1, &ObjectDet::ObjectDetectedCallback, this);
+}
+
+ObjectDet::~ObjectDet()
+{}
+
+void ObjectDet::ObjectDetectedCallback(const std_msgs::Bool::ConstPtr& det)
+{
+    if(det && (called == false)){
+        std_msgs::Bool stand_msg;
+        std_msgs::Bool walk_msg;
+
+        stand_msg.data = true;
+        walk_msg.data = true;
+
+        stand_cmd.publish(stand_msg);
+        walk_cmd.publish(walk_msg);
+
+        called = true;
+    }
+    else
+    {
+        ROS_INFO_STREAM("No Object detected or already in walk mode");   
+    }
+}
+
+
+int main(int argc, char **argv) {
+    ros::init(argc, argv, "move_publisher");
+
+    ros::NodeHandle nh;
+    ObjectDet detected(nh);
+    
+    ros::spin();
+    return 0;
+}
\ No newline at end of file
-- 
GitLab