From ec7c200fa19df7cbb1dc0ddf022a464894cc3242 Mon Sep 17 00:00:00 2001 From: SquidneySquush <64729009+SquidneySquush@users.noreply.github.com> Date: Wed, 6 Oct 2021 16:41:17 -0400 Subject: [PATCH 01/29] Create project2 --- project2 | 1 + 1 file changed, 1 insertion(+) create mode 100644 project2 diff --git a/project2 b/project2 new file mode 100644 index 0000000..8b13789 --- /dev/null +++ b/project2 @@ -0,0 +1 @@ + From a971ce8288c9faa08e9b001c099485a8aca71a76 Mon Sep 17 00:00:00 2001 From: SquidneySquush <64729009+SquidneySquush@users.noreply.github.com> Date: Wed, 6 Oct 2021 16:41:49 -0400 Subject: [PATCH 02/29] Delete project2 --- project2 | 1 - 1 file changed, 1 deletion(-) delete mode 100644 project2 diff --git a/project2 b/project2 deleted file mode 100644 index 8b13789..0000000 --- a/project2 +++ /dev/null @@ -1 +0,0 @@ - From ac42210438a68a3b9dacc2957d4a6dd87b254a95 Mon Sep 17 00:00:00 2001 From: SquidneySquush Date: Wed, 6 Oct 2021 16:55:31 -0400 Subject: [PATCH 03/29] Make project 2 folder add readme --- project2/README.md | 0 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 project2/README.md diff --git a/project2/README.md b/project2/README.md new file mode 100644 index 0000000..e69de29 From fbcd299d94592e1ea5c46f6ec46c9f98f7dcf5e3 Mon Sep 17 00:00:00 2001 From: Emma Fredin Date: Wed, 27 Oct 2021 16:28:36 -0400 Subject: [PATCH 04/29] New Files --- project2/main.py | 0 project2/maze_navigator.py | 0 project2/moveturtle.py | 0 3 files changed, 0 insertions(+), 0 deletions(-) create mode 100644 project2/main.py create mode 100644 project2/maze_navigator.py create mode 100644 project2/moveturtle.py diff --git a/project2/main.py b/project2/main.py new file mode 100644 index 0000000..e69de29 diff --git a/project2/maze_navigator.py b/project2/maze_navigator.py new file mode 100644 index 0000000..e69de29 diff --git a/project2/moveturtle.py b/project2/moveturtle.py new file mode 100644 index 0000000..e69de29 From 06ce9e0766a23f01b87d6140eead8f0561dcacf0 Mon Sep 17 00:00:00 2001 From: Emma Fredin Date: Wed, 3 Nov 2021 16:44:21 -0400 Subject: [PATCH 05/29] Week One Deliverables --- FinalProject/README.md | 0 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 FinalProject/README.md diff --git a/FinalProject/README.md b/FinalProject/README.md new file mode 100644 index 0000000..e69de29 From 0ba7e956e9a9be6bbf0309cac05184a223b8901b Mon Sep 17 00:00:00 2001 From: SquidneySquush <64729009+SquidneySquush@users.noreply.github.com> Date: Wed, 17 Nov 2021 16:49:12 -0500 Subject: [PATCH 06/29] Create dance_cmd.dic --- FinalProject/dance_cmd.dic | 10 ++++++++++ 1 file changed, 10 insertions(+) create mode 100644 FinalProject/dance_cmd.dic diff --git a/FinalProject/dance_cmd.dic b/FinalProject/dance_cmd.dic new file mode 100644 index 0000000..7bb1e90 --- /dev/null +++ b/FinalProject/dance_cmd.dic @@ -0,0 +1,10 @@ +HANDS +LEFT +RIGHT +BACK +HOP +RIGHT FOOT +LEFT FOOT +CHA +CRISS CROSS +REVERSE From ee303089ba4d1cc921fb0359521d91c1d786689c Mon Sep 17 00:00:00 2001 From: SquidneySquush <64729009+SquidneySquush@users.noreply.github.com> Date: Wed, 17 Nov 2021 17:37:21 -0500 Subject: [PATCH 07/29] Create voice_control.py --- FinalProject/voice_control.py | 120 ++++++++++++++++++++++++++++++++++ 1 file changed, 120 insertions(+) create mode 100644 FinalProject/voice_control.py diff --git a/FinalProject/voice_control.py b/FinalProject/voice_control.py new file mode 100644 index 0000000..02b3370 --- /dev/null +++ b/FinalProject/voice_control.py @@ -0,0 +1,120 @@ +#!/usr/bin/env python + +import argparse +import roslib +import rospy + +from geometry_msgs.msg import Twist + +from pocketsphinx.pocketsphinx import * +from sphinxbase.sphinxbase import * +import pyaudio + +class vControl(object): + def __init__(self, model, lexicon, kwlist, pub): + # initialize ROS + self.speed = 0.2 + self.msg = Twist() + + rospy.init_node('voice_cmd_vel') + rospy.on_shutdown(self.shutdown) + + # you may need to change publisher destination depending on what you run + self.pub_ = rospy.Publisher(pub, Twist, queue_size=10) + + # initialize pocketsphinx + config = Decoder.default_config() + config.set_string('-hmm', model) + config.set_string('-dict', lexicon) + config.set_string('-kws', kwlist) + + stream = pyaudio.PyAudio().open(format=pyaudio.paInt16, channels=1, + rate=16000, input=True, frames_per_buffer=1024) + stream.start_stream() + + self.decoder = Decoder(config) + self.decoder.start_utt() + + while not rospy.is_shutdown(): + buf = stream.read(1024) + if buf: + self.decoder.process_raw(buf, False, False) + else: + break + self.parse_asr_result() + + def parse_asr_result(self): + """ + move the robot based on ASR hypothesis + """ + if self.decoder.hyp() != None: + print ([(seg.word, seg.prob, seg.start_frame, seg.end_frame) + for seg in self.decoder.seg()]) + print ("Detected keyphrase, restarting search") + seg.word = seg.word.lower() + self.decoder.end_utt() + self.decoder.start_utt() + # you may want to modify the main logic here + if seg.word.find("full speed") > -1: + if self.speed == 0.2: + self.msg.linear.x = self.msg.linear.x*2 + self.msg.angular.z = self.msg.angular.z*2 + self.speed = 0.4 + if seg.word.find("half speed") > -1: + if self.speed == 0.4: + self.msg.linear.x = self.msg.linear.x/2 + self.msg.angular.z = self.msg.angular.z/2 + self.speed = 0.2 + if seg.word.find("forward") > -1: + self.msg.linear.x = self.speed + self.msg.angular.z = 0 + elif seg.word.find("left") > -1: + if self.msg.linear.x != 0: + if self.msg.angular.z < self.speed: + self.msg.angular.z += 0.05 + else: + self.msg.angular.z = self.speed*2 + elif seg.word.find("right") > -1: + if self.msg.linear.x != 0: + if self.msg.angular.z > -self.speed: + self.msg.angular.z -= 0.05 + else: + self.msg.angular.z = -self.speed*2 + elif seg.word.find("back") > -1: + self.msg.linear.x = -self.speed + self.msg.angular.z = 0 + elif seg.word.find("stop") > -1 or seg.word.find("halt") > -1: + self.msg = Twist() + + self.pub_.publish(self.msg) + + def shutdown(self): + """ + command executed after Ctrl+C is pressed + """ + rospy.loginfo("Stop ASRControl") + self.pub_.publish(Twist()) + rospy.sleep(1) + +if __name__ == '__main__': + parser = argparse.ArgumentParser( + description='Control ROS turtlebot using pocketsphinx.') + parser.add_argument('--model', type=str, + default='/usr/share/pocketsphinx/model/hmm/en_US/hub4wsj_sc_8k', + help='''acoustic model path + (default: /usr/share/pocketsphinx/model/hmm/en_US/hub4wsj_sc_8k)''') + parser.add_argument('--lexicon', type=str, + default='dance_cmd.dic', + help='''pronunciation dictionary + (default: dance_cmd.dic)''') + parser.add_argument('--kwlist', type=str, + default='voice_cmd.kwlist', + help='''keyword list with thresholds + (default: voice_cmd.kwlist)''') + parser.add_argument('--rospub', type=str, + default='mobile_base/commands/velocity', + help='''ROS publisher destination + (default: mobile_base/commands/velocity)''') + + args = parser.parse_args() + vControl(args.model, args.lexicon, args.kwlist, args.rospub) From 9b355d7fa5f6266ef97da8667b302f5ed2c12569 Mon Sep 17 00:00:00 2001 From: SquidneySquush <64729009+SquidneySquush@users.noreply.github.com> Date: Wed, 17 Nov 2021 17:37:45 -0500 Subject: [PATCH 08/29] Create main.py --- FinalProject/main.py | 20 ++++++++++++++++++++ 1 file changed, 20 insertions(+) create mode 100644 FinalProject/main.py diff --git a/FinalProject/main.py b/FinalProject/main.py new file mode 100644 index 0000000..13e50ad --- /dev/null +++ b/FinalProject/main.py @@ -0,0 +1,20 @@ +#!/usr/bin/env python + +# Import libraries +import rospy +from moveturtle import Turtlebot +from maze_navigator import navigate + + +# Run bot to solve the maze. +def runTurtleBot(): + rospy.init_node('squishy_turtlebot3') + turtleBot = Turtlebot(namespace="squishy") + + navigate(turtleBot) + + rospy.spin() + + +if __name__ == '__main__': + runTurtleBot() From 0aa9c4892a3effa40767009bc2322df1bf359e74 Mon Sep 17 00:00:00 2001 From: SquidneySquush <64729009+SquidneySquush@users.noreply.github.com> Date: Wed, 17 Nov 2021 17:41:10 -0500 Subject: [PATCH 09/29] Create FinalProject.launch --- FinalProject/FinalProject.launch | 4 ++++ 1 file changed, 4 insertions(+) create mode 100644 FinalProject/FinalProject.launch diff --git a/FinalProject/FinalProject.launch b/FinalProject/FinalProject.launch new file mode 100644 index 0000000..1be4581 --- /dev/null +++ b/FinalProject/FinalProject.launch @@ -0,0 +1,4 @@ + + + + From 57178c4aa4517f58f2fb26e16aa6faea33e7b87c Mon Sep 17 00:00:00 2001 From: SquidneySquush <64729009+SquidneySquush@users.noreply.github.com> Date: Wed, 1 Dec 2021 02:15:30 -0500 Subject: [PATCH 10/29] Update main.py --- FinalProject/main.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/FinalProject/main.py b/FinalProject/main.py index 13e50ad..bb857d2 100644 --- a/FinalProject/main.py +++ b/FinalProject/main.py @@ -6,7 +6,7 @@ from maze_navigator import navigate -# Run bot to solve the maze. +# Run bot def runTurtleBot(): rospy.init_node('squishy_turtlebot3') turtleBot = Turtlebot(namespace="squishy") From 6867673532e467704b3bafd6962288e86f3b5160 Mon Sep 17 00:00:00 2001 From: SquidneySquush <64729009+SquidneySquush@users.noreply.github.com> Date: Wed, 1 Dec 2021 02:24:42 -0500 Subject: [PATCH 11/29] Create twistleft.py --- FinalProject/twistleft.py | 47 +++++++++++++++++++++++++++++++++++++++ 1 file changed, 47 insertions(+) create mode 100644 FinalProject/twistleft.py diff --git a/FinalProject/twistleft.py b/FinalProject/twistleft.py new file mode 100644 index 0000000..d1ddb0d --- /dev/null +++ b/FinalProject/twistleft.py @@ -0,0 +1,47 @@ +#!/usr/bin/env python + +import rospy +import smach +import time +from geometry_msgs.msg import Twist +from random import randint + +# define state TwistLeft +class TwistLeft(smach.State): + def __init__(self): + smach.State.__init__(self, outcomes=[ ]) #'do_circle','do_line']) + self.counter = 0 + + def execute(self, userdata): + rospy.loginfo('The turtle is twisting left') + velocity_publisher = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10) + + #Setup Twist message + vel_msg = Twist() + vel_msg.linear.x=0 + vel_msg.linear.y = 0 + vel_msg.linear.z = 0 + vel_msg.angular.x = 0 + vel_msg.angular.y = 0 + vel_msg.angular.z = 10 + t0=rospy.Time.now().to_sec() + t1=rospy.Time.now().to_sec() + while(t1-t0<3): + #Publish the velocity + velocity_publisher.publish(vel_msg) + t1=rospy.Time.now().to_sec() + + #After the loop, stops the robot + vel_msg.angular.z = 0 + + #Force the robot to stop + velocity_publisher.publish(vel_msg) + + #Simulate a random environmental factor that determines what the TB does next + #value = randint(1, 10) + #rospy.loginfo('Value is %s', value) + #if (value > 5): + # return 'do_circle' + #else: + #return 'do_line' + # rospy.loginfo('DO NOTHING') From 3050f6cf0e3ef523f60cb3d1ec833ccb5854a23e Mon Sep 17 00:00:00 2001 From: SquidneySquush <64729009+SquidneySquush@users.noreply.github.com> Date: Wed, 1 Dec 2021 02:26:09 -0500 Subject: [PATCH 12/29] Create twistright.py --- FinalProject/twistright.py | 41 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 41 insertions(+) create mode 100644 FinalProject/twistright.py diff --git a/FinalProject/twistright.py b/FinalProject/twistright.py new file mode 100644 index 0000000..e775791 --- /dev/null +++ b/FinalProject/twistright.py @@ -0,0 +1,41 @@ +#!/usr/bin/env python + +import rospy +import smach +import time +from geometry_msgs.msg import Twist + +# define state TwistRight +class TwistRight(smach.State): + def __init__(self): + smach.State.__init__(self, outcomes=[]) #'do_twist_left']) + self.counter = 0 + + def execute(self, userdata): + rospy.loginfo('The turtle is twisting right') + velocity_publisher = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10) + + # Set Twist to twist right + vel_msg = Twist() + vel_msg.linear.x=0 + vel_msg.linear.y = 0 + vel_msg.linear.z = 0 + vel_msg.angular.x = 0 + vel_msg.angular.y = 0 + vel_msg.angular.z = -10 + + # Setup time to twist + t0=rospy.Time.now().to_sec() + t1=rospy.Time.now().to_sec() + + while(t1-t0<3): + #Publish the velocity + velocity_publisher.publish(vel_msg) + t1=rospy.Time.now().to_sec() + + #After the loop, stops the robot + vel_msg.angular.z = 0 + #Force the robot to stop + velocity_publisher.publish(vel_msg) + + # return 'do_twist_left' From f3222f3eea1ac9e42f0ab7e58266397a293eed05 Mon Sep 17 00:00:00 2001 From: SquidneySquush <64729009+SquidneySquush@users.noreply.github.com> Date: Wed, 1 Dec 2021 02:27:26 -0500 Subject: [PATCH 13/29] Create circle.py --- FinalProject/circle.py | 44 ++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 44 insertions(+) create mode 100644 FinalProject/circle.py diff --git a/FinalProject/circle.py b/FinalProject/circle.py new file mode 100644 index 0000000..f77b589 --- /dev/null +++ b/FinalProject/circle.py @@ -0,0 +1,44 @@ +#!/usr/bin/env python + +import rospy +import smach +import time +from geometry_msgs.msg import Twist + +# define state circle +class Circle(smach.State): + def __init__(self): + smach.State.__init__(self, outcomes=['do_stop']) + self.counter = 0 + + def execute(self, userdata): + rospy.loginfo('The turtle is turning in a circle') + + velocity_publisher = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10) + + # Set Twist to circle + vel_msg = Twist() + vel_msg.linear.x=5 + vel_msg.linear.y = 0 + vel_msg.linear.z = 0 + vel_msg.angular.x = 0 + vel_msg.angular.y = 0 + vel_msg.angular.z = 5 + + # Setup time to twist + t0=rospy.Time.now().to_sec() + t1=rospy.Time.now().to_sec() + + while(t1-t0<3): + #Publish the velocity + velocity_publisher.publish(vel_msg) + t1=rospy.Time.now().to_sec() + + #After the loop, stops the robot + vel_msg.angular.z = 0 + vel_msg.linear.x = 0 + #Force the robot to stop + velocity_publisher.publish(vel_msg) + + + return 'do_stop' From 02291147371bcad80380ef6e1f9d2ccacc9cb829 Mon Sep 17 00:00:00 2001 From: SquidneySquush <64729009+SquidneySquush@users.noreply.github.com> Date: Wed, 1 Dec 2021 02:33:03 -0500 Subject: [PATCH 14/29] Update main.py --- FinalProject/main.py | 31 +++++++++++++++++++++++++++++-- 1 file changed, 29 insertions(+), 2 deletions(-) diff --git a/FinalProject/main.py b/FinalProject/main.py index bb857d2..8a621af 100644 --- a/FinalProject/main.py +++ b/FinalProject/main.py @@ -2,18 +2,45 @@ # Import libraries import rospy +import smach +import time +from geometry_msgs.msg import Twist from moveturtle import Turtlebot from maze_navigator import navigate +#Add states to library + + +# define state Stop +class Stop(smach.State): + def __init__(self): + smach.State.__init__(self, outcomes=['do_exit']) + self.counter = 0 + + def execute(self, userdata): + rospy.loginfo('The turtle has stopped') + time.sleep(1) + return 'do_exit' # Run bot def runTurtleBot(): rospy.init_node('squishy_turtlebot3') turtleBot = Turtlebot(namespace="squishy") + + # Create a SMACH state machine + sm = smach.StateMachine(outcomes=['do_exit']) + + # Open the container + with sm: + # Add states to the container + smach.StateMachine.add('TwistRight', TwistRight(), + transitions={'do_twist_left':'TwistLeft'}) - navigate(turtleBot) + #navigate(turtleBot) - rospy.spin() + #rospy.spin() + # Execute SMACH plan + outcome = sm.execute() if __name__ == '__main__': From 3dfd6ea2280a24e02685737e6d3842484e702bd3 Mon Sep 17 00:00:00 2001 From: Your Name Date: Wed, 1 Dec 2021 16:15:03 -0500 Subject: [PATCH 15/29] add cha cha state machine folder --- FinalProject/{ => ChaCha}/circle.py | 0 FinalProject/{ => ChaCha}/twistleft.py | 0 FinalProject/{ => ChaCha}/twistright.py | 0 3 files changed, 0 insertions(+), 0 deletions(-) rename FinalProject/{ => ChaCha}/circle.py (100%) rename FinalProject/{ => ChaCha}/twistleft.py (100%) rename FinalProject/{ => ChaCha}/twistright.py (100%) diff --git a/FinalProject/circle.py b/FinalProject/ChaCha/circle.py similarity index 100% rename from FinalProject/circle.py rename to FinalProject/ChaCha/circle.py diff --git a/FinalProject/twistleft.py b/FinalProject/ChaCha/twistleft.py similarity index 100% rename from FinalProject/twistleft.py rename to FinalProject/ChaCha/twistleft.py diff --git a/FinalProject/twistright.py b/FinalProject/ChaCha/twistright.py similarity index 100% rename from FinalProject/twistright.py rename to FinalProject/ChaCha/twistright.py From 9b32fe4c754b8d5dfd503d67c7c487be57739601 Mon Sep 17 00:00:00 2001 From: efredin1 <78491631+efredin1@users.noreply.github.com> Date: Wed, 1 Dec 2021 17:23:57 -0500 Subject: [PATCH 16/29] Update main.py --- FinalProject/main.py | 25 +++++++++++++++++++++++-- 1 file changed, 23 insertions(+), 2 deletions(-) diff --git a/FinalProject/main.py b/FinalProject/main.py index 8a621af..9a02c18 100644 --- a/FinalProject/main.py +++ b/FinalProject/main.py @@ -8,6 +8,12 @@ from moveturtle import Turtlebot from maze_navigator import navigate #Add states to library +from ChaCha.forward import Forward +from ChaCha.backward import Backward +for ChaCha.circleL import CircleL +for ChaCha.circleR import CircleR +for ChaCha.turnL import TurnL +for ChaCha.turnR import TurnR # define state Stop @@ -33,8 +39,23 @@ def runTurtleBot(): # Open the container with sm: # Add states to the container - smach.StateMachine.add('TwistRight', TwistRight(), - transitions={'do_twist_left':'TwistLeft'}) + smach.StateMachine.add('Plan', Plan(), + transitions={'do_forward':'Forward', 'do_backward': 'Backward', 'do_circleL':'CircleL', 'do_circleR': 'CirlceR', + 'do_turnL':'TurnL', 'do_turnR':'TurnR', 'do_threepoint': 'ThreePoint', 'do_exit':'Stop'}) + smach.StateMachine.add('Forward', Forward(), + transitions={'do_plan':'Plan'}) + smach.StateMachine.add('Backward', Backward(), + transitions={'do_plan':'Plan'}) + smach.StateMachine.add('CircleL', CircleL(), + transitions={'do_plan':'Plan'}) + smach.StateMachine.add('CircleR', CircleR(), + transitions={'do_plan':'Plan'}) + smach.StateMachine.add('TurnL', TurnL(), + transitions={'do_plan':'Plan'}) + smach.StateMachine.add('TurnR', TurnR(), + transitions={'do_plan':'Plan'}) + smach.StateMachine.add('Stop', Stop(), + transitions={'do_exit':'do_exit'}) #navigate(turtleBot) From 97d8b471d55907bec0668f93dd7550bd43d2b3dd Mon Sep 17 00:00:00 2001 From: Your Name Date: Wed, 1 Dec 2021 17:24:51 -0500 Subject: [PATCH 17/29] add states --- .../ChaCha/{twistleft.py => goleft.py} | 0 .../ChaCha/{twistright.py => goright.py} | 0 FinalProject/ChaCha/plan.py | 38 +++++++++++++++++++ pocketsphinx | 1 + 4 files changed, 39 insertions(+) rename FinalProject/ChaCha/{twistleft.py => goleft.py} (100%) rename FinalProject/ChaCha/{twistright.py => goright.py} (100%) create mode 100644 FinalProject/ChaCha/plan.py create mode 160000 pocketsphinx diff --git a/FinalProject/ChaCha/twistleft.py b/FinalProject/ChaCha/goleft.py similarity index 100% rename from FinalProject/ChaCha/twistleft.py rename to FinalProject/ChaCha/goleft.py diff --git a/FinalProject/ChaCha/twistright.py b/FinalProject/ChaCha/goright.py similarity index 100% rename from FinalProject/ChaCha/twistright.py rename to FinalProject/ChaCha/goright.py diff --git a/FinalProject/ChaCha/plan.py b/FinalProject/ChaCha/plan.py new file mode 100644 index 0000000..4a17751 --- /dev/null +++ b/FinalProject/ChaCha/plan.py @@ -0,0 +1,38 @@ +#!/usr/bin/env python + +# Import libraries +import rospy +import smach +import time +from send_init_pos import send_init_pos + +# Define planning state + + +class Plan(smach.State): + def __init__(self, pub_init_pos): + smach.State.__init__(self, outcomes=["do_circleL", "do_circleR", "do_turnL", "do_turnR", "do_forward", "do_backward", "do_chacha", "do_clap" "do_exit"], + input_keys=["plan", "curr_state", "plan_length"], + output_keys=["curr_state"]) + + self.pub_init_pos = pub_init_pos + self.counter = 0 + + def execute(self, userdata): + # if plan is empty, exit + if not userdata.plan: + return 'do_exit' + + # if first state, set initial pos + if userdata.plan_length == len(userdata.plan): + send_init_pos(self.pub_init_pos) + + # get the first state + current_state = userdata.plan.pop(0) + userdata.curr_state = current_state + rospy.loginfo("Planning attempting to run: {}" + .format(current_state["name"])) + + time.sleep(1) + + return current_state["name"] \ No newline at end of file diff --git a/pocketsphinx b/pocketsphinx new file mode 160000 index 0000000..051fabb --- /dev/null +++ b/pocketsphinx @@ -0,0 +1 @@ +Subproject commit 051fabb69e4439051abada8bed2c3ed3f1c35ebf From e877507a7442c9ecb170019bccb12839fb835773 Mon Sep 17 00:00:00 2001 From: efredin1 <78491631+efredin1@users.noreply.github.com> Date: Wed, 1 Dec 2021 17:32:17 -0500 Subject: [PATCH 18/29] Update main.py --- FinalProject/main.py | 21 ++++++++++++--------- 1 file changed, 12 insertions(+), 9 deletions(-) diff --git a/FinalProject/main.py b/FinalProject/main.py index 9a02c18..a7c0c9f 100644 --- a/FinalProject/main.py +++ b/FinalProject/main.py @@ -10,10 +10,11 @@ #Add states to library from ChaCha.forward import Forward from ChaCha.backward import Backward -for ChaCha.circleL import CircleL -for ChaCha.circleR import CircleR -for ChaCha.turnL import TurnL -for ChaCha.turnR import TurnR +form ChaCha.circle import Circle +form ChaCha.clap import Clap +from ChaCha.chacha import Chacha +form ChaCha.turnL import TurnL +form ChaCha.turnR import TurnR # define state Stop @@ -40,20 +41,22 @@ def runTurtleBot(): with sm: # Add states to the container smach.StateMachine.add('Plan', Plan(), - transitions={'do_forward':'Forward', 'do_backward': 'Backward', 'do_circleL':'CircleL', 'do_circleR': 'CirlceR', - 'do_turnL':'TurnL', 'do_turnR':'TurnR', 'do_threepoint': 'ThreePoint', 'do_exit':'Stop'}) + transitions={'do_forward':'Forward', 'do_backward': 'Backward', 'do_circle':'Circle', + 'do_turnL':'TurnL', 'do_turnR':'TurnR','do_clap':'Clap','do_chacha':'Chacha','do_exit':'Stop'}) smach.StateMachine.add('Forward', Forward(), transitions={'do_plan':'Plan'}) smach.StateMachine.add('Backward', Backward(), transitions={'do_plan':'Plan'}) - smach.StateMachine.add('CircleL', CircleL(), - transitions={'do_plan':'Plan'}) - smach.StateMachine.add('CircleR', CircleR(), + smach.StateMachine.add('Circle', Circle(), transitions={'do_plan':'Plan'}) smach.StateMachine.add('TurnL', TurnL(), transitions={'do_plan':'Plan'}) smach.StateMachine.add('TurnR', TurnR(), transitions={'do_plan':'Plan'}) + smach.StateMachine.add('Clap', Clap(), + transitions={'do_clap':'Clap'}) + smach.StateMachine.add('Chacha', Chacha(), + transitions={'do_chacha':'Chacha'}) smach.StateMachine.add('Stop', Stop(), transitions={'do_exit':'do_exit'}) From d250972bf75e03fd625b96e9a402c5fe2f2469f0 Mon Sep 17 00:00:00 2001 From: Your Name Date: Wed, 1 Dec 2021 17:33:56 -0500 Subject: [PATCH 19/29] change file names --- FinalProject/ChaCha/goleft.py | 47 ---------------------------------- FinalProject/ChaCha/goright.py | 41 ----------------------------- FinalProject/ChaCha/plan.py | 2 +- FinalProject/ChaCha/turnL.py | 44 +++++++++++++++++++++++++++++++ FinalProject/ChaCha/turnR.py | 37 ++++++++++++++++++++++++++ 5 files changed, 82 insertions(+), 89 deletions(-) delete mode 100644 FinalProject/ChaCha/goleft.py delete mode 100644 FinalProject/ChaCha/goright.py create mode 100644 FinalProject/ChaCha/turnL.py create mode 100644 FinalProject/ChaCha/turnR.py diff --git a/FinalProject/ChaCha/goleft.py b/FinalProject/ChaCha/goleft.py deleted file mode 100644 index d1ddb0d..0000000 --- a/FinalProject/ChaCha/goleft.py +++ /dev/null @@ -1,47 +0,0 @@ -#!/usr/bin/env python - -import rospy -import smach -import time -from geometry_msgs.msg import Twist -from random import randint - -# define state TwistLeft -class TwistLeft(smach.State): - def __init__(self): - smach.State.__init__(self, outcomes=[ ]) #'do_circle','do_line']) - self.counter = 0 - - def execute(self, userdata): - rospy.loginfo('The turtle is twisting left') - velocity_publisher = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10) - - #Setup Twist message - vel_msg = Twist() - vel_msg.linear.x=0 - vel_msg.linear.y = 0 - vel_msg.linear.z = 0 - vel_msg.angular.x = 0 - vel_msg.angular.y = 0 - vel_msg.angular.z = 10 - t0=rospy.Time.now().to_sec() - t1=rospy.Time.now().to_sec() - while(t1-t0<3): - #Publish the velocity - velocity_publisher.publish(vel_msg) - t1=rospy.Time.now().to_sec() - - #After the loop, stops the robot - vel_msg.angular.z = 0 - - #Force the robot to stop - velocity_publisher.publish(vel_msg) - - #Simulate a random environmental factor that determines what the TB does next - #value = randint(1, 10) - #rospy.loginfo('Value is %s', value) - #if (value > 5): - # return 'do_circle' - #else: - #return 'do_line' - # rospy.loginfo('DO NOTHING') diff --git a/FinalProject/ChaCha/goright.py b/FinalProject/ChaCha/goright.py deleted file mode 100644 index e775791..0000000 --- a/FinalProject/ChaCha/goright.py +++ /dev/null @@ -1,41 +0,0 @@ -#!/usr/bin/env python - -import rospy -import smach -import time -from geometry_msgs.msg import Twist - -# define state TwistRight -class TwistRight(smach.State): - def __init__(self): - smach.State.__init__(self, outcomes=[]) #'do_twist_left']) - self.counter = 0 - - def execute(self, userdata): - rospy.loginfo('The turtle is twisting right') - velocity_publisher = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10) - - # Set Twist to twist right - vel_msg = Twist() - vel_msg.linear.x=0 - vel_msg.linear.y = 0 - vel_msg.linear.z = 0 - vel_msg.angular.x = 0 - vel_msg.angular.y = 0 - vel_msg.angular.z = -10 - - # Setup time to twist - t0=rospy.Time.now().to_sec() - t1=rospy.Time.now().to_sec() - - while(t1-t0<3): - #Publish the velocity - velocity_publisher.publish(vel_msg) - t1=rospy.Time.now().to_sec() - - #After the loop, stops the robot - vel_msg.angular.z = 0 - #Force the robot to stop - velocity_publisher.publish(vel_msg) - - # return 'do_twist_left' diff --git a/FinalProject/ChaCha/plan.py b/FinalProject/ChaCha/plan.py index 4a17751..2147a75 100644 --- a/FinalProject/ChaCha/plan.py +++ b/FinalProject/ChaCha/plan.py @@ -11,7 +11,7 @@ class Plan(smach.State): def __init__(self, pub_init_pos): - smach.State.__init__(self, outcomes=["do_circleL", "do_circleR", "do_turnL", "do_turnR", "do_forward", "do_backward", "do_chacha", "do_clap" "do_exit"], + smach.State.__init__(self, outcomes=["do_circle", "do_turnL", "do_turnR", "do_forward", "do_backward", "do_chacha", "do_clap" "do_exit"], input_keys=["plan", "curr_state", "plan_length"], output_keys=["curr_state"]) diff --git a/FinalProject/ChaCha/turnL.py b/FinalProject/ChaCha/turnL.py new file mode 100644 index 0000000..e1bc9be --- /dev/null +++ b/FinalProject/ChaCha/turnL.py @@ -0,0 +1,44 @@ +#!/usr/bin/env python + +import rospy +import smach +import time +from geometry_msgs.msg import Twist +from ackermann_msgs.msg import AckermannDrive, AckermannDriveStamped +from random import randint + +# define state TwistLeft +#!/usr/bin/env python + +import rospy +import smach +import time +from geometry_msgs.msg import Twist +from ackermann_msgs.msg import AckermannDrive, AckermannDriveStamped + +# define state turnL - turn left +class TurnL(smach.State): + def __init__(self, pub_controls): + smach.State.__init__(self, outcomes=['do_plan'], + input_keys=['curr_state']) + self.counter = 0 + + self.pub_controls = pub_controls + + def execute(self, userdata): + # get state attributes + state_name = userdata.curr_state["name"] + rospy.loginfo("Running {} state".format(state_name)) + #time.sleep(2) + v, delta = float(1), float(0.9) + dur = rospy.Duration(2.0) + rate = rospy.Rate(10) + start = rospy.Time.now() + + drive = AckermannDrive(steering_angle=delta, speed=v) + + while rospy.Time.now() - start < dur: + self.pub_controls.publish(AckermannDriveStamped(drive=drive)) + rate.sleep() + + return 'do_plan' diff --git a/FinalProject/ChaCha/turnR.py b/FinalProject/ChaCha/turnR.py new file mode 100644 index 0000000..cb4192b --- /dev/null +++ b/FinalProject/ChaCha/turnR.py @@ -0,0 +1,37 @@ +#!/usr/bin/env python + +import rospy +import smach +import time +from geometry_msgs.msg import Twist +from ackermann_msgs.msg import AckermannDrive, AckermannDriveStamped + + +# define state TwistRight +class TurnR(smach.State): + def __init__(self, pub_controls): + smach.State.__init__(self, outcomes=['do_plan'], + input_keys=["curr_state"]) + self.counter = 0 + + self.pub_controls = pub_controls + + + def execute(self, userdata): + # get state attributes + state_name = userdata.curr_state["name"] + rospy.loginfo("Running {} state".format(state_name)) + + v, delta = float(1), float(-0.9) # negative angle turns right + dur = rospy.Duration(2.0) + rate = rospy.Rate(10) + start = rospy.Time.now() + + drive = AckermannDrive(steering_angle=delta, speed=v) + + + while rospy.Time.now() - start < dur: + self.pub_controls.publish(AckermannDriveStamped(drive=drive)) + rate.sleep() + + return 'do_plan' From 42d5c9aee809fac1425089aa2c1305179d42ec2a Mon Sep 17 00:00:00 2001 From: Your Name Date: Wed, 1 Dec 2021 17:42:40 -0500 Subject: [PATCH 20/29] adjust cricle --- FinalProject/ChaCha/circle.py | 68 +++++++++++++++++------------------ 1 file changed, 34 insertions(+), 34 deletions(-) diff --git a/FinalProject/ChaCha/circle.py b/FinalProject/ChaCha/circle.py index f77b589..d559e88 100644 --- a/FinalProject/ChaCha/circle.py +++ b/FinalProject/ChaCha/circle.py @@ -1,44 +1,44 @@ #!/usr/bin/env python +# Import libraries import rospy import smach import time -from geometry_msgs.msg import Twist +import math +from ackermann_msgs.msg import AckermannDrive, AckermannDriveStamped + +# Define circle state + -# define state circle class Circle(smach.State): - def __init__(self): - smach.State.__init__(self, outcomes=['do_stop']) + def __init__(self, pub_controls): + smach.State.__init__(self, + outcomes=["do_plan"], + input_keys=["curr_state"]) self.counter = 0 + self.pub_controls = pub_controls def execute(self, userdata): - rospy.loginfo('The turtle is turning in a circle') - - velocity_publisher = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10) - - # Set Twist to circle - vel_msg = Twist() - vel_msg.linear.x=5 - vel_msg.linear.y = 0 - vel_msg.linear.z = 0 - vel_msg.angular.x = 0 - vel_msg.angular.y = 0 - vel_msg.angular.z = 5 - - # Setup time to twist - t0=rospy.Time.now().to_sec() - t1=rospy.Time.now().to_sec() - - while(t1-t0<3): - #Publish the velocity - velocity_publisher.publish(vel_msg) - t1=rospy.Time.now().to_sec() - - #After the loop, stops the robot - vel_msg.angular.z = 0 - vel_msg.linear.x = 0 - #Force the robot to stop - velocity_publisher.publish(vel_msg) - - - return 'do_stop' + # get state attributes + state_name = userdata.curr_state["name"] + radius = userdata.curr_state["attributes"]["radius"] + + rospy.loginfo("Running {} state".format(state_name)) + #Circling right + l = 0.785 # shortest radius that the car can traverse (length of car / 2). + delta = -1 * math.asin(l / (2 * radius)) # constant steering angle of the circle turn. + velocity = 2.0 # default velocity + + c = 2 * math.pi * radius # circumfrence of the circle to be traversed. + dur = rospy.Duration((c / velocity)) # duration the circle turn should take. + rate = rospy.Rate(10) + + drive = AckermannDrive(steering_angle=delta, speed=velocity) + start = rospy.Time.now() + while rospy.Time.now() - start < dur: + self.pub_controls.publish(AckermannDriveStamped(drive=drive)) + rate.sleep() + + time.sleep(1) + + return "do_plan" \ No newline at end of file From 17224e07f7f3bff2bd85312a0716c2f869febe68 Mon Sep 17 00:00:00 2001 From: Your Name Date: Wed, 1 Dec 2021 17:50:44 -0500 Subject: [PATCH 21/29] add clap state and edits --- FinalProject/ChaChaDance/backward.py | 33 ++++++++++++++++++ FinalProject/ChaChaDance/chacha.py | 33 ++++++++++++++++++ .../{ChaCha => ChaChaDance}/circle.py | 0 FinalProject/ChaChaDance/clap.py | 33 ++++++++++++++++++ FinalProject/ChaChaDance/forward.py | 34 +++++++++++++++++++ FinalProject/{ChaCha => ChaChaDance}/plan.py | 0 FinalProject/{ChaCha => ChaChaDance}/turnL.py | 0 FinalProject/{ChaCha => ChaChaDance}/turnR.py | 0 8 files changed, 133 insertions(+) create mode 100644 FinalProject/ChaChaDance/backward.py create mode 100644 FinalProject/ChaChaDance/chacha.py rename FinalProject/{ChaCha => ChaChaDance}/circle.py (100%) create mode 100644 FinalProject/ChaChaDance/clap.py create mode 100644 FinalProject/ChaChaDance/forward.py rename FinalProject/{ChaCha => ChaChaDance}/plan.py (100%) rename FinalProject/{ChaCha => ChaChaDance}/turnL.py (100%) rename FinalProject/{ChaCha => ChaChaDance}/turnR.py (100%) diff --git a/FinalProject/ChaChaDance/backward.py b/FinalProject/ChaChaDance/backward.py new file mode 100644 index 0000000..3476742 --- /dev/null +++ b/FinalProject/ChaChaDance/backward.py @@ -0,0 +1,33 @@ +#!/usr/bin/env python + +# Import libraries +import rospy +import smach +import time +import math +from ackermann_msgs.msg import AckermannDrive, AckermannDriveStamped + +# Define circle state + + +class Backward(smach.State): + def __init__(self, pub_controls): + smach.State.__init__(self, + outcomes=["do_plan"], + input_keys=["curr_state"]) + self.counter = 0 + self.pub_controls = pub_controls + + def execute(self, userdata): + # get state attributes + state_name = userdata.curr_state["name"] + + rospy.loginfo("Running {} state".format(state_name)) + + while rospy.Time.now() - start < dur: + self.pub_controls.publish(AckermannDriveStamped(drive=drive)) + rate.sleep() + + time.sleep(1) + + return "do_plan" diff --git a/FinalProject/ChaChaDance/chacha.py b/FinalProject/ChaChaDance/chacha.py new file mode 100644 index 0000000..b13f7ce --- /dev/null +++ b/FinalProject/ChaChaDance/chacha.py @@ -0,0 +1,33 @@ +#!/usr/bin/env python + +# Import libraries +import rospy +import smach +import time +import math +from ackermann_msgs.msg import AckermannDrive, AckermannDriveStamped + +# Define circle state + + +class ChaCha(smach.State): + def __init__(self, pub_controls): + smach.State.__init__(self, + outcomes=["do_plan"], + input_keys=["curr_state"]) + self.counter = 0 + self.pub_controls = pub_controls + + def execute(self, userdata): + # get state attributes + state_name = userdata.curr_state["name"] + + rospy.loginfo("Running {} state".format(state_name)) + + while rospy.Time.now() - start < dur: + self.pub_controls.publish(AckermannDriveStamped(drive=drive)) + rate.sleep() + + time.sleep(1) + + return "do_plan" \ No newline at end of file diff --git a/FinalProject/ChaCha/circle.py b/FinalProject/ChaChaDance/circle.py similarity index 100% rename from FinalProject/ChaCha/circle.py rename to FinalProject/ChaChaDance/circle.py diff --git a/FinalProject/ChaChaDance/clap.py b/FinalProject/ChaChaDance/clap.py new file mode 100644 index 0000000..000a261 --- /dev/null +++ b/FinalProject/ChaChaDance/clap.py @@ -0,0 +1,33 @@ +#!/usr/bin/env python + +# Import libraries +import rospy +import smach +import time +import math +from ackermann_msgs.msg import AckermannDrive, AckermannDriveStamped + +# Define circle state + + +class Clap(smach.State): + def __init__(self, pub_controls): + smach.State.__init__(self, + outcomes=["do_plan"], + input_keys=["curr_state"]) + self.counter = 0 + self.pub_controls = pub_controls + + def execute(self, userdata): + # get state attributes + state_name = userdata.curr_state["name"] + + rospy.loginfo("Running {} state".format(state_name)) + + while rospy.Time.now() - start < dur: + self.pub_controls.publish(AckermannDriveStamped(drive=drive)) + rate.sleep() + + time.sleep(1) + + return "do_plan" \ No newline at end of file diff --git a/FinalProject/ChaChaDance/forward.py b/FinalProject/ChaChaDance/forward.py new file mode 100644 index 0000000..81113f9 --- /dev/null +++ b/FinalProject/ChaChaDance/forward.py @@ -0,0 +1,34 @@ +#!/usr/bin/env python + +# Import libraries +import rospy +import smach +import time +import math +from ackermann_msgs.msg import AckermannDrive, AckermannDriveStamped + +# Define circle state + + +class Forward(smach.State): + def __init__(self, pub_controls): + smach.State.__init__(self, + outcomes=["do_plan"], + input_keys=["curr_state"]) + self.counter = 0 + self.pub_controls = pub_controls + + + def execute(self, userdata): + # get state attributes + state_name = userdata.curr_state["name"] + + rospy.loginfo("Running {} state".format(state_name)) + + while rospy.Time.now() - start < dur: + self.pub_controls.publish(AckermannDriveStamped(drive=drive)) + rate.sleep() + + time.sleep(1) + + return "do_plan" \ No newline at end of file diff --git a/FinalProject/ChaCha/plan.py b/FinalProject/ChaChaDance/plan.py similarity index 100% rename from FinalProject/ChaCha/plan.py rename to FinalProject/ChaChaDance/plan.py diff --git a/FinalProject/ChaCha/turnL.py b/FinalProject/ChaChaDance/turnL.py similarity index 100% rename from FinalProject/ChaCha/turnL.py rename to FinalProject/ChaChaDance/turnL.py diff --git a/FinalProject/ChaCha/turnR.py b/FinalProject/ChaChaDance/turnR.py similarity index 100% rename from FinalProject/ChaCha/turnR.py rename to FinalProject/ChaChaDance/turnR.py From 17481f83fd51dd19be175c825413b53ec0be2ee5 Mon Sep 17 00:00:00 2001 From: Gerry Migwi Date: Thu, 2 Dec 2021 19:30:44 -0500 Subject: [PATCH 22/29] updated forward and backward --- FinalProject/ChaChaDance/backward.py | 16 ++++++++++------ FinalProject/ChaChaDance/forward.py | 17 ++++++++++------- 2 files changed, 20 insertions(+), 13 deletions(-) diff --git a/FinalProject/ChaChaDance/backward.py b/FinalProject/ChaChaDance/backward.py index 3476742..12957d8 100644 --- a/FinalProject/ChaChaDance/backward.py +++ b/FinalProject/ChaChaDance/backward.py @@ -4,13 +4,11 @@ import rospy import smach import time -import math -from ackermann_msgs.msg import AckermannDrive, AckermannDriveStamped - -# Define circle state +from geometry_msgs.msg import Twist -class Backward(smach.State): +# Define circle state +class Forward(smach.State): def __init__(self, pub_controls): smach.State.__init__(self, outcomes=["do_plan"], @@ -24,8 +22,14 @@ def execute(self, userdata): rospy.loginfo("Running {} state".format(state_name)) + dur = rospy.Duration(2.8 / (3.0)) + rate = rospy.Rate(10) + start = rospy.Time.now() + while rospy.Time.now() - start < dur: - self.pub_controls.publish(AckermannDriveStamped(drive=drive)) + move_cmd = Twist() + move_cmd.linear.x = -3.0 + self.pub_controls.publish(move_cmd) rate.sleep() time.sleep(1) diff --git a/FinalProject/ChaChaDance/forward.py b/FinalProject/ChaChaDance/forward.py index 81113f9..c40c604 100644 --- a/FinalProject/ChaChaDance/forward.py +++ b/FinalProject/ChaChaDance/forward.py @@ -4,12 +4,10 @@ import rospy import smach import time -import math -from ackermann_msgs.msg import AckermannDrive, AckermannDriveStamped - -# Define circle state +from geometry_msgs.msg import Twist +# Define circle state class Forward(smach.State): def __init__(self, pub_controls): smach.State.__init__(self, @@ -18,17 +16,22 @@ def __init__(self, pub_controls): self.counter = 0 self.pub_controls = pub_controls - def execute(self, userdata): # get state attributes state_name = userdata.curr_state["name"] rospy.loginfo("Running {} state".format(state_name)) + dur = rospy.Duration(2.8 / (3.0)) + rate = rospy.Rate(10) + start = rospy.Time.now() + while rospy.Time.now() - start < dur: - self.pub_controls.publish(AckermannDriveStamped(drive=drive)) + move_cmd = Twist() + move_cmd.linear.x = 3.0 + self.pub_controls.publish(move_cmd) rate.sleep() time.sleep(1) - return "do_plan" \ No newline at end of file + return "do_plan" From 9d0a4f1c1a59b95c5c267a81e333de910bf8b0e1 Mon Sep 17 00:00:00 2001 From: Your Name Date: Thu, 2 Dec 2021 19:36:38 -0500 Subject: [PATCH 23/29] Update --- FinalProject/ChaChaDance/circle.py | 52 +++++++++++++++++------------- FinalProject/ChaChaDance/turnL.py | 51 ++++++++++++++--------------- FinalProject/ChaChaDance/turnR.py | 45 +++++++++++++++----------- FinalProject/main.py | 15 +++++---- 4 files changed, 87 insertions(+), 76 deletions(-) diff --git a/FinalProject/ChaChaDance/circle.py b/FinalProject/ChaChaDance/circle.py index d559e88..e1237ae 100644 --- a/FinalProject/ChaChaDance/circle.py +++ b/FinalProject/ChaChaDance/circle.py @@ -19,26 +19,32 @@ def __init__(self, pub_controls): self.pub_controls = pub_controls def execute(self, userdata): - # get state attributes - state_name = userdata.curr_state["name"] - radius = userdata.curr_state["attributes"]["radius"] - - rospy.loginfo("Running {} state".format(state_name)) - #Circling right - l = 0.785 # shortest radius that the car can traverse (length of car / 2). - delta = -1 * math.asin(l / (2 * radius)) # constant steering angle of the circle turn. - velocity = 2.0 # default velocity - - c = 2 * math.pi * radius # circumfrence of the circle to be traversed. - dur = rospy.Duration((c / velocity)) # duration the circle turn should take. - rate = rospy.Rate(10) - - drive = AckermannDrive(steering_angle=delta, speed=velocity) - start = rospy.Time.now() - while rospy.Time.now() - start < dur: - self.pub_controls.publish(AckermannDriveStamped(drive=drive)) - rate.sleep() - - time.sleep(1) - - return "do_plan" \ No newline at end of file + rospy.loginfo('The turtle is turning in a circle') + velocity_publisher = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10) + + # Set Twist to circle + vel_msg = Twist() + vel_msg.linear.x=5 + vel_msg.linear.y = 0 + vel_msg.linear.z = 0 + vel_msg.angular.x = 0 + vel_msg.angular.y = 0 + vel_msg.angular.z = 5 + + # Setup time to twist + t0=rospy.Time.now().to_sec() + t1=rospy.Time.now().to_sec() + + while(t1-t0<3): + #Publish the velocity + velocity_publisher.publish(vel_msg) + t1=rospy.Time.now().to_sec() + + #After the loop, stops the robot + vel_msg.angular.z = 0 + vel_msg.linear.x = 0 + #Force the robot to stop + velocity_publisher.publish(vel_msg) + + + return 'do_plan' \ No newline at end of file diff --git a/FinalProject/ChaChaDance/turnL.py b/FinalProject/ChaChaDance/turnL.py index e1bc9be..d569198 100644 --- a/FinalProject/ChaChaDance/turnL.py +++ b/FinalProject/ChaChaDance/turnL.py @@ -4,17 +4,6 @@ import smach import time from geometry_msgs.msg import Twist -from ackermann_msgs.msg import AckermannDrive, AckermannDriveStamped -from random import randint - -# define state TwistLeft -#!/usr/bin/env python - -import rospy -import smach -import time -from geometry_msgs.msg import Twist -from ackermann_msgs.msg import AckermannDrive, AckermannDriveStamped # define state turnL - turn left class TurnL(smach.State): @@ -23,22 +12,30 @@ def __init__(self, pub_controls): input_keys=['curr_state']) self.counter = 0 - self.pub_controls = pub_controls - def execute(self, userdata): - # get state attributes - state_name = userdata.curr_state["name"] - rospy.loginfo("Running {} state".format(state_name)) - #time.sleep(2) - v, delta = float(1), float(0.9) - dur = rospy.Duration(2.0) - rate = rospy.Rate(10) - start = rospy.Time.now() + def execute(self, userdata): + rospy.loginfo('The turtle is twisting left') + velocity_publisher = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10) - drive = AckermannDrive(steering_angle=delta, speed=v) + #Setup Twist message + vel_msg = Twist() + vel_msg.linear.x=0 + vel_msg.linear.y = 0 + vel_msg.linear.z = 0 + vel_msg.angular.x = 0 + vel_msg.angular.y = 0 + vel_msg.angular.z = 10 + t0=rospy.Time.now().to_sec() + t1=rospy.Time.now().to_sec() + while(t1-t0<3): + #Publish the velocity + velocity_publisher.publish(vel_msg) + t1=rospy.Time.now().to_sec() + + #After the loop, stops the robot + vel_msg.angular.z = 0 - while rospy.Time.now() - start < dur: - self.pub_controls.publish(AckermannDriveStamped(drive=drive)) - rate.sleep() - - return 'do_plan' + #Force the robot to stop + velocity_publisher.publish(vel_msg) + + return 'do_plan' diff --git a/FinalProject/ChaChaDance/turnR.py b/FinalProject/ChaChaDance/turnR.py index cb4192b..4bce67b 100644 --- a/FinalProject/ChaChaDance/turnR.py +++ b/FinalProject/ChaChaDance/turnR.py @@ -4,7 +4,6 @@ import smach import time from geometry_msgs.msg import Twist -from ackermann_msgs.msg import AckermannDrive, AckermannDriveStamped # define state TwistRight @@ -14,24 +13,32 @@ def __init__(self, pub_controls): input_keys=["curr_state"]) self.counter = 0 - self.pub_controls = pub_controls - def execute(self, userdata): - # get state attributes - state_name = userdata.curr_state["name"] - rospy.loginfo("Running {} state".format(state_name)) - - v, delta = float(1), float(-0.9) # negative angle turns right - dur = rospy.Duration(2.0) - rate = rospy.Rate(10) - start = rospy.Time.now() - - drive = AckermannDrive(steering_angle=delta, speed=v) - + rospy.loginfo('The turtle is twisting right') + velocity_publisher = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10) + + # Set Twist to twist right + vel_msg = Twist() + vel_msg.linear.x=0 + vel_msg.linear.y = 0 + vel_msg.linear.z = 0 + vel_msg.angular.x = 0 + vel_msg.angular.y = 0 + vel_msg.angular.z = -10 + + # Setup time to twist + t0=rospy.Time.now().to_sec() + t1=rospy.Time.now().to_sec() + + while(t1-t0<3): + #Publish the velocity + velocity_publisher.publish(vel_msg) + t1=rospy.Time.now().to_sec() + + #After the loop, stops the robot + vel_msg.angular.z = 0 + #Force the robot to stop + velocity_publisher.publish(vel_msg) - while rospy.Time.now() - start < dur: - self.pub_controls.publish(AckermannDriveStamped(drive=drive)) - rate.sleep() - - return 'do_plan' + return 'do_plan' \ No newline at end of file diff --git a/FinalProject/main.py b/FinalProject/main.py index a7c0c9f..42cce6b 100644 --- a/FinalProject/main.py +++ b/FinalProject/main.py @@ -8,13 +8,14 @@ from moveturtle import Turtlebot from maze_navigator import navigate #Add states to library -from ChaCha.forward import Forward -from ChaCha.backward import Backward -form ChaCha.circle import Circle -form ChaCha.clap import Clap -from ChaCha.chacha import Chacha -form ChaCha.turnL import TurnL -form ChaCha.turnR import TurnR +from ChaChaDance.forward import Forward +from ChaChaDance.backward import Backward +from ChaChaDance.circle import Circle +from ChaChaDance.clap import Clap +from ChaChaDance.chacha import Chacha +from ChaChaDance.turnL import TurnL +from ChaChaDance.turnR import TurnR +from ChaChaDance.plan import Plan # define state Stop From ea6d0ecc73b98dc585f4563ef5945e61c0c23255 Mon Sep 17 00:00:00 2001 From: Your Name Date: Thu, 2 Dec 2021 19:47:30 -0500 Subject: [PATCH 24/29] update --- FinalProject/ChaChaDance/circle.py | 26 +++++++++++++------------- FinalProject/ChaChaDance/turnL.py | 24 ++++++++++++------------ FinalProject/ChaChaDance/turnR.py | 24 ++++++++++++------------ 3 files changed, 37 insertions(+), 37 deletions(-) diff --git a/FinalProject/ChaChaDance/circle.py b/FinalProject/ChaChaDance/circle.py index e1237ae..b75f902 100644 --- a/FinalProject/ChaChaDance/circle.py +++ b/FinalProject/ChaChaDance/circle.py @@ -20,31 +20,31 @@ def __init__(self, pub_controls): def execute(self, userdata): rospy.loginfo('The turtle is turning in a circle') - velocity_publisher = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10) + move_publisher = rospy.Publisher('/cmd_vel', Twist, queue_size=10) # Set Twist to circle - vel_msg = Twist() - vel_msg.linear.x=5 - vel_msg.linear.y = 0 - vel_msg.linear.z = 0 - vel_msg.angular.x = 0 - vel_msg.angular.y = 0 - vel_msg.angular.z = 5 + move_cmd = Twist() + move_cmd.linear.x=5 + move_cmd.linear.y = 0 + move_cmd.linear.z = 0 + move_cmd.angular.x = 0 + move_cmd.angular.y = 0 + move_cmd.angular.z = 5 # Setup time to twist t0=rospy.Time.now().to_sec() t1=rospy.Time.now().to_sec() while(t1-t0<3): - #Publish the velocity - velocity_publisher.publish(vel_msg) + #Publish the move + move_publisher.publish(move_cmd) t1=rospy.Time.now().to_sec() #After the loop, stops the robot - vel_msg.angular.z = 0 - vel_msg.linear.x = 0 + move_cmd.angular.z = 0 + move_cmd.linear.x = 0 #Force the robot to stop - velocity_publisher.publish(vel_msg) + move_publisher.publish(move_cmd) return 'do_plan' \ No newline at end of file diff --git a/FinalProject/ChaChaDance/turnL.py b/FinalProject/ChaChaDance/turnL.py index d569198..2332628 100644 --- a/FinalProject/ChaChaDance/turnL.py +++ b/FinalProject/ChaChaDance/turnL.py @@ -15,27 +15,27 @@ def __init__(self, pub_controls): def execute(self, userdata): rospy.loginfo('The turtle is twisting left') - velocity_publisher = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10) + move_publisher = rospy.Publisher('/cmd_vel', Twist, queue_size=10) #Setup Twist message - vel_msg = Twist() - vel_msg.linear.x=0 - vel_msg.linear.y = 0 - vel_msg.linear.z = 0 - vel_msg.angular.x = 0 - vel_msg.angular.y = 0 - vel_msg.angular.z = 10 + move_cmd = Twist() + move_cmd.linear.x=0 + move_cmd.linear.y = 0 + move_cmd.linear.z = 0 + move_cmd.angular.x = 0 + move_cmd.angular.y = 0 + move_cmd.angular.z = 10 t0=rospy.Time.now().to_sec() t1=rospy.Time.now().to_sec() while(t1-t0<3): - #Publish the velocity - velocity_publisher.publish(vel_msg) + #Publish the move + move_publisher.publish(move_cmd) t1=rospy.Time.now().to_sec() #After the loop, stops the robot - vel_msg.angular.z = 0 + move_cmd.angular.z = 0 #Force the robot to stop - velocity_publisher.publish(vel_msg) + move_publisher.publish(move_cmd) return 'do_plan' diff --git a/FinalProject/ChaChaDance/turnR.py b/FinalProject/ChaChaDance/turnR.py index 4bce67b..702aee6 100644 --- a/FinalProject/ChaChaDance/turnR.py +++ b/FinalProject/ChaChaDance/turnR.py @@ -16,29 +16,29 @@ def __init__(self, pub_controls): def execute(self, userdata): rospy.loginfo('The turtle is twisting right') - velocity_publisher = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10) + move_publisher = rospy.Publisher('/cmd_vel', Twist, queue_size=10) #might need to add turtle name # Set Twist to twist right - vel_msg = Twist() - vel_msg.linear.x=0 - vel_msg.linear.y = 0 - vel_msg.linear.z = 0 - vel_msg.angular.x = 0 - vel_msg.angular.y = 0 - vel_msg.angular.z = -10 + move_cmd = Twist() + move_cmd.linear.x=0 + move_cmd.linear.y = 0 + move_cmd.linear.z = 0 + move_cmd.angular.x = 0 + move_cmd.angular.y = 0 + move_cmd.angular.z = -10 # Setup time to twist t0=rospy.Time.now().to_sec() t1=rospy.Time.now().to_sec() while(t1-t0<3): - #Publish the velocity - velocity_publisher.publish(vel_msg) + #Publish the move + move_publisher.publish(move_cmd) t1=rospy.Time.now().to_sec() #After the loop, stops the robot - vel_msg.angular.z = 0 + move_cmd.angular.z = 0 #Force the robot to stop - velocity_publisher.publish(vel_msg) + move_publisher.publish(move_cmd) return 'do_plan' \ No newline at end of file From 5dbb158483339af021b21a470d28a0dfaaa2baa6 Mon Sep 17 00:00:00 2001 From: Gerry Migwi <23432496+gerry101@users.noreply.github.com> Date: Thu, 2 Dec 2021 20:02:20 -0500 Subject: [PATCH 25/29] Update main.py --- FinalProject/main.py | 80 +++++++++++++++++++++++--------------------- 1 file changed, 42 insertions(+), 38 deletions(-) diff --git a/FinalProject/main.py b/FinalProject/main.py index 42cce6b..5c55fc0 100644 --- a/FinalProject/main.py +++ b/FinalProject/main.py @@ -1,21 +1,20 @@ #!/usr/bin/env python # Import libraries +from ChaCha.chacha import Chacha import rospy import smach import time +import json +from plan_parser import parse_plan from geometry_msgs.msg import Twist -from moveturtle import Turtlebot -from maze_navigator import navigate -#Add states to library -from ChaChaDance.forward import Forward -from ChaChaDance.backward import Backward -from ChaChaDance.circle import Circle -from ChaChaDance.clap import Clap -from ChaChaDance.chacha import Chacha -from ChaChaDance.turnL import TurnL -from ChaChaDance.turnR import TurnR -from ChaChaDance.plan import Plan +# Add states to library +from ChaCha.forward import Forward +from ChaCha.backward import Backward +from ChaCha.circle import Circle +from ChaCha.clap import Clap +from ChaCha.turnL import TurnL +from ChaCha.turnR import TurnR # define state Stop @@ -26,47 +25,52 @@ def __init__(self): def execute(self, userdata): rospy.loginfo('The turtle has stopped') - time.sleep(1) + time.sleep(1) return 'do_exit' - # Run bot -def runTurtleBot(): + + +def runTurtleBot(plan): rospy.init_node('squishy_turtlebot3') turtleBot = Turtlebot(namespace="squishy") - - # Create a SMACH state machine + + # Create a SMACH state machine sm = smach.StateMachine(outcomes=['do_exit']) + sm.userdata.plan = plan + sm.userdata.plan_length = len(plan) + sm.userdata.curr_state = {} # Open the container with sm: # Add states to the container - smach.StateMachine.add('Plan', Plan(), - transitions={'do_forward':'Forward', 'do_backward': 'Backward', 'do_circle':'Circle', - 'do_turnL':'TurnL', 'do_turnR':'TurnR','do_clap':'Clap','do_chacha':'Chacha','do_exit':'Stop'}) - smach.StateMachine.add('Forward', Forward(), - transitions={'do_plan':'Plan'}) - smach.StateMachine.add('Backward', Backward(), - transitions={'do_plan':'Plan'}) - smach.StateMachine.add('Circle', Circle(), - transitions={'do_plan':'Plan'}) - smach.StateMachine.add('TurnL', TurnL(), - transitions={'do_plan':'Plan'}) - smach.StateMachine.add('TurnR', TurnR(), - transitions={'do_plan':'Plan'}) - smach.StateMachine.add('Clap', Clap(), - transitions={'do_clap':'Clap'}) - smach.StateMachine.add('Chacha', Chacha(), - transitions={'do_chacha':'Chacha'}) - smach.StateMachine.add('Stop', Stop(), - transitions={'do_exit':'do_exit'}) + smach.StateMachine.add('Plan', Plan(), + transitions={'do_forward': 'Forward', 'do_backward': 'Backward', 'do_circle': 'Circle', + 'do_turnL': 'TurnL', 'do_turnR': 'TurnR', 'do_clap': 'Clap', 'do_chacha': 'Chacha', 'do_exit': 'Stop'}) + smach.StateMachine.add('Forward', Forward(), + transitions={'do_plan': 'Plan'}) + smach.StateMachine.add('Backward', Backward(), + transitions={'do_plan': 'Plan'}) + smach.StateMachine.add('Circle', Circle(), + transitions={'do_plan': 'Plan'}) + smach.StateMachine.add('TurnL', TurnL(), + transitions={'do_plan': 'Plan'}) + smach.StateMachine.add('TurnR', TurnR(), + transitions={'do_plan': 'Plan'}) + smach.StateMachine.add('Clap', Clap(), + transitions={'do_clap': 'Clap'}) + smach.StateMachine.add('Chacha', Chacha(), + transitions={'do_chacha': 'Chacha'}) + smach.StateMachine.add('Stop', Stop(), + transitions={'do_exit': 'do_exit'}) - #navigate(turtleBot) + # navigate(turtleBot) - #rospy.spin() + # rospy.spin() # Execute SMACH plan outcome = sm.execute() if __name__ == '__main__': - runTurtleBot() + plan = parse_plan("FinalProject/plans/plan2.json") + runTurtleBot(plan) From 9072a2fd34100bd8f725bdcdc7c3905fce902a7e Mon Sep 17 00:00:00 2001 From: Gerry Migwi <23432496+gerry101@users.noreply.github.com> Date: Thu, 2 Dec 2021 20:03:02 -0500 Subject: [PATCH 26/29] Create plan2.json --- FinalProject/plans/plan2.json | 46 +++++++++++++++++++++++++++++++++++ 1 file changed, 46 insertions(+) create mode 100644 FinalProject/plans/plan2.json diff --git a/FinalProject/plans/plan2.json b/FinalProject/plans/plan2.json new file mode 100644 index 0000000..3d84463 --- /dev/null +++ b/FinalProject/plans/plan2.json @@ -0,0 +1,46 @@ +{ + "states": [ + { + "name": "do_circleL", + "attributes": { + "radius": 0.95 + } + }, + { + "name": "do_backward", + "attributes": { + "distance": 4.5 + } + }, + { + "name": "do_forward", + "attributes": { + "distance": 4.20 + } + }, + { + "name": "do_threepoint" + }, + { + "name": "do_forward", + "attributes": { + "distance": 3.0 + } + }, + { + "name": "do_circleR", + "attributes": { + "radius": 1.7 + } + }, + { + "name": "do_turnL" + }, + { + "name": "do_turnR" + }, + { + "name": "do_turnL" + } + ] +} From 0de1318562539ffbc82ef481a294669cc8d50be3 Mon Sep 17 00:00:00 2001 From: Robot Class Date: Sun, 12 Dec 2021 21:24:50 -0800 Subject: [PATCH 27/29] init commit --- CMakeLists.txt | 199 +++++++++++++++++++++++++++++++++++++++ ChaChaDance/__init__.py | 0 ChaChaDance/__init__.pyc | Bin 0 -> 147 bytes ChaChaDance/backward.py | 38 ++++++++ ChaChaDance/backward.pyc | Bin 0 -> 1481 bytes ChaChaDance/circle.py | 44 +++++++++ ChaChaDance/circle.pyc | Bin 0 -> 1563 bytes ChaChaDance/forward.py | 38 ++++++++ ChaChaDance/forward.pyc | Bin 0 -> 1476 bytes ChaChaDance/plan.py | 31 ++++++ ChaChaDance/plan.pyc | Bin 0 -> 1271 bytes ChaChaDance/stop.py | 26 +++++ ChaChaDance/stop.pyc | Bin 0 -> 1143 bytes ChaChaDance/turnL.py | 40 ++++++++ ChaChaDance/turnL.pyc | Bin 0 -> 1457 bytes ChaChaDance/turnR.py | 40 ++++++++ ChaChaDance/turnR.pyc | Bin 0 -> 1462 bytes FinalProject.launch | 4 + main.py | 108 +++++++++++++++++++++ moves.py | 65 +++++++++++++ moves.pyc | Bin 0 -> 1938 bytes package.xml | 68 +++++++++++++ plan_handler.py | 166 ++++++++++++++++++++++++++++++++ plan_handler.pyc | Bin 0 -> 3500 bytes plan_parser.py | 11 +++ plan_parser.pyc | Bin 0 -> 454 bytes plans/plan1.json | 10 ++ 27 files changed, 888 insertions(+) create mode 100644 CMakeLists.txt create mode 100644 ChaChaDance/__init__.py create mode 100644 ChaChaDance/__init__.pyc create mode 100644 ChaChaDance/backward.py create mode 100644 ChaChaDance/backward.pyc create mode 100644 ChaChaDance/circle.py create mode 100644 ChaChaDance/circle.pyc create mode 100644 ChaChaDance/forward.py create mode 100644 ChaChaDance/forward.pyc create mode 100644 ChaChaDance/plan.py create mode 100644 ChaChaDance/plan.pyc create mode 100644 ChaChaDance/stop.py create mode 100644 ChaChaDance/stop.pyc create mode 100644 ChaChaDance/turnL.py create mode 100644 ChaChaDance/turnL.pyc create mode 100644 ChaChaDance/turnR.py create mode 100644 ChaChaDance/turnR.pyc create mode 100755 FinalProject.launch create mode 100755 main.py create mode 100644 moves.py create mode 100644 moves.pyc create mode 100644 package.xml create mode 100644 plan_handler.py create mode 100644 plan_handler.pyc create mode 100644 plan_parser.py create mode 100644 plan_parser.pyc create mode 100644 plans/plan1.json diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..3314378 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,199 @@ +cmake_minimum_required(VERSION 2.8.3) +project(final_project) + +## Compile as C++11, supported in ROS Kinetic and newer +# add_compile_options(-std=c++11) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + geometry_msgs + rospy + smach +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a exec_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# geometry_msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES final_project +# CATKIN_DEPENDS geometry_msgs rospy smach +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( +# include + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/final_project.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## 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/final_project_node.cpp) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(${PROJECT_NAME}_node +# ${catkin_LIBRARIES} +# ) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables and/or libraries for installation +# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_final_project.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/ChaChaDance/__init__.py b/ChaChaDance/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/ChaChaDance/__init__.pyc b/ChaChaDance/__init__.pyc new file mode 100644 index 0000000000000000000000000000000000000000..9b885a072a6e13a9b8c351569e66205ff980d3be GIT binary patch literal 147 zcmZSn%*!>cd}U%X0~9aC1;tPuMvr>~w^qn&jfygB>FF92|K0Y%qvm`!Vub{Go189Iv RZhlH>PO2TqnqnYk0014xAsqk! literal 0 HcmV?d00001 diff --git a/ChaChaDance/backward.py b/ChaChaDance/backward.py new file mode 100644 index 0000000..0724057 --- /dev/null +++ b/ChaChaDance/backward.py @@ -0,0 +1,38 @@ +#!/usr/bin/env python + +# Import libraries +import rospy +import smach +import time +from geometry_msgs.msg import Twist + + +# Define backward state +class Backward(smach.State): + def __init__(self, pub_controls): + smach.State.__init__(self, outcomes=["do_plan"], input_keys=["curr_state"]) + self.counter = 0 + self.pub_controls = pub_controls + + def execute(self, userdata): + # get state attributes + state_name = userdata.curr_state["name"] + + rospy.loginfo("Running {} state".format(state_name)) + + dur = rospy.Duration(2.8 / (6.0)) + rate = rospy.Rate(10) + start = rospy.Time.now() + + while rospy.Time.now() - start < dur: + move_cmd = Twist() + move_cmd.linear.x = -3.0 + self.pub_controls.publish(move_cmd) + rate.sleep() + + move_cmd = Twist() + move_cmd.linear.x = 0.0 + self.pub_controls.publish(move_cmd) + + return "do_plan" + diff --git a/ChaChaDance/backward.pyc b/ChaChaDance/backward.pyc new file mode 100644 index 0000000000000000000000000000000000000000..2f146da1f4dfa53a223c8e45dc87fcda729e22ca GIT binary patch literal 1481 zcmb_b&2G~`5T3Q2H2p6^pn@t?amuN=a;Q+%_J}IsRI12j<=UInuK)0^X(N(Tc@mxh zapnPd6OKFpd^0JnIB=pX?|620XZD+KX8oV*D?h$`jbgeTKK=)o_ANw$Pf<=Zy3-Yn z)E~P?BTsaXa-WKTMgghBC!;9q&=}_e`b2a?^lsGUNW*>yQPW=-Cw&Fg@%r>Cwa)dR z9Y*oxRn#PRX!y(_rhN`!fJ5#^Oukrg@R3j9ud>=PK~z8vfj=;dV~ z%PwV0+z;~(UBCIuGV9Rx%%@AD<}-mIK7^j>(hNS!q*u5$*(zmgY~dSc0cGoaPQ5>) zvwN4Nf4d|-6m}U2od~IAgsN^~8MbAamXqC&^IfUmWGth5Fu^9q_Cd!P!ZWTEC zF!whgLhfX#aZ}lPDrL>9Nm`C8$MlY?rid0@e%v;ZOHpDjY{(74%4u2{$KlHAD#VY( zEl*1mH6WMhTIw%>(Ia_k7cFITW9mMaEL>$TE^2E{lSJq>MlBg>&Q-!EZ6o&qgIRQ^ z!v*42R3D6vi{t@&OyHVp6SvMhVlX=pHaz)zeda>YXu*sZ z!bxadR7sl~t)*+j7qH#K6z%`t1GCH=TE@^-%bf+S>sW{OmZ5FkP_VAvpN@hY*3NLNF1n zB&#x_1uziwfR(o@Qf0z(I;NEpWxxp?r;KtGwjkR+6m4UC7{Mr8y{vN=wxL_M?1Mfv z7hcSw?>g;69*o5#Nv4vXZ}Q?qv}2wXP_3=AL2Cgm+TI1zg&k~L_AhkNy3nAo1P%D>C#S%Je*pm&-~PP#dL06Uh8GoR;CEk!;>8Ev)mPCS_r`h z%ZrRtn_zg(l+-;nRze~BxgQQO{3i(TQVDrC*PwA@M09flu*t7tFG$3j z&J^ZT%}-Z|?Qciyb;Y>GaGbAdth=dVw#G>9}idopG;m2K>kX6RbKypa0W?ZUcw zD>q=?c1<3nN1pXv&c{g<89FCpC|OloXYvkA6P*i0?iPK0>3J<1o?C>?$GY}RT1VEQ zy=v<>-RNs$mMPaeyTZV%_1<)4jyC1^B`?kSXWS`*R`#91BeYE?H|;B3^G_rm=Y z^1C#sglo=C(Fb!4nEoCkM-w%PevE!ld(l|!sQqY9-B$Z*H)f0Nl&{O>730Wp#W)qM zo3`w0n9}&!UHHC*D%$_OP+SbY!JZ=+s&TTRb{AI0cT8vtToqn`BAH~0raRy?Kv literal 0 HcmV?d00001 diff --git a/ChaChaDance/forward.py b/ChaChaDance/forward.py new file mode 100644 index 0000000..af67d04 --- /dev/null +++ b/ChaChaDance/forward.py @@ -0,0 +1,38 @@ +#!/usr/bin/env python + +# Import libraries +import rospy +import smach +import time +from geometry_msgs.msg import Twist + + +# Define forward state +class Forward(smach.State): + def __init__(self, pub_controls): + smach.State.__init__(self, outcomes=["do_plan"], input_keys=["curr_state"]) + self.counter = 0 + self.pub_controls = pub_controls + + def execute(self, userdata): + # get state attributes + state_name = userdata.curr_state["name"] + + rospy.loginfo("Running {} state".format(state_name)) + + dur = rospy.Duration(2.8 / (6.0)) + rate = rospy.Rate(10) + start = rospy.Time.now() + + while rospy.Time.now() - start < dur: + move_cmd = Twist() + move_cmd.linear.x = 3.0 + self.pub_controls.publish(move_cmd) + rate.sleep() + + move_cmd = Twist() + move_cmd.linear.x = 0.0 + self.pub_controls.publish(move_cmd) + + return "do_plan" + diff --git a/ChaChaDance/forward.pyc b/ChaChaDance/forward.pyc new file mode 100644 index 0000000000000000000000000000000000000000..eb4f6e187cbfa4be0185ae0b7980c4368920885f GIT binary patch literal 1476 zcmb_bO>fgc5S_K1G-*>>gg^yVsN$4Ub43E7idLLZC7enXxvY%6N$vV0c-N#6$*KGj zegooH@Nc*x!JA1x#DNpFyyMx~-Pt$qjs2gS>%YEzk7Bxde0&cu>^qnQucDl2bmJ=; zsq45$BTsyfa-WKTMgghBC!;89(HQ#z`a*O`^nTRlOv8Q)S<~Nmj{6F(fsh`Nnw*py{jPWC=8_e8$QSf2L51dDif4pOH6YSy_9vbg%({l$5A2+zg>js&tY#rTB**f! zG*Jz5i7rHbNsJcBQ@iRYn;SFjvt;20pJGuPYw9FIt1)V+NOP@{K51&X4;ajM? zt&1vYa-+30ZTJ+iTa@Dc_us%AGlP~Pv{kpYB6Sn9kQEfx9IM2YObq@zSI>1}Cw3o3 cGz&!s&UDATD(r;bpN!TRa?=X$}{mC zybBKi-+0@?oo4OH%-Eiv@0-uIMX8mnv5I zR9?(cYXTCMFrK_?+ zH*LMKhG&QC4C_5;#8uSyyg5tv@K~Vn0vD-AY7gHeGD4iplQ17R`wZnAgyHHm%tvp4 zLr{0A=mJb)Dbf*fOdBu>xg}1p`WQE`6c1@L5{|{VI+`a##6C(TRlvqSg(l-xxoUNg zG260XQMo>7yXxh^w~K&7m6g^BDv4m03RlQX)YMHX?F3NU+Batb$D&@jYFVQMB1}bz zz><<(X?r3HYnwy@q<6(HK|62TJOeLN7(%hFxM%s62ndzgQ)@c5_mRWPBvUk2gZQ2r ztNmyi^`j!yXe}*cETlWF%R280n1RIf34DKx3fljT2I7$<@`BP(jkgmS*Ytr1fg^n= ZLh4PiZ#8$o{#D=KYf0~gv|WX7@CTIA4?_R| literal 0 HcmV?d00001 diff --git a/ChaChaDance/stop.py b/ChaChaDance/stop.py new file mode 100644 index 0000000..a716ce5 --- /dev/null +++ b/ChaChaDance/stop.py @@ -0,0 +1,26 @@ +#!/usr/bin/env python + +# Import libraries +import rospy +import smach +import time +from geometry_msgs.msg import Twist + + +# Define the stop state +class Stop(smach.State): + def __init__(self, pub_controls): + smach.State.__init__(self, outcomes=["do_exit"]) + self.counter = 0 + self.pub_controls = pub_controls + + def execute(self, userdata): + rospy.loginfo("Running Stop state") + + move_cmd = Twist() + move_cmd.linear.x = 0.0 + self.pub_controls.publish(move_cmd) + + time.sleep(1) + + return "do_exit" diff --git a/ChaChaDance/stop.pyc b/ChaChaDance/stop.pyc new file mode 100644 index 0000000000000000000000000000000000000000..600f38b41f958d9eb763aa3405e25a6d8ecfa788 GIT binary patch literal 1143 zcmb_bO^?$s5FMv!w_RHCb%N6#BMzG@2ZRvIxe_iHda**{wk}B=#SXibKpe`S;rHx2|8VU>j47;D^xYj3{{9v# zJO@~Ckh;jnMHMGrk4Uew1fNR=UYz9$E98Jkvw z2QT9k=!aPP#)f!?P34WfanZpfxqz?^O;PT|TE!w-2{XpEE*c|Yr61a8`#48fH+(Xs zZ)5N4AcW<*!PXnibfT??ZPGiE6)f?Z#_9 z8kuwLT3hsygKk6_IR^D=7j~jqbd5dfUt8N{oLNg|)^P~7uL=<0P2;~QxgCSHXe`Fy94Cxvd^ObAN_+HE`2JB6>;Ib0Vm-hL zJiZe(8)rPUnrmxz>4$*Gl(!ah75jr}!Zut2M0m~rLCsn2la>ha>U8zjOe0JIb7%#% F(m%_V=w1K- literal 0 HcmV?d00001 diff --git a/ChaChaDance/turnL.py b/ChaChaDance/turnL.py new file mode 100644 index 0000000..8758e1a --- /dev/null +++ b/ChaChaDance/turnL.py @@ -0,0 +1,40 @@ +#!/usr/bin/env python + +# Import libraries +import rospy +import smach +import time +from geometry_msgs.msg import Twist + + +# define state turnL - turn left +class TurnL(smach.State): + def __init__(self, pub_controls): + smach.State.__init__(self, outcomes=['do_plan'], + input_keys=['curr_state']) + self.pub_controls = pub_controls + self.counter = 0 + + def execute(self, userdata): + # get state attributes + state_name = userdata.curr_state["name"] + + rospy.loginfo("Running {} state".format(state_name)) + + move_cmd = Twist() + move_cmd.angular.z = 10 + + rate = rospy.Rate(10) + + dur = rospy.Duration(3.5 / (7.0)) + t0 = rospy.Time.now() + + while rospy.Time.now() - t0 < dur: + self.pub_controls.publish(move_cmd) + rate.sleep() + + move_cmd = Twist() + move_cmd.angular.z = 0.0 + self.pub_controls.publish(move_cmd) + + return "do_plan" diff --git a/ChaChaDance/turnL.pyc b/ChaChaDance/turnL.pyc new file mode 100644 index 0000000000000000000000000000000000000000..3f4f7f49c1c4c112fd026e5f6afb3892a159fcd0 GIT binary patch literal 1457 zcmb_b&2G~`5T13Mv`G_MK%jyu)MHK!H$XzDqCKKYIF%5&tc<-$?fMVznl_M}$^-BW z+;|t>h6jLeCjAjyspTEd%&zC>`(|6e2EC1MKf;JEpBBFRSoSSMj8{=cG`ZFlP1Kd& zqlqWFM_G$=_}tHU$We~mI%L_nsLYLZYcS$cS6NuN9`vNB8mCXq+=>}Bb*-%nok_4s z(x~)g^DsIQZOpq8RO_TjoYt<7qgC@kM`hvaGFxSd%BFCpmcE>i6V_zYpvzIQR>Ho3 zHabCNqbeCip*u|qeP&0tjz-g@2s2&PWojZfIyedO9)?9^Mvn2_tL6cB#;Ey=c=ZQT?5%NjE6%b9m0U@F^Yef9hjK?{4yUZu0;8 zOS}eNx!ohdvN zAz3*~ifQS1fTm@ghc19dvKJP!CJSp|Nc35X2(AX=u&G0rUTR70jJ*np6kYOJZ_5F^I*ODCFpl)b$CWOS6r5+0dW;p3)ud}Z9z+nOsW z)SR2BapoSEu>(QgzY6n)?Y#QZ;Q|0Xj4 literal 0 HcmV?d00001 diff --git a/ChaChaDance/turnR.py b/ChaChaDance/turnR.py new file mode 100644 index 0000000..7ef6e78 --- /dev/null +++ b/ChaChaDance/turnR.py @@ -0,0 +1,40 @@ +#!/usr/bin/env python + +# Import libraries +import rospy +import smach +import time +from geometry_msgs.msg import Twist + + +# define state turnR - turn right +class TurnR(smach.State): + def __init__(self, pub_controls): + smach.State.__init__(self, outcomes=['do_plan'], + input_keys=['curr_state']) + self.pub_controls = pub_controls + self.counter = 0 + + def execute(self, userdata): + # get state attributes + state_name = userdata.curr_state["name"] + + rospy.loginfo("Running {} state".format(state_name)) + + move_cmd = Twist() + move_cmd.angular.z = -10 + + rate = rospy.Rate(10) + + dur = rospy.Duration(3.5 / (7.0)) + t0 = rospy.Time.now() + + while rospy.Time.now() - t0 < dur: + self.pub_controls.publish(move_cmd) + rate.sleep() + + move_cmd = Twist() + move_cmd.angular.z = 0.0 + self.pub_controls.publish(move_cmd) + + return "do_plan" diff --git a/ChaChaDance/turnR.pyc b/ChaChaDance/turnR.pyc new file mode 100644 index 0000000000000000000000000000000000000000..d8620c4d897bc0d5323a44e72f08945870db60e2 GIT binary patch literal 1462 zcmb_cOK;Oa5T3P@G)YrhK%jyu)MHK!H$XzDqCHo_sf5U7W$aCA*Dt(l+CXwDKY-uB zjsL>GLE;MEO!^R9skJ+q*~dJ-Z#MqVjsEu6?_orjpO4>tO#22R#!!?IO|ErC6Lq!j z(Zmzoqs*tgMUxh(*e9bXZPOIzTJ(kJg6Q3(!=A>&HoPXk@f{5nRLA!5Sz_HvYw9An zPK6J=#=d!G0djE5e!BNjY6m1?56{ zYN2>fW2r4li|ZK=*~^jZLY9q-%G_AD1|u$Ym4$`tLr;pTar)HEt(Z|$*V?+!nFO08 zg-T5}52F*&#=I*|wN8q}Y3&9$S~c%=R2Hr-vsIL+Yzk*;smrx+!kTOv^w=xrO4t|B zMkmN@R3)P*bf-z7&+N$7(P)|!VWz9POikoQ2PYxMVOT_F?O79J4fp^)SS(GnnH)o1Q?M~<#B(m1hfiP>e8 z;|@LGrM4{fDDBJT4xK;yYh}@)?M0hTiRy1mO}b9$fZcP%MT^o6zNbE9^!6tH+D-g_ zuf$#O${-#IrbP%+w_pZMQ6$A|_w#ahMc*%UXTl86F!2rcFO{7u1(z72;T?#8J6Pzg zOIyvQO|o*96w}gi7fs7L4_yF_WG^gcO%~PwlIW9U5?pn~VN-`LK_@Wh$4PD+M=8oP z;a+Z|SF*&e+R0|dR72(~Scj0tYOJZ_5Pin^OF5bgmA$(QghWF6xse!lY-B+95j@nXp*j8KKkn6pC7o08L1*fldUdBxZ zQ`$9ng0-6*Mf=ZJz#K4imJ{fxUT1~o2CRU&6QmpyaTnIi;I+AWu5&xHdk{iuAOx_d XTg^@2c)0@LmHY>E5n=$mfLQAfU+pTu literal 0 HcmV?d00001 diff --git a/FinalProject.launch b/FinalProject.launch new file mode 100755 index 0000000..8ac6139 --- /dev/null +++ b/FinalProject.launch @@ -0,0 +1,4 @@ + + + + diff --git a/main.py b/main.py new file mode 100755 index 0000000..c2e93b7 --- /dev/null +++ b/main.py @@ -0,0 +1,108 @@ +#!/usr/bin/env python + +# Import libraries +import rospy +import smach +import time +import pathlib +import websocket +import thread +import time +from plan_handler import get_filtered_plans +from geometry_msgs.msg import Twist +from plan_parser import parse_plan +from ChaChaDance.plan import Plan +from ChaChaDance.stop import Stop +from ChaChaDance.forward import Forward +from ChaChaDance.backward import Backward +from ChaChaDance.turnL import TurnL +from ChaChaDance.turnR import TurnR +from ChaChaDance.circle import Circle + +handling_msg = False + +def on_message(ws, message): + global handling_msg + + if handling_msg: + return + + handling_msg = True + + plan = get_filtered_plans(message) + if plan: + runTurtleBot(plan) + + handling_msg = False + + +def on_error(ws, error): + print(error) + + +def on_close(ws, close_status_code, close_msg): + print("### closed ###") + + +def on_open(ws): + def run(*args): + time.sleep(1) + print("WSS connection opened") + thread.start_new_thread(run, ()) + + +def runTurtleBot(plan): + pub_controls = rospy.Publisher('/cmd_vel', Twist, queue_size=1) + + # Create a SMACH state machine + sm = smach.StateMachine(outcomes=['do_exit']) + sm.userdata.plan = plan + sm.userdata.plan_length = len(plan) + sm.userdata.curr_state = {} + + with sm: + # Add states to the container + smach.StateMachine.add('Plan', Plan(), + transitions={'do_forward': 'Forward', 'do_backward': 'Backward', + 'do_turnL': 'TurnL', 'do_turnR': 'TurnR', 'do_exit': 'Stop', + 'do_circle': 'Circle'}) + smach.StateMachine.add('Forward', Forward(pub_controls), + transitions={'do_plan': 'Plan'}, + remapping={"curr_state": "curr_state"}) + smach.StateMachine.add("Backward", Backward(pub_controls), + transitions={"do_plan": "Plan"}, + remapping={"curr_state": "curr_state"}) + smach.StateMachine.add("TurnL", TurnL(pub_controls), + transitions={"do_plan": "Plan"}, + remapping={"curr_state": "curr_state"}) + smach.StateMachine.add("TurnR", TurnR(pub_controls), + transitions={"do_plan": "Plan"}, + remapping={"curr_state": "curr_state"}) + smach.StateMachine.add("Circle", Circle(pub_controls), + transitions={"do_plan": "Plan"}, + remapping={"curr_state": "curr_state"}) + smach.StateMachine.add("Stop", Stop(pub_controls), + transitions={"do_exit": "do_exit"}) + outcome = sm.execute() + + +def read_cha_cha(): + websocket.enableTrace(True) + ws = websocket.WebSocketApp("wss://squushy.herokuapp.com", + on_open=on_open, + on_message=on_message, + on_error=on_error, + on_close=on_close) + ws.run_forever() + + +if __name__ == '__main__': + rospy.init_node('squishy_turtlebot3') + + abs_path = pathlib.Path(__file__).parent.resolve() + plan_path = str(abs_path) + "/plans/plan1.json" + plan = parse_plan(plan_path) + print(plan) + + read_cha_cha() + #runTurtleBot(plan) diff --git a/moves.py b/moves.py new file mode 100644 index 0000000..8b23894 --- /dev/null +++ b/moves.py @@ -0,0 +1,65 @@ +#!/usr/bin/env python + +# Import libraries +import rospy +import smach +import time +import math +from geometry_msgs.msg import Twist + +def move_circle(pub_controls, direction): + rospy.loginfo("Running circle state. Direction: {}".format(direction)) + + dur = rospy.Duration(47 / (70.0)) + rate = rospy.Rate(10) + + move_cmd = Twist() + move_cmd.linear.x= 5 + move_cmd.angular.z = 5 * direction + + t0=rospy.Time.now() + while rospy.Time.now() - t0 < dur: + pub_controls.publish(move_cmd) + rate.sleep() + + #After the loop, stops the robot + move_cmd = Twist() + move_cmd.angular.z = 0 + move_cmd.linear.x = 0 + pub_controls.publish(move_cmd) + +def move_forward(pub_controls, direction): + rospy.loginfo("Running forward state. Direction: {}".format(direction)) + + dur = rospy.Duration(2.8 / (6.0)) + rate = rospy.Rate(10) + start = rospy.Time.now() + + while rospy.Time.now() - start < dur: + move_cmd = Twist() + move_cmd.linear.x = 3.0 * direction + pub_controls.publish(move_cmd) + rate.sleep() + + move_cmd = Twist() + move_cmd.linear.x = 0.0 + pub_controls.publish(move_cmd) + +def move_left(pub_controls): + rospy.loginfo("Running left state.") + + move_cmd = Twist() + move_cmd.angular.z = 10 + + rate = rospy.Rate(10) + + dur = rospy.Duration(3.5 / (7.0)) + t0 = rospy.Time.now() + + while rospy.Time.now() - t0 < dur: + pub_controls.publish(move_cmd) + rate.sleep() + + move_cmd = Twist() + move_cmd.angular.z = 0.0 + pub_controls.publish(move_cmd) diff --git a/moves.pyc b/moves.pyc new file mode 100644 index 0000000000000000000000000000000000000000..45a655a7cf7741caa7f56e14d61eb8e2e83e2a33 GIT binary patch literal 1938 zcmbtU&5qMZ5U#f4%w$NIVfmZGNQ?xBT$pg+v|6o3g40Sn5)A?=7t7dArZfJ@xH~(; zZbn)OF1)}#08hYaZ#)L#L3jZ8s%$49IDr$FtIPG%UEk;OKMuC45C2CoHJ69yA#NK& zB={@Jh{h{j(O9*1kH%iB`!x1N_bBU79?&=-mH6}u!Cgu^qHItSh_cCf9}l~zVLstH z8Y-wRfOv9dtc!UY{MN_)0JnV);fQ7^u4v}bQqfEKE4HFEcH+}@*e*PA>~K+D`kePA z(FH64Eju*pQYbq?j%&Q$q_8J3TQs{Qt^u9j|H`rqXm{z+2~qWimZ;e0Oh~VmKFtOk zq)tnEwl39`Qj$8Ubt(H*Ec*L+xPg-$+1rqMQ52?_?#HHzvvl9O$fXDS4^5TE&XmQk z`~O^+5&WjOiT-_bXtwY)+%?0N;bjPEVpZDt3&)wWa%zf6>A0DbvdSapHlaRTR1xQN z9V|5JXgo>0GE<~cYk7P&|m88UWQrTHv{*{Jto;ap}yElX+5%w;$(Q%olqq!NykvlO( zcxFeoiboSuL|Hhm${AWU;uQ7(4Y-7eI-hiuyJhzvNZs(Rs%@{Q`fA79R(oooF8jRd zt8c0I4go(*Jy0cJ#?=Ho|5Gxx7Eq{V)qjLWvQr=|WoE)l=);uRA}m|F2}jlGAE4VR z1{C|Q{}&XygLAYL0~OAqD)}zQrW3ikhc(Y=x8_;1+HB2$h4Ix)6wP31W`q_(F(oy( zTQda0EzJznOi3+a18Bk$&8*eiEOuC2fvAb(Ksx;kmOn)Tt|laC&U_1Ny9?2<06f@J zJ+;Fe5Jtp&B0d41=K*f}7GlK!MginxtT2pZ5@G&?od#K0;L!{rOg@aJPCb_}h=Ak< zqsqaZcE+?8VS_I-=C#b2UNhwsCJz{ZAa(i_ps$K!oM9MBaVE*iUqI)6AK9yrE$CS~ zaSh@!IT}R!Euwp^f{qJrYiN;$G!G8VpRg8u1?e@&YgFZ}pvvG0pf$9-nV-Uviwl-~ zlzUh-Kn@r-!kU?6b^f6L2ck)^+Leal_qsl&e=uXnw7 + + final_project + 0.0.0 + The final_project package + + + + + pi + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + geometry_msgs + rospy + smach + geometry_msgs + rospy + smach + geometry_msgs + rospy + smach + + + + + + + + diff --git a/plan_handler.py b/plan_handler.py new file mode 100644 index 0000000..4cb7e5d --- /dev/null +++ b/plan_handler.py @@ -0,0 +1,166 @@ +#!/usr/bin/env/ python + +# Import libraries +import rospy +from geometry_msgs.msg import Twist +from moves import move_circle, move_forward, move_left +from datetime import timedelta, datetime + +# Global Counters +in_progress = False +last_moves = [] +last_move_time = datetime.utcnow() - timedelta(days=1) + +# Return plans depending on song filter +def get_filtered_plans(last_words): + last_words = ' '.join(last_words.split()[-5:]) + + global in_progress + global last_move_time + + if in_progress: + return [] + + in_progress = True + + last_words_str = str(last_words) + last_words_lower = last_words_str.lower() + + #print(last_words_lower) + cur_time = datetime.utcnow() + + pub_controls = rospy.Publisher('/cmd_vel', Twist, queue_size=1) + if cur_time > last_move_time + timedelta(seconds=2.5): + cha_cha_handler(last_words_lower, pub_controls) + clap_handler(last_words_lower, pub_controls) + left_handler(last_words_lower, pub_controls) + back_handler(last_words_lower, pub_controls) + one_hop_handler(last_words_lower, pub_controls) + + in_progress = False + + return [] + + +def cha_cha_handler(last_words, pub_controls): + global last_moves + global last_move_time + + if len(last_moves) >= 1 and \ + last_moves[-1] == 'cha cha': + return [] + + matches = [ + "we're gonna get funky", + "we're going to get funky" + ] + + for match in matches: + if match in last_words: + last_move_time = datetime.utcnow() + last_moves.append('cha cha') + + print("Found cha-cha move. Attempting to execute.") + + move_circle(pub_controls, 1) + move_circle(pub_controls, -1) + + return [] + + +def clap_handler(last_words, pub_controls): + global last_moves + global last_move_time + + if len(last_moves) >= 2 and \ + last_moves[-1] == 'clap': + return [] + + matches = [ + "everybody clap your hands", + "clap clap clap" + ] + + for match in matches: + if match in last_words: + last_move_time = datetime.utcnow() + last_moves.append('clap') + + print("Found clap. Attempting to execute.") + + move_forward(pub_controls, 1) + move_forward(pub_controls, -1) + + return [] + + +def left_handler(last_words, pub_controls): + global last_moves + global last_move_time + + if len(last_moves) >= 1 and \ + last_moves[-1] == 'left': + return [] + + matches = [ + "to the left" + ] + + for match in matches: + if match in last_words: + last_move_time = datetime.utcnow() + last_moves.append('left') + + print("Found left. Attempting to execute.") + + move_left(pub_controls) + + return [] + + +def back_handler(last_words, pub_controls): + global last_moves + global last_move_time + + if len(last_moves) >= 1 and \ + last_moves[-1] == 'back': + return [] + + matches = [ + "take it back", + "back now y'all" + ] + + for match in matches: + if match in last_words: + last_move_time = datetime.utcnow() + last_moves.append('back') + + print("Found back. Attempting to execute.") + + move_forward(pub_controls, -1) + + return [] + +def one_hop_handler(last_words, pub_controls): + global last_moves + global last_move_time + + if len(last_moves) >= 1 and \ + last_moves[-1] == 'one hop': + return [] + + matches = [ + "one hop this time", + ] + + for match in matches: + if match in last_words: + last_move_time = datetime.utcnow() + last_moves.append('one hop') + + print("Found one hop. Attempting to execute.") + + move_forward(pub_controls, -1) + + return [] diff --git a/plan_handler.pyc b/plan_handler.pyc new file mode 100644 index 0000000000000000000000000000000000000000..e07ac2519722966e4ca01fbf24bb7a96e951b902 GIT binary patch literal 3500 zcmd5;-EJFI5T3K^pE$8;l9o29P`glQEu_Xbs8S_BML^pdBD5_+vV>Nf-4lE5{c-mk z*EJ+as9Ym)!DDdC18~g)@E%-p0pE5Lu0d8?VTAz%jz1staz-0C@G^<+vbtJUzC%ujBVq{ zkm`NgcK97O*(B4kPHkvcpv9rpY_aU9IGmb<_lmgV{kVOMW`0A5Qw@j?hz5$U8as6L zsiH$H=}>u%J@LT06gq^(FpI|&9pI%ZPd|jcMuWP@E@AgUgI#V{DXy^7o;vCfFT%1$ zxD9*==fRDah7~G*CEKC{hYl*3N6NmosGM6=))$qH1trHpH~Dyc&^BLw*YZ8RKn8&Fu1rbpij!qdE45tixVKh88@G?cNgMM-H7N75Km>S)d zld@~%1e2nS&B=j84fty7r)C6lO95LnE&~A@V|6s%3*by!7O81NC+W<4kUIS$)16V$ zi9$Ps7bm7;%BZuS!jfY!8UeD;!S{s~4C!z*(s}GN&D!i( z|1x)1&CU3f(Tjts}? z?5u!uQrb-zni6nHBTQ+t4U>MX%jsSbPdzqy(_&nD%za}xONi~bGjG8(k25Bp6<@)Y z1ljlSg8wFWO#73#`n;0QNBT!Z{cAk%HV>e^Af0`N^F6d>qsVzxX!{L>rD@wkjRI#s zopJVq)11Ba9L`pcIm;A&bc^uJILfeJ;HZR*ldj={4eHsx_C!C42&-pBWSL;_-Ct@j zE@Fca&9HDpsYOCK>eTR&-=4j+&LYKCG+RMrc4P3mpsEY$c97hCDs!I zlZFKkMcA9(2-9?dG(7MkA$sY2xrEbC(Ind+h3ItFdsfc-mc4*ysUkOL%d;c299vL3 zIpIBrFjCa;4dUS&#BAVra~Z@*jK`l3<{emPxokf6N=Jsv?~iC_lIMcc=9=a6Wi59- zjqe{@PJ_(!%nrKQw;$(WF3v{ksia_^sXt55QeL?%_>AmX<~}aQ{#CZgE}U3>=CRL^ w@%eNo`C6p^{td?E>iw*U$Ep61&m&-Lq5!|4F1j^UQ`>IiO5-yAwi;W117I@DN&o-= literal 0 HcmV?d00001 diff --git a/plan_parser.py b/plan_parser.py new file mode 100644 index 0000000..864e11c --- /dev/null +++ b/plan_parser.py @@ -0,0 +1,11 @@ +#!/usr/bin/env/ python + +# Import libraries +import json + + +# Parse the task planner json file into individual states. +def parse_plan(task_planner_path): + with open(task_planner_path) as planner_file: + planner_states = json.load(planner_file) + return planner_states["states"] diff --git a/plan_parser.pyc b/plan_parser.pyc new file mode 100644 index 0000000000000000000000000000000000000000..97b641756e018d3ff09d8e4d327dd5921ad8147a GIT binary patch literal 454 zcmbV|F;BxV5QWca1*j?@#xBg6FcK?5NbHqp1`tbRC9ay(jji}lh15>4^E>!``~Z08 z7Qu{@dwKV)+!tr?c|0=f4^z>8Wq97A;sjFTR^*7DU^04xQIZt-0K3f>*0fTv(jK7h zQ1KGNL>r=4MepadRm6(gj5bP=Eleg;CdC5)_m7Yw7w(j0Mr6Y_CIS!9`(V3d+UR%2 zc~kH9&je-SvJB32o$bqDc)??&Kf%0lmQVf!`|(I97l~I8z3?lmL!&Fh%cd*WQOCa0 z^QJSd483n{#ac!J&_~-}gcly+z0@W6k&KehCv}P|oo6P8njJzC=~w>E6w39=*U!%0 L$iIm~7!SVz?zv~4 literal 0 HcmV?d00001 diff --git a/plans/plan1.json b/plans/plan1.json new file mode 100644 index 0000000..3efd175 --- /dev/null +++ b/plans/plan1.json @@ -0,0 +1,10 @@ +{ + "states": [ + { + "name": "do_forward" + }, + { + "name": "do_backward" + } + ] +} From 99276ab75063b3a3d73b6f126761171a91700bb3 Mon Sep 17 00:00:00 2001 From: Robot Class Date: Wed, 15 Dec 2021 11:22:48 -0800 Subject: [PATCH 28/29] added tests --- .moves.py.swp | Bin 0 -> 12288 bytes __pycache__/plan_handler.cpython-35.pyc | Bin 0 -> 4477 bytes .../plan_handler_test.cpython-27-PYTEST.pyc | Bin 0 -> 16934 bytes main.py | 13 -- moves.py | 25 +++- moves.pyc | Bin 1938 -> 2417 bytes plan_handler.py | 114 +++++++++++++-- plan_handler.pyc | Bin 3500 -> 5204 bytes plan_handler_test.py | 133 ++++++++++++++++++ 9 files changed, 261 insertions(+), 24 deletions(-) create mode 100644 .moves.py.swp create mode 100644 __pycache__/plan_handler.cpython-35.pyc create mode 100644 __pycache__/plan_handler_test.cpython-27-PYTEST.pyc create mode 100644 plan_handler_test.py diff --git a/.moves.py.swp b/.moves.py.swp new file mode 100644 index 0000000000000000000000000000000000000000..310a83df6e233e58e64f6f0cf5515927d3a9a0dc GIT binary patch literal 12288 zcmeI2J!}+56vv04NZ4Q?(4-vOQnZI)Z~YZp7KdcXSfZe@42nc??A^`X4eahLvvWR< zC?Cp45v25K=#WTKg^nUkB#I;r9Sw982_R4+K@I=ey<5)TSO_GeH`33(oteFv_j_-) zu(#r!J$0JTxE@10$k>m|xBUMW82fuSW3jZITQ}_|aYbm|+2V#TV_B^F=Aw+s?WCCK zYOyXOKPbmqT@Y1M4Arto+_&TQ*;noI`B z02v?yWPl8i0Wv@a$iU7wVB%fu1;Y3M%z<59@QWH5AOmE843GgbKnBPF86X2>fDDiU zGVq=l@N10S-NV?^0R)8q|J%O-JUYPGLvR=T1%3y=f$QLF@D(@%2EhP$upjflpWr6= z30wj(_zaAJLGXNtv1i~8xCO3(Z^3y`29x05eT@AB{s1??Rqz8SgL&{V_ zfDG(-0~{@GRmxQr8La}mY}u*=GHE*Y)yr}u({H5E4XwX#tF#0%63EJBMDjMXV!h@@`qrFq zJ${7Oq{d6T(43mQ_9xMun1UyQgI4SnO}(V~*_WOnadk(yR~Pi+)<+P}>TnVhx- zGL9pZKOWYvkgC*GJfE`DyqyBcZrrRs>RN@oAyg=g?v%r%k+}H5Wf&`*jr16??gh%Q x+52qpknDXF}QY4Ix4i{UAE>>hz6wxw|JEn=V`w}x8G5P=i literal 0 HcmV?d00001 diff --git a/__pycache__/plan_handler.cpython-35.pyc b/__pycache__/plan_handler.cpython-35.pyc new file mode 100644 index 0000000000000000000000000000000000000000..3e168372b61f5254be8b9deed82f7087906bfe71 GIT binary patch literal 4477 zcmc&%&2Jk;6o32WudI_eA5Bsy-Ilhgkw`e8N|ma%6jUT63Q{XoS_xSj@5EnguRAkt z>Qy3D3UKBKe*z~i9QYUb7xoCJNSry8LwIj?ZLb?Q3RM9oGk$O0%&zzM`@J`_Yvpq3 z_Q5aS&*uPshs?)B`z|(dgq@H78Uz4tV(SoS(AB}!L8F)ffdO3;Tobw$xE6GC;O3y4 z2REP0Fd-;Fw+L<#x&?3xIBJ1cf(C#$2dn^X8x8?j5&RtZd9<~4uMB!{8eS6aSr@7Wv_z24*rI+aZrPY z)${bY@Sv*2)}y_akX19D>xNHRt={7GfW@|Qc0<1BaX&69FJQYeE-90@8ckW%3c(Qq~Q(f)X~RE+sF7V{=^P{MH^rrpsf)g^w0p`=&*69!2y8O zfmaE&hX}f-gEVL^#WdK*r6w&pz;O%OIb|Cl^Uy{dO^}GLMF1b_+JOoC7%G9BBfJr4 zgnX@I0{;V~4f{Im+Cr%NYl$#Ydorsm?ukp*cr{#-pgP zUGMs}rz~(wPX=tjYN8b}Uc{K27pxxke1U7|W8S@g^w;t6adjm&+hMC0TcRJdWIWgE z)%rYaa3(}-2+3nB2=^H01zZ&82D07@_hO5OqCa$tUk-MHmS|#Fr|^8>30YHg5^X$B zgA4VhSEJVSdVauoY}W&?Kk-Nd6K}_>cgEgA*kiS3IGMbpR8rdw!^x70y1>-Xd?LBg zb*ffeQon33tdZS;k)gQ)74JK>5tw1u){2E2Y0$xpA z!w$5IR!%eZoNjBjVe9MKhE|^b!x+DWGwXyjeN1e`y3+p-Xh{4=I*o)tWb&3$pODdNz*nc89dAZOcJ#WZ=uPg z){vE_^uc>7$tO-xIZy7vVl zh9kpFgjT*!D=S3HbnLzR;h^W!b!mn}^4oGgl9F}%GQALc#_9vfwvJHo$NVyaRV~Cu zzG@*6}8`?F`L>vQfp+XRlGLzQy9HD zi%}#vWfbe%gi$@qC>A-aXisjYj20gpkq(p6Vp2j!8|*3L!=2C{I^;XUaKIhPc@fR4 zfpHgIP8q{#uVPA(lrqKNrCVL0j;hdvB7T)zN+3>fW!d4IGoQh2*rUK?gQLuIvINRW0-+M6OK)t?o#Cbz1S70z^i-_>6=f67&#|2X zI93{(H5+hRI~#E8q^=uyaKepaHrh94m6?gC@pv>TGjygxUEfG6ORDQAk9)s^dw*0( z4HR@sI8^?lt4aMx1OGkH8wk|**aX#xk+QP|1YxE*vtj-gBeP+qfX)A(VDhrphvLixcyDLd0bCsa%|` zz~m5~vlSRB;6Gs#Z(}#2;pF~`MTTZhkuk32uP!nt$QUzWoWjJdf5^nGmzY?fGOdVk zgbcBI7smt9>e$3OFE>RxV`XZ1G`g8kGmgChX3k#v-$J~9#rD!3YiQXdL0)Ly6fggO z@lVdNFK@GME_tQEjh6OSEdSv^Mx~K6=to!5GmU!LeFI>VLeG{4r2gLs>{suzSdVB{ zb&W6Mms@Ete1VaCSnG<0*uqY|zqwX&4yIeH0~II6IVEIr@S^uQPA@IVVQhR{C>$ACWnasgs$5E8@bfZs-pJcAEqs=y+}8dHJ$-&d6ywZNsoH P*%$2d_J+M~uhIA4^Fez+ literal 0 HcmV?d00001 diff --git a/__pycache__/plan_handler_test.cpython-27-PYTEST.pyc b/__pycache__/plan_handler_test.cpython-27-PYTEST.pyc new file mode 100644 index 0000000000000000000000000000000000000000..ba51ded3123e5468667bf2ee59e0b4037f79ff31 GIT binary patch literal 16934 zcmeHO+iv4j6x~kK^p;-dR<2_PhRMLt!f+Fa0wXl=fEbVmG*TpFd6JXXtuK*%LaEdW ziGeTRV-R1!A0YmN7kB~o+K!vJ?MxC%LXFZiJNELq`FO9rY@c%`ew&~D^~ax_Kjw*Q z)c%{`-}m_&|85Y~`QIRqsA{C!1=^mV?IKktQ=~-Il57{qn<9Ujs?%g3=LC5(P6ZldQhPHBz?yl231!m zgO^hLTo#{7@$*@HI>j$!@tG9An8jyP{8AR5OYzk#UQY4HviN+8Kc2-GQv8W5zL?@q zX7QyIe=3VFr})!Zd?m#%XYq3>Zf5cGDgF#D_}$6{_5k-U{<~9QHy}O9rs?R~R%IHy zi)?CJj*ah@6VyEw=`t|SX=VUVHFO5J@6>h<0TdGk%H9e)xmFm9+|4a*H^Q)!cbQ=+ zEQ2nfPB*G~Tt?E2_!2`+&k0gs&2wUH`{7qAuHjUs_=|J+fWPs_jO-I_i<{RAqz!g9 z!_e$`6L4vE!2^T#4IJ3v?E>xdA)=jWioSuHvV(d>@+awFf_dYQoqBjD-_6yGxRG;Q;-i`Z>&>PziP8wGk$B$q2)Bt1*p zbM$~~FH!t6Axjy_xb)~3*`06i23+eR)tBgv`f@hgpY3dNq7=$UfeNk?^Yh;PQ4Savdvrq^uC>3CvlTV z=~fijUpZb&#c;Jsz6()|>7+C)sfWdpBN>quP4I4IIyIM&Y)q>Gkdo zu+j3h=AJHbZqE%=5V~k?Twt@Y5yR)NR+@V+Uysd=4HKI;ug4mpIO3f6<%7D&zn@1f z)dp^A-lrHGXty63Uu$RNE$Nqb9m)2|6`fE-Wr<6*P`(A`SrPTxE)1dq|YHiBaD1Ui+4DI zqxoP3u|R6^!`hCDEW{&?2BR5CpS~iel$=F+Bq{0D!<5PX>lMyOVovELU=jLO!fRKE zwKZRSv{@KNg>!|)LYd06#Gk8Fq916PR;hN1C|7)Vl$#{K)E(tS=?9H+Gqla&??{v@ zQYOmHA*^+x9A`B>AwpPSjs>x>o`880<|&w`VJ@=~&RZsS!FKBz z7%2+uwywZD%jRDrGg@;ZnH%Uuv|(s$9^bR{f6oHX>6Yv@xo6?X&O%lfI*Xg*2G=|^ zXP(;d{eG#P2>no5edMMB@iAd_o(PS$CWP*K*b!+)Fd7<}$I^_)&iu@-l~XA~{IHcT3iwYYyR?m^>x2r=Fyo?RjCa5uj~oFT9X z@%k_sJQm{qg}C?Kbd2{=+SyU2t4)mXoOvj>GI(y@=+R#G=GY29~O_t(%io^V_f0*J8PNFn7p*cOas|9 zF^}=gEAp3ctbX}Caz!q2>++|_n3N)le0-Lxi1I!3`LM`5epv4RVYwHX^YN&x2BGlC zAC)^4&YdJ9SDUmIYoK1*dCJlck!x zqH`@u76$V&fpk$UqZ;=#?IW^IOxAoR%TZ-fEtY>;oQ7l#YPT}U!dFXXy2~s(8QPa| zgc2vW;!y hE+|C{-|@qG%Tw>-G-4EArWGn1OUB!U)#*ol{sxpQL!kfw literal 0 HcmV?d00001 diff --git a/main.py b/main.py index c2e93b7..3d47238 100755 --- a/main.py +++ b/main.py @@ -19,23 +19,11 @@ from ChaChaDance.turnR import TurnR from ChaChaDance.circle import Circle -handling_msg = False - def on_message(ws, message): - global handling_msg - - if handling_msg: - return - - handling_msg = True - plan = get_filtered_plans(message) if plan: runTurtleBot(plan) - handling_msg = False - - def on_error(ws, error): print(error) @@ -105,4 +93,3 @@ def read_cha_cha(): print(plan) read_cha_cha() - #runTurtleBot(plan) diff --git a/moves.py b/moves.py index 8b23894..4cdbeaa 100644 --- a/moves.py +++ b/moves.py @@ -10,7 +10,7 @@ def move_circle(pub_controls, direction): rospy.loginfo("Running circle state. Direction: {}".format(direction)) - dur = rospy.Duration(47 / (70.0)) + dur = rospy.Duration(47 / (20.0)) rate = rospy.Rate(10) move_cmd = Twist() @@ -31,7 +31,7 @@ def move_circle(pub_controls, direction): def move_forward(pub_controls, direction): rospy.loginfo("Running forward state. Direction: {}".format(direction)) - dur = rospy.Duration(2.8 / (6.0)) + dur = rospy.Duration(10.8 / (6.0)) rate = rospy.Rate(10) start = rospy.Time.now() @@ -50,7 +50,26 @@ def move_left(pub_controls): move_cmd = Twist() move_cmd.angular.z = 10 - + + rate = rospy.Rate(10) + + dur = rospy.Duration(3.5 / (7.0)) + t0 = rospy.Time.now() + + while rospy.Time.now() - t0 < dur: + pub_controls.publish(move_cmd) + rate.sleep() + + move_cmd = Twist() + move_cmd.angular.z = 0.0 + pub_controls.publish(move_cmd) + +def move_right(pub_controls): + rospy.loginfo("Running left state.") + + move_cmd = Twist() + move_cmd.angular.z = -10 + rate = rospy.Rate(10) dur = rospy.Duration(3.5 / (7.0)) diff --git a/moves.pyc b/moves.pyc index 45a655a7cf7741caa7f56e14d61eb8e2e83e2a33..f5b26231be2e0427a44c00f9286135a2efabc537 100644 GIT binary patch delta 178 zcmbQl|4~Sq`73BXih86~fC{BhHMuuPw_Ko#6j7$tB zoA)wCFtN{?2?DB{Wmpa}O%`Fb7iMN?W@M;kVklu?$YNtiVPQyNot(xR!N@jwKdTHQ z`{X;UvMk^J|NlSv2kUlzE}#{;`DLl`MVaXtCC-!gvMDlhO}@|O#}p(nS&KbPL;$FO PhYKoD`7= 1 and \ + last_moves[-1] == 'right foot': + return [] + + matches = [ + "right foot", + "to the right now", + "the right now", + "right now" + ] + + for match in matches: + if match in last_words: + last_move_time = datetime.utcnow() + last_moves.append('right foot') + + print("Found right foot. Attempting to execute.") + + move_right(pub_controls) + + return True + + return False + +def left_foot_handler(last_words, pub_controls): + global last_moves + global last_move_time + + if len(last_moves) >= 1 and \ + last_moves[-1] == 'left foot stomp': + return [] + + matches = [ + "left foot", + "left foot let's stomp", + "left foot lets stomp" + ] + + for match in matches: + if match in last_words: + last_move_time = datetime.utcnow() + last_moves.append('left foot stomp') + + print("Found left foot. Attempting to execute.") + + move_left(pub_controls) + + return True + + return False + +def circle_handler(last_words, pub_controls): + global last_moves + global last_move_time + + if len(last_moves) >= 1 and \ + last_moves[-1] == 'circle': + return [] + + matches = [ + "turn it out", + "turn it up" + ] + + for match in matches: + if match in last_words: + last_move_time = datetime.utcnow() + last_moves.append('circle') + + print("Found circle. Attempting to execute.") + + move_circle(pub_controls, 1) + + return True + + return False diff --git a/plan_handler.pyc b/plan_handler.pyc index e07ac2519722966e4ca01fbf24bb7a96e951b902..9c9b1320a2870e12a9c0414e20b550dd0297edd5 100644 GIT binary patch literal 5204 zcmdT|OK%)S5U!bh*lT-Z$8jD`0vQsBS4h|#P^3svo+!Y@pm7A!N@z8nX|KmSkIi&% z93xvu1h{hL$iG1O6WloP6SyJe$N}oB*(b3#u^m}r*WKGy-P1i?UwzfpF1)Bt{qgH_ z&!_IE$iH{F=-;>s_|Kw8gmp> zq;Z;pDQV18FfENU6wF9tfr6?u&Qfqn8s{jOmBx8&W34vFTMGZ-&uY!$rYUjtXgk!V zhFzO!uHy8GYWQL1N6O6fnYBq~9!i2F3RAf9e$*RCJ z3L1kY0&ho$SlUO0 z{c<<%&x!YY=tWx9+|bp77YI}vn&v;Rif-V zL}u9{v!uloyDl!7lLhQ3fFR8hnDz8ac<{$xuhL!t6k__d%9Py)HMa`PBJzrujj70w z@e&qCu1`K?e`0>}a>A)xuybL8)*j!duh^h#H3XJb@oouE8ZOQrRM@Rqc|^u47LQ+yyT40MGrz)nVMF zX^-)en`6ABJn#|@#)!NxdCASLmmo5(hVX3h5{YGqD|(St>WRvB*3)3efy&uQJDGz3 zq_u95=jsWSyocOm#iT`?#A`Y;VuRZQT2v!aeT_8m8*2q1Wzh+Z8B3B`I zvLx4daa%k&#|#4w!t8b6f<;aUuJgQ9i;KR<)&6iz?aROKc=!>(`YZ?Q-ci8fgFyKA zD!gz|Yn*(>Ybhr*jwrw~G#pMHoSh{viiQwF$8iulL4x5>`H{;Q#P*MvhoV=LQ&&Dz z_lN7om~dGdgv%kgZge9gg3}PaZygTOTL(ajoE>MCAnIt7@+k(u`Z~#N`w|~~W=VI)EewxCPp<>n#NZf| zsgDEk37&M9p;+D!K*?+`{Kr)e|p%GFsRSW?_9d1~#WJBGCB@5~nVAFv{3|at`MgG!@5O z!-dd-M*#Ib5ZVz*KO7-O zw?dlJexF_K3{3WV56Vx<(w|xTCys92J7g+SzsL?m$C|Y#bEW`~JqLyUv z=(6d_faBIi8OG?2IdAxi9-)jZWzhfU>`yJ<8D^&!*2<*Ck;@Ew8MdV7?LG#6;Z_-Z zwx8huwcTlBMA7L#1ZnCtvg0n>kqBpia-cVOM83mxHF^Rh7Xf8@MTw#dRHts$Ay>=G{a;sdsWGJ93l;WS|qhh?Z z8>N3YFqT#NR#<6{3Nk;kT~#0aYT{7UnmpwU4W##S;s51Sa!N{% Date: Wed, 15 Dec 2021 11:34:36 -0800 Subject: [PATCH 29/29] Cleaned up project --- FinalProject/ChaChaDance/backward.py | 37 --------- FinalProject/ChaChaDance/chacha.py | 33 -------- FinalProject/ChaChaDance/circle.py | 50 ----------- FinalProject/ChaChaDance/clap.py | 33 -------- FinalProject/ChaChaDance/forward.py | 37 --------- FinalProject/ChaChaDance/plan.py | 38 --------- FinalProject/ChaChaDance/turnL.py | 41 --------- FinalProject/ChaChaDance/turnR.py | 44 ---------- FinalProject/FinalProject.launch | 4 - FinalProject/README.md | 0 FinalProject/dance_cmd.dic | 10 --- FinalProject/main.py | 76 ----------------- FinalProject/plans/plan2.json | 46 ---------- FinalProject/voice_control.py | 120 --------------------------- 14 files changed, 569 deletions(-) delete mode 100644 FinalProject/ChaChaDance/backward.py delete mode 100644 FinalProject/ChaChaDance/chacha.py delete mode 100644 FinalProject/ChaChaDance/circle.py delete mode 100644 FinalProject/ChaChaDance/clap.py delete mode 100644 FinalProject/ChaChaDance/forward.py delete mode 100644 FinalProject/ChaChaDance/plan.py delete mode 100644 FinalProject/ChaChaDance/turnL.py delete mode 100644 FinalProject/ChaChaDance/turnR.py delete mode 100644 FinalProject/FinalProject.launch delete mode 100644 FinalProject/README.md delete mode 100644 FinalProject/dance_cmd.dic delete mode 100644 FinalProject/main.py delete mode 100644 FinalProject/plans/plan2.json delete mode 100644 FinalProject/voice_control.py diff --git a/FinalProject/ChaChaDance/backward.py b/FinalProject/ChaChaDance/backward.py deleted file mode 100644 index 12957d8..0000000 --- a/FinalProject/ChaChaDance/backward.py +++ /dev/null @@ -1,37 +0,0 @@ -#!/usr/bin/env python - -# Import libraries -import rospy -import smach -import time -from geometry_msgs.msg import Twist - - -# Define circle state -class Forward(smach.State): - def __init__(self, pub_controls): - smach.State.__init__(self, - outcomes=["do_plan"], - input_keys=["curr_state"]) - self.counter = 0 - self.pub_controls = pub_controls - - def execute(self, userdata): - # get state attributes - state_name = userdata.curr_state["name"] - - rospy.loginfo("Running {} state".format(state_name)) - - dur = rospy.Duration(2.8 / (3.0)) - rate = rospy.Rate(10) - start = rospy.Time.now() - - while rospy.Time.now() - start < dur: - move_cmd = Twist() - move_cmd.linear.x = -3.0 - self.pub_controls.publish(move_cmd) - rate.sleep() - - time.sleep(1) - - return "do_plan" diff --git a/FinalProject/ChaChaDance/chacha.py b/FinalProject/ChaChaDance/chacha.py deleted file mode 100644 index b13f7ce..0000000 --- a/FinalProject/ChaChaDance/chacha.py +++ /dev/null @@ -1,33 +0,0 @@ -#!/usr/bin/env python - -# Import libraries -import rospy -import smach -import time -import math -from ackermann_msgs.msg import AckermannDrive, AckermannDriveStamped - -# Define circle state - - -class ChaCha(smach.State): - def __init__(self, pub_controls): - smach.State.__init__(self, - outcomes=["do_plan"], - input_keys=["curr_state"]) - self.counter = 0 - self.pub_controls = pub_controls - - def execute(self, userdata): - # get state attributes - state_name = userdata.curr_state["name"] - - rospy.loginfo("Running {} state".format(state_name)) - - while rospy.Time.now() - start < dur: - self.pub_controls.publish(AckermannDriveStamped(drive=drive)) - rate.sleep() - - time.sleep(1) - - return "do_plan" \ No newline at end of file diff --git a/FinalProject/ChaChaDance/circle.py b/FinalProject/ChaChaDance/circle.py deleted file mode 100644 index b75f902..0000000 --- a/FinalProject/ChaChaDance/circle.py +++ /dev/null @@ -1,50 +0,0 @@ -#!/usr/bin/env python - -# Import libraries -import rospy -import smach -import time -import math -from ackermann_msgs.msg import AckermannDrive, AckermannDriveStamped - -# Define circle state - - -class Circle(smach.State): - def __init__(self, pub_controls): - smach.State.__init__(self, - outcomes=["do_plan"], - input_keys=["curr_state"]) - self.counter = 0 - self.pub_controls = pub_controls - - def execute(self, userdata): - rospy.loginfo('The turtle is turning in a circle') - move_publisher = rospy.Publisher('/cmd_vel', Twist, queue_size=10) - - # Set Twist to circle - move_cmd = Twist() - move_cmd.linear.x=5 - move_cmd.linear.y = 0 - move_cmd.linear.z = 0 - move_cmd.angular.x = 0 - move_cmd.angular.y = 0 - move_cmd.angular.z = 5 - - # Setup time to twist - t0=rospy.Time.now().to_sec() - t1=rospy.Time.now().to_sec() - - while(t1-t0<3): - #Publish the move - move_publisher.publish(move_cmd) - t1=rospy.Time.now().to_sec() - - #After the loop, stops the robot - move_cmd.angular.z = 0 - move_cmd.linear.x = 0 - #Force the robot to stop - move_publisher.publish(move_cmd) - - - return 'do_plan' \ No newline at end of file diff --git a/FinalProject/ChaChaDance/clap.py b/FinalProject/ChaChaDance/clap.py deleted file mode 100644 index 000a261..0000000 --- a/FinalProject/ChaChaDance/clap.py +++ /dev/null @@ -1,33 +0,0 @@ -#!/usr/bin/env python - -# Import libraries -import rospy -import smach -import time -import math -from ackermann_msgs.msg import AckermannDrive, AckermannDriveStamped - -# Define circle state - - -class Clap(smach.State): - def __init__(self, pub_controls): - smach.State.__init__(self, - outcomes=["do_plan"], - input_keys=["curr_state"]) - self.counter = 0 - self.pub_controls = pub_controls - - def execute(self, userdata): - # get state attributes - state_name = userdata.curr_state["name"] - - rospy.loginfo("Running {} state".format(state_name)) - - while rospy.Time.now() - start < dur: - self.pub_controls.publish(AckermannDriveStamped(drive=drive)) - rate.sleep() - - time.sleep(1) - - return "do_plan" \ No newline at end of file diff --git a/FinalProject/ChaChaDance/forward.py b/FinalProject/ChaChaDance/forward.py deleted file mode 100644 index c40c604..0000000 --- a/FinalProject/ChaChaDance/forward.py +++ /dev/null @@ -1,37 +0,0 @@ -#!/usr/bin/env python - -# Import libraries -import rospy -import smach -import time -from geometry_msgs.msg import Twist - - -# Define circle state -class Forward(smach.State): - def __init__(self, pub_controls): - smach.State.__init__(self, - outcomes=["do_plan"], - input_keys=["curr_state"]) - self.counter = 0 - self.pub_controls = pub_controls - - def execute(self, userdata): - # get state attributes - state_name = userdata.curr_state["name"] - - rospy.loginfo("Running {} state".format(state_name)) - - dur = rospy.Duration(2.8 / (3.0)) - rate = rospy.Rate(10) - start = rospy.Time.now() - - while rospy.Time.now() - start < dur: - move_cmd = Twist() - move_cmd.linear.x = 3.0 - self.pub_controls.publish(move_cmd) - rate.sleep() - - time.sleep(1) - - return "do_plan" diff --git a/FinalProject/ChaChaDance/plan.py b/FinalProject/ChaChaDance/plan.py deleted file mode 100644 index 2147a75..0000000 --- a/FinalProject/ChaChaDance/plan.py +++ /dev/null @@ -1,38 +0,0 @@ -#!/usr/bin/env python - -# Import libraries -import rospy -import smach -import time -from send_init_pos import send_init_pos - -# Define planning state - - -class Plan(smach.State): - def __init__(self, pub_init_pos): - smach.State.__init__(self, outcomes=["do_circle", "do_turnL", "do_turnR", "do_forward", "do_backward", "do_chacha", "do_clap" "do_exit"], - input_keys=["plan", "curr_state", "plan_length"], - output_keys=["curr_state"]) - - self.pub_init_pos = pub_init_pos - self.counter = 0 - - def execute(self, userdata): - # if plan is empty, exit - if not userdata.plan: - return 'do_exit' - - # if first state, set initial pos - if userdata.plan_length == len(userdata.plan): - send_init_pos(self.pub_init_pos) - - # get the first state - current_state = userdata.plan.pop(0) - userdata.curr_state = current_state - rospy.loginfo("Planning attempting to run: {}" - .format(current_state["name"])) - - time.sleep(1) - - return current_state["name"] \ No newline at end of file diff --git a/FinalProject/ChaChaDance/turnL.py b/FinalProject/ChaChaDance/turnL.py deleted file mode 100644 index 2332628..0000000 --- a/FinalProject/ChaChaDance/turnL.py +++ /dev/null @@ -1,41 +0,0 @@ -#!/usr/bin/env python - -import rospy -import smach -import time -from geometry_msgs.msg import Twist - -# define state turnL - turn left -class TurnL(smach.State): - def __init__(self, pub_controls): - smach.State.__init__(self, outcomes=['do_plan'], - input_keys=['curr_state']) - self.counter = 0 - - - def execute(self, userdata): - rospy.loginfo('The turtle is twisting left') - move_publisher = rospy.Publisher('/cmd_vel', Twist, queue_size=10) - - #Setup Twist message - move_cmd = Twist() - move_cmd.linear.x=0 - move_cmd.linear.y = 0 - move_cmd.linear.z = 0 - move_cmd.angular.x = 0 - move_cmd.angular.y = 0 - move_cmd.angular.z = 10 - t0=rospy.Time.now().to_sec() - t1=rospy.Time.now().to_sec() - while(t1-t0<3): - #Publish the move - move_publisher.publish(move_cmd) - t1=rospy.Time.now().to_sec() - - #After the loop, stops the robot - move_cmd.angular.z = 0 - - #Force the robot to stop - move_publisher.publish(move_cmd) - - return 'do_plan' diff --git a/FinalProject/ChaChaDance/turnR.py b/FinalProject/ChaChaDance/turnR.py deleted file mode 100644 index 702aee6..0000000 --- a/FinalProject/ChaChaDance/turnR.py +++ /dev/null @@ -1,44 +0,0 @@ -#!/usr/bin/env python - -import rospy -import smach -import time -from geometry_msgs.msg import Twist - - -# define state TwistRight -class TurnR(smach.State): - def __init__(self, pub_controls): - smach.State.__init__(self, outcomes=['do_plan'], - input_keys=["curr_state"]) - self.counter = 0 - - - def execute(self, userdata): - rospy.loginfo('The turtle is twisting right') - move_publisher = rospy.Publisher('/cmd_vel', Twist, queue_size=10) #might need to add turtle name - - # Set Twist to twist right - move_cmd = Twist() - move_cmd.linear.x=0 - move_cmd.linear.y = 0 - move_cmd.linear.z = 0 - move_cmd.angular.x = 0 - move_cmd.angular.y = 0 - move_cmd.angular.z = -10 - - # Setup time to twist - t0=rospy.Time.now().to_sec() - t1=rospy.Time.now().to_sec() - - while(t1-t0<3): - #Publish the move - move_publisher.publish(move_cmd) - t1=rospy.Time.now().to_sec() - - #After the loop, stops the robot - move_cmd.angular.z = 0 - #Force the robot to stop - move_publisher.publish(move_cmd) - - return 'do_plan' \ No newline at end of file diff --git a/FinalProject/FinalProject.launch b/FinalProject/FinalProject.launch deleted file mode 100644 index 1be4581..0000000 --- a/FinalProject/FinalProject.launch +++ /dev/null @@ -1,4 +0,0 @@ - - - - diff --git a/FinalProject/README.md b/FinalProject/README.md deleted file mode 100644 index e69de29..0000000 diff --git a/FinalProject/dance_cmd.dic b/FinalProject/dance_cmd.dic deleted file mode 100644 index 7bb1e90..0000000 --- a/FinalProject/dance_cmd.dic +++ /dev/null @@ -1,10 +0,0 @@ -HANDS -LEFT -RIGHT -BACK -HOP -RIGHT FOOT -LEFT FOOT -CHA -CRISS CROSS -REVERSE diff --git a/FinalProject/main.py b/FinalProject/main.py deleted file mode 100644 index 5c55fc0..0000000 --- a/FinalProject/main.py +++ /dev/null @@ -1,76 +0,0 @@ -#!/usr/bin/env python - -# Import libraries -from ChaCha.chacha import Chacha -import rospy -import smach -import time -import json -from plan_parser import parse_plan -from geometry_msgs.msg import Twist -# Add states to library -from ChaCha.forward import Forward -from ChaCha.backward import Backward -from ChaCha.circle import Circle -from ChaCha.clap import Clap -from ChaCha.turnL import TurnL -from ChaCha.turnR import TurnR - - -# define state Stop -class Stop(smach.State): - def __init__(self): - smach.State.__init__(self, outcomes=['do_exit']) - self.counter = 0 - - def execute(self, userdata): - rospy.loginfo('The turtle has stopped') - time.sleep(1) - return 'do_exit' - -# Run bot - - -def runTurtleBot(plan): - rospy.init_node('squishy_turtlebot3') - turtleBot = Turtlebot(namespace="squishy") - - # Create a SMACH state machine - sm = smach.StateMachine(outcomes=['do_exit']) - sm.userdata.plan = plan - sm.userdata.plan_length = len(plan) - sm.userdata.curr_state = {} - - # Open the container - with sm: - # Add states to the container - smach.StateMachine.add('Plan', Plan(), - transitions={'do_forward': 'Forward', 'do_backward': 'Backward', 'do_circle': 'Circle', - 'do_turnL': 'TurnL', 'do_turnR': 'TurnR', 'do_clap': 'Clap', 'do_chacha': 'Chacha', 'do_exit': 'Stop'}) - smach.StateMachine.add('Forward', Forward(), - transitions={'do_plan': 'Plan'}) - smach.StateMachine.add('Backward', Backward(), - transitions={'do_plan': 'Plan'}) - smach.StateMachine.add('Circle', Circle(), - transitions={'do_plan': 'Plan'}) - smach.StateMachine.add('TurnL', TurnL(), - transitions={'do_plan': 'Plan'}) - smach.StateMachine.add('TurnR', TurnR(), - transitions={'do_plan': 'Plan'}) - smach.StateMachine.add('Clap', Clap(), - transitions={'do_clap': 'Clap'}) - smach.StateMachine.add('Chacha', Chacha(), - transitions={'do_chacha': 'Chacha'}) - smach.StateMachine.add('Stop', Stop(), - transitions={'do_exit': 'do_exit'}) - - # navigate(turtleBot) - - # rospy.spin() - # Execute SMACH plan - outcome = sm.execute() - - -if __name__ == '__main__': - plan = parse_plan("FinalProject/plans/plan2.json") - runTurtleBot(plan) diff --git a/FinalProject/plans/plan2.json b/FinalProject/plans/plan2.json deleted file mode 100644 index 3d84463..0000000 --- a/FinalProject/plans/plan2.json +++ /dev/null @@ -1,46 +0,0 @@ -{ - "states": [ - { - "name": "do_circleL", - "attributes": { - "radius": 0.95 - } - }, - { - "name": "do_backward", - "attributes": { - "distance": 4.5 - } - }, - { - "name": "do_forward", - "attributes": { - "distance": 4.20 - } - }, - { - "name": "do_threepoint" - }, - { - "name": "do_forward", - "attributes": { - "distance": 3.0 - } - }, - { - "name": "do_circleR", - "attributes": { - "radius": 1.7 - } - }, - { - "name": "do_turnL" - }, - { - "name": "do_turnR" - }, - { - "name": "do_turnL" - } - ] -} diff --git a/FinalProject/voice_control.py b/FinalProject/voice_control.py deleted file mode 100644 index 02b3370..0000000 --- a/FinalProject/voice_control.py +++ /dev/null @@ -1,120 +0,0 @@ -#!/usr/bin/env python - -import argparse -import roslib -import rospy - -from geometry_msgs.msg import Twist - -from pocketsphinx.pocketsphinx import * -from sphinxbase.sphinxbase import * -import pyaudio - -class vControl(object): - def __init__(self, model, lexicon, kwlist, pub): - # initialize ROS - self.speed = 0.2 - self.msg = Twist() - - rospy.init_node('voice_cmd_vel') - rospy.on_shutdown(self.shutdown) - - # you may need to change publisher destination depending on what you run - self.pub_ = rospy.Publisher(pub, Twist, queue_size=10) - - # initialize pocketsphinx - config = Decoder.default_config() - config.set_string('-hmm', model) - config.set_string('-dict', lexicon) - config.set_string('-kws', kwlist) - - stream = pyaudio.PyAudio().open(format=pyaudio.paInt16, channels=1, - rate=16000, input=True, frames_per_buffer=1024) - stream.start_stream() - - self.decoder = Decoder(config) - self.decoder.start_utt() - - while not rospy.is_shutdown(): - buf = stream.read(1024) - if buf: - self.decoder.process_raw(buf, False, False) - else: - break - self.parse_asr_result() - - def parse_asr_result(self): - """ - move the robot based on ASR hypothesis - """ - if self.decoder.hyp() != None: - print ([(seg.word, seg.prob, seg.start_frame, seg.end_frame) - for seg in self.decoder.seg()]) - print ("Detected keyphrase, restarting search") - seg.word = seg.word.lower() - self.decoder.end_utt() - self.decoder.start_utt() - # you may want to modify the main logic here - if seg.word.find("full speed") > -1: - if self.speed == 0.2: - self.msg.linear.x = self.msg.linear.x*2 - self.msg.angular.z = self.msg.angular.z*2 - self.speed = 0.4 - if seg.word.find("half speed") > -1: - if self.speed == 0.4: - self.msg.linear.x = self.msg.linear.x/2 - self.msg.angular.z = self.msg.angular.z/2 - self.speed = 0.2 - if seg.word.find("forward") > -1: - self.msg.linear.x = self.speed - self.msg.angular.z = 0 - elif seg.word.find("left") > -1: - if self.msg.linear.x != 0: - if self.msg.angular.z < self.speed: - self.msg.angular.z += 0.05 - else: - self.msg.angular.z = self.speed*2 - elif seg.word.find("right") > -1: - if self.msg.linear.x != 0: - if self.msg.angular.z > -self.speed: - self.msg.angular.z -= 0.05 - else: - self.msg.angular.z = -self.speed*2 - elif seg.word.find("back") > -1: - self.msg.linear.x = -self.speed - self.msg.angular.z = 0 - elif seg.word.find("stop") > -1 or seg.word.find("halt") > -1: - self.msg = Twist() - - self.pub_.publish(self.msg) - - def shutdown(self): - """ - command executed after Ctrl+C is pressed - """ - rospy.loginfo("Stop ASRControl") - self.pub_.publish(Twist()) - rospy.sleep(1) - -if __name__ == '__main__': - parser = argparse.ArgumentParser( - description='Control ROS turtlebot using pocketsphinx.') - parser.add_argument('--model', type=str, - default='/usr/share/pocketsphinx/model/hmm/en_US/hub4wsj_sc_8k', - help='''acoustic model path - (default: /usr/share/pocketsphinx/model/hmm/en_US/hub4wsj_sc_8k)''') - parser.add_argument('--lexicon', type=str, - default='dance_cmd.dic', - help='''pronunciation dictionary - (default: dance_cmd.dic)''') - parser.add_argument('--kwlist', type=str, - default='voice_cmd.kwlist', - help='''keyword list with thresholds - (default: voice_cmd.kwlist)''') - parser.add_argument('--rospub', type=str, - default='mobile_base/commands/velocity', - help='''ROS publisher destination - (default: mobile_base/commands/velocity)''') - - args = parser.parse_args() - vControl(args.model, args.lexicon, args.kwlist, args.rospub)