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