From 5d5feec1f7f7127af1f08f38a688242296fc5290 Mon Sep 17 00:00:00 2001 From: Emiliano Borghi Date: Mon, 17 Jun 2019 17:11:39 -0300 Subject: [PATCH 1/9] Add ranking_controller --- ca_node/scripts/ranking_controller.py | 88 +++++++++++++++++++++++++++ 1 file changed, 88 insertions(+) create mode 100755 ca_node/scripts/ranking_controller.py diff --git a/ca_node/scripts/ranking_controller.py b/ca_node/scripts/ranking_controller.py new file mode 100755 index 000000000..12c4bc08e --- /dev/null +++ b/ca_node/scripts/ranking_controller.py @@ -0,0 +1,88 @@ +#!/usr/bin/env python +import rospy +import threading +from ca_msgs.msg import Bumper +from geometry_msgs.msg import Twist, Vector3 + +class StateMachine(object): + + def __init__(self): + self.pub = rospy.Publisher("/cmd_vel", Twist, queue_size=10) + self.goal_queue = [] + + def rotate(self, ang_vel): + self.move(0., ang_vel) + + def rotate_left(self, ang_vel): + self.rotate(ang_vel) + + def rotate_right(self, ang_vel): + self.rotate(-ang_vel) + + def set_goal(self, data): + if data.is_left_pressed and data.is_right_pressed: + self.goal_queue.append({'goal': self.move_backward, 'velocity': 0.1, 'duration': 3.}) + if data.is_left_pressed: + self.goal_queue.append({'goal': self.move_backward, 'velocity': 0.1, 'duration': 1.}) + self.goal_queue.append({'goal': self.rotate_right, 'velocity': 0.3, 'duration': 1.}) + elif data.is_right_pressed: + self.goal_queue.append({'goal': self.move_backward, 'velocity': 0.1, 'duration': 1.}) + self.goal_queue.append({'goal': self.rotate_left, 'velocity': 0.3, 'duration': 1.}) + else: + self.goal_queue.append({'goal': self.move_straight, 'velocity': 0.2, 'duration': 0.}) + + def stop(self): + self.move(0., 0.) + + def close(self): + self.stop() + self.goal_queue = [] + + def move(self, lin_vel, ang_vel): + msg = Twist() + msg.linear.x = lin_vel + msg.angular.z = ang_vel + self.pub.publish(msg) + + def move_straight(self, lin_vel): + self.move(lin_vel, 0.) + + def move_backward(self, lin_vel): + self.move_straight(-lin_vel) + + def run(self): + if len(self.goal_queue) > 0: + # Execute next goal + goal = self.goal_queue.pop() + goal.get('goal')(goal.get('velocity')) + else: + # Move straight + self.move_straight(0.2) + +class RankingController(): + + def __init__(self): + rospy.init_node("ranking_controller", log_level=rospy.INFO) + self.sub = rospy.Subscriber("bumper", Bumper, self.callback) + self.state_machine = StateMachine() + self.rate = rospy.Rate(10) # Hz + rospy.on_shutdown(self.stop) + threading.Thread(name="ranking_controller", target=self.run).start() + rospy.spin() + + def callback(self, data): + rospy.logdebug("{} {}".format(data.is_left_pressed, data.is_right_pressed)) + self.state_machine.set_goal(data) + + def stop(self): + rospy.loginfo("Thread stopped.") + self.state_machine.close() + + def run(self): + rospy.loginfo("Thread started.") + while not rospy.is_shutdown(): + self.state_machine.run() + self.rate.sleep() + +if __name__ == "__main__": + rc = RankingController() From 74e7066e21e7db7eb502d53b8cdd74ed7efba54c Mon Sep 17 00:00:00 2001 From: Emiliano Borghi Date: Sat, 22 Jun 2019 07:33:29 -0300 Subject: [PATCH 2/9] Implement duration field --- ca_node/scripts/ranking_controller.py | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/ca_node/scripts/ranking_controller.py b/ca_node/scripts/ranking_controller.py index 12c4bc08e..201969c88 100755 --- a/ca_node/scripts/ranking_controller.py +++ b/ca_node/scripts/ranking_controller.py @@ -23,11 +23,11 @@ def set_goal(self, data): if data.is_left_pressed and data.is_right_pressed: self.goal_queue.append({'goal': self.move_backward, 'velocity': 0.1, 'duration': 3.}) if data.is_left_pressed: - self.goal_queue.append({'goal': self.move_backward, 'velocity': 0.1, 'duration': 1.}) - self.goal_queue.append({'goal': self.rotate_right, 'velocity': 0.3, 'duration': 1.}) + self.goal_queue.append({'goal': self.move_backward, 'velocity': 0.1, 'duration': 1.5}) + self.goal_queue.append({'goal': self.rotate_right, 'velocity': 0.3, 'duration': 2.}) elif data.is_right_pressed: - self.goal_queue.append({'goal': self.move_backward, 'velocity': 0.1, 'duration': 1.}) - self.goal_queue.append({'goal': self.rotate_left, 'velocity': 0.3, 'duration': 1.}) + self.goal_queue.append({'goal': self.move_backward, 'velocity': 0.1, 'duration': 1.5}) + self.goal_queue.append({'goal': self.rotate_left, 'velocity': 0.3, 'duration': 2.}) else: self.goal_queue.append({'goal': self.move_straight, 'velocity': 0.2, 'duration': 0.}) @@ -54,7 +54,9 @@ def run(self): if len(self.goal_queue) > 0: # Execute next goal goal = self.goal_queue.pop() - goal.get('goal')(goal.get('velocity')) + end_time = rospy.Time.now().secs + goal.get('duration') + while end_time > rospy.Time.now().secs: + goal.get('goal')(goal.get('velocity')) else: # Move straight self.move_straight(0.2) From 4e5d93163bb6f03c183e58a02b2e35a04fe6530c Mon Sep 17 00:00:00 2001 From: Emiliano Borghi Date: Sat, 22 Jun 2019 07:33:45 -0300 Subject: [PATCH 3/9] Add friction to stage worlds --- ca_gazebo/worlds/stage_1.world | 20 ++++++++++++++++---- ca_gazebo/worlds/stage_2.world | 26 ++++++++++++++++++++++++++ 2 files changed, 42 insertions(+), 4 deletions(-) diff --git a/ca_gazebo/worlds/stage_1.world b/ca_gazebo/worlds/stage_1.world index bf2ee4ff0..faadd3263 100644 --- a/ca_gazebo/worlds/stage_1.world +++ b/ca_gazebo/worlds/stage_1.world @@ -52,7 +52,10 @@ - + + 10 + 10 + @@ -88,7 +91,10 @@ - + + 10 + 10 + @@ -124,7 +130,10 @@ - + + 10 + 10 + @@ -160,7 +169,10 @@ - + + 10 + 10 + diff --git a/ca_gazebo/worlds/stage_2.world b/ca_gazebo/worlds/stage_2.world index 8946bc981..febd5747e 100644 --- a/ca_gazebo/worlds/stage_2.world +++ b/ca_gazebo/worlds/stage_2.world @@ -48,6 +48,19 @@ 0.25 + 10 + + + + + 10 + 10 + + + + + + @@ -71,6 +84,19 @@ 0.25 + 10 + + + + + 10 + 10 + + + + + + From cf33f360ea83f9223ccfd76bccc79a30f4e3f4d2 Mon Sep 17 00:00:00 2001 From: Emiliano Borghi Date: Tue, 9 Jul 2019 00:45:56 -0300 Subject: [PATCH 4/9] Add ranking controller using smach --- ca_node/CMakeLists.txt | 44 ++---- ca_node/package.xml | 39 ++---- ca_node/scripts/smach_ranking_controller.py | 145 ++++++++++++++++++++ 3 files changed, 172 insertions(+), 56 deletions(-) create mode 100755 ca_node/scripts/smach_ranking_controller.py diff --git a/ca_node/CMakeLists.txt b/ca_node/CMakeLists.txt index 9a8eb364b..01b26ba52 100644 --- a/ca_node/CMakeLists.txt +++ b/ca_node/CMakeLists.txt @@ -4,20 +4,10 @@ project(ca_node) add_compile_options(-std=c++11) find_package(catkin REQUIRED COMPONENTS - libcreate - roscpp - std_msgs - geometry_msgs - nav_msgs - sensor_msgs - tf - ca_msgs - diagnostic_msgs - diagnostic_updater + ca_driver + ecl_threads nodelet pluginlib - ecl_threads - ca_driver ) find_package(Boost REQUIRED system thread) @@ -25,20 +15,17 @@ find_package(Boost REQUIRED system thread) catkin_package( LIBRARIES ca_nodelet ca_driver CATKIN_DEPENDS - libcreate - roscpp - std_msgs - geometry_msgs - nav_msgs - sensor_msgs - tf - ca_msgs - ca_description - diagnostic_msgs - diagnostic_updater - nodelet - pluginlib + ca_description + ca_msgs ecl_threads + geometry_msgs + nodelet + pluginlib + rospy + smach + smach_viewer + # smach_ros + std_msgs ) include_directories( @@ -48,12 +35,9 @@ include_directories( add_subdirectory(src) -install(DIRECTORY plugins - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) -install(DIRECTORY launch +install(DIRECTORY launch plugins DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) -install(PROGRAMS scripts/kinect_power.py +install(DIRECTORY scripts DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) diff --git a/ca_node/package.xml b/ca_node/package.xml index 5f07edd9a..cccec19f9 100644 --- a/ca_node/package.xml +++ b/ca_node/package.xml @@ -2,7 +2,7 @@ ca_node 1.1.0 - ROS driver for iRobot's Create 1 and 2, based on libcreate + ROS driver for iRobot Create 2, based on libcreate Jacob Perron @@ -11,37 +11,24 @@ Jacob Perron catkin - git - roscpp - std_msgs - geometry_msgs - nav_msgs - sensor_msgs - tf - ca_msgs - diagnostic_msgs - diagnostic_updater - libcreate + + ca_driver + ecl_threads nodelet pluginlib - ecl_threads - ca_driver - roscpp - std_msgs - geometry_msgs - nav_msgs - sensor_msgs - tf - ca_msgs ca_description - diagnostic_msgs - diagnostic_updater - libcreate + ca_driver + ca_msgs + ecl_threads + geometry_msgs nodelet pluginlib - ecl_threads - ca_driver + rospy + smach + smach_viewer + + std_msgs diff --git a/ca_node/scripts/smach_ranking_controller.py b/ca_node/scripts/smach_ranking_controller.py new file mode 100755 index 000000000..257900325 --- /dev/null +++ b/ca_node/scripts/smach_ranking_controller.py @@ -0,0 +1,145 @@ +#!/usr/bin/env python + +import rospy +import smach +import smach_ros + +from ca_msgs.msg import Bumper +from geometry_msgs.msg import Twist + +is_left_pressed = False +is_right_pressed = False + +# define state Forward +class Forward(smach.State): + def __init__(self): + smach.State.__init__(self, outcomes=['left_pressed', 'right_pressed', 'both_pressed']) + + def execute(self, userdata): + rospy.loginfo('Going forward') + global is_left_pressed, is_right_pressed + while not is_left_pressed or not is_right_pressed: + global vel_pub + vel_msg = Twist() + vel_msg.linear.x = 0.2 + vel_pub.publish(vel_msg) + + if is_left_pressed and is_left_pressed: + return 'both_pressed' + elif is_left_pressed: + return 'left_pressed' + elif is_right_pressed: + return 'right_pressed' + else: + return 'aborted' + +# define state Backward +class Backward(smach.State): + def __init__(self): + smach.State.__init__(self, outcomes=['done']) + + def execute(self, userdata): + rospy.loginfo('Going backward') + end_time = rospy.Time.now().secs + 1.5 + while end_time > rospy.Time.now().secs: + global vel_pub + vel_msg = Twist() + vel_msg.linear.x = -0.1 + vel_pub.publish(vel_msg) + return 'done' + +# define state RotateLeft +class RotateLeft(smach.State): + def __init__(self): + smach.State.__init__(self, outcomes=['done']) + + def execute(self, userdata): + rospy.loginfo('Rotating to the left') + end_time = rospy.Time.now().secs + 2. + while end_time > rospy.Time.now().secs: + global vel_pub + vel_msg = Twist() + vel_msg.angular.z = 0.3 + vel_pub.publish(vel_msg) + return 'done' + +# define state RotateRight +class RotateRight(smach.State): + def __init__(self): + smach.State.__init__(self, outcomes=['done']) + + def execute(self, userdata): + rospy.loginfo('Rotating to the right') + end_time = rospy.Time.now().secs + 2. + while end_time > rospy.Time.now().secs: + global vel_pub + vel_msg = Twist() + vel_msg.angular.z = -0.3 + vel_pub.publish(vel_msg) + return 'done' + +def bumper_cb(msg): + global is_left_pressed, is_right_pressed + is_left_pressed = msg.is_left_pressed + is_right_pressed = msg.is_right_pressed + +def stop_cb(): + vel_pub.publish(Twist()) + +# main +def main(): + rospy.init_node('smach_ranking_controller') + _ = rospy.Subscriber("bumper", Bumper, bumper_cb) + global vel_pub + vel_pub = rospy.Publisher("cmd_vel", Twist, queue_size=1) + rospy.on_shutdown(stop_cb) + + # Create a SMACH state machine + sm = smach.StateMachine(outcomes=['aborted']) + + # Open the container + with sm: + # Add states to the container + smach.StateMachine.add('FORWARD', Forward(), + transitions={'left_pressed':'ROTATE_LEFT_SEQ', + 'right_pressed':'ROTATE_RIGHT_SEQ', + # 'both_pressed': 'BACKWARD'}) + 'both_pressed': 'ROTATE_LEFT_SEQ'}) + smach.StateMachine.add('BACKWARD', Backward(), + transitions={'done':'FORWARD'}) + + left_sq = smach.Sequence( + outcomes = ['done','aborted'], + connector_outcome = 'done') + + with left_sq: + smach.Sequence.add('BACKWARD', Backward()) + smach.Sequence.add('ROTATE_RIGHT', RotateRight()) + + right_sq = smach.Sequence( + outcomes = ['done','aborted'], + connector_outcome = 'done') + + with right_sq: + smach.Sequence.add('BACKWARD', Backward()) + smach.Sequence.add('ROTATE_LEFT', RotateLeft()) + + smach.StateMachine.add('ROTATE_LEFT_SEQ', right_sq, + transitions={'done':'FORWARD'}) + smach.StateMachine.add('ROTATE_RIGHT_SEQ', left_sq, + transitions={'done':'FORWARD'}) + + # Create and start the introspection server + sis = smach_ros.IntrospectionServer('smach_introspection', sm, '/SM_ROOT') + sis.start() + + # Execute SMACH plan + _ = sm.execute() + + rospy.spin() + sis.stop() + + +if __name__ == '__main__': + while not rospy.is_shutdown(): + main() From c25ae3a0ccfe575e07d9e26f382b5361ed01a361 Mon Sep 17 00:00:00 2001 From: Emiliano Borghi Date: Wed, 10 Jul 2019 00:03:13 -0300 Subject: [PATCH 5/9] Fix state machine --- ca_node/scripts/smach_ranking_controller.py | 31 +++++++++++++-------- 1 file changed, 20 insertions(+), 11 deletions(-) diff --git a/ca_node/scripts/smach_ranking_controller.py b/ca_node/scripts/smach_ranking_controller.py index 257900325..8b0e95c90 100755 --- a/ca_node/scripts/smach_ranking_controller.py +++ b/ca_node/scripts/smach_ranking_controller.py @@ -18,7 +18,13 @@ def __init__(self): def execute(self, userdata): rospy.loginfo('Going forward') global is_left_pressed, is_right_pressed - while not is_left_pressed or not is_right_pressed: + # Keep in this loop while: + # LEFT | RIGHT | KEEP? + # 0 | 0 | 1 --> Keep waiting + # 0 | 1 | 0 --> right_pressed + # 1 | 0 | 0 --> left_pressed + # 1 | 1 | 0 --> both_pressed + while not (is_left_pressed or is_right_pressed): global vel_pub vel_msg = Twist() vel_msg.linear.x = 0.2 @@ -101,32 +107,32 @@ def main(): with sm: # Add states to the container smach.StateMachine.add('FORWARD', Forward(), - transitions={'left_pressed':'ROTATE_LEFT_SEQ', - 'right_pressed':'ROTATE_RIGHT_SEQ', + transitions={'left_pressed':'ROTATE_RIGHT_SEQ', + 'right_pressed':'ROTATE_LEFT_SEQ', # 'both_pressed': 'BACKWARD'}) 'both_pressed': 'ROTATE_LEFT_SEQ'}) smach.StateMachine.add('BACKWARD', Backward(), transitions={'done':'FORWARD'}) - left_sq = smach.Sequence( + right_sq = smach.Sequence( outcomes = ['done','aborted'], connector_outcome = 'done') - with left_sq: + with right_sq: smach.Sequence.add('BACKWARD', Backward()) smach.Sequence.add('ROTATE_RIGHT', RotateRight()) - right_sq = smach.Sequence( + left_sq = smach.Sequence( outcomes = ['done','aborted'], connector_outcome = 'done') - with right_sq: + with left_sq: smach.Sequence.add('BACKWARD', Backward()) smach.Sequence.add('ROTATE_LEFT', RotateLeft()) - smach.StateMachine.add('ROTATE_LEFT_SEQ', right_sq, + smach.StateMachine.add('ROTATE_RIGHT_SEQ', right_sq, transitions={'done':'FORWARD'}) - smach.StateMachine.add('ROTATE_RIGHT_SEQ', left_sq, + smach.StateMachine.add('ROTATE_LEFT_SEQ', left_sq, transitions={'done':'FORWARD'}) # Create and start the introspection server @@ -141,5 +147,8 @@ def main(): if __name__ == '__main__': - while not rospy.is_shutdown(): - main() + try: + while not rospy.is_shutdown(): + main() + except rospy.ROSInterruptException: + pass From 0c909d52eb0b0c9acef04e3b54ecd8ad3e1832f7 Mon Sep 17 00:00:00 2001 From: Emiliano Borghi Date: Wed, 10 Jul 2019 00:06:27 -0300 Subject: [PATCH 6/9] Fix indentation and add smach_viewer as dependency --- ca_node/CMakeLists.txt | 6 +++--- ca_node/package.xml | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/ca_node/CMakeLists.txt b/ca_node/CMakeLists.txt index 01b26ba52..1314d75a5 100644 --- a/ca_node/CMakeLists.txt +++ b/ca_node/CMakeLists.txt @@ -22,9 +22,9 @@ catkin_package( nodelet pluginlib rospy - smach - smach_viewer - # smach_ros + smach + smach_ros + smach_viewer std_msgs ) diff --git a/ca_node/package.xml b/ca_node/package.xml index cccec19f9..5258911e7 100644 --- a/ca_node/package.xml +++ b/ca_node/package.xml @@ -26,8 +26,8 @@ pluginlib rospy smach + smach_ros smach_viewer - std_msgs From 10226ec237df503a38f86566452b2dc8910a83b3 Mon Sep 17 00:00:00 2001 From: Emiliano Borghi Date: Fri, 30 Aug 2019 18:56:13 -0300 Subject: [PATCH 7/9] Add angle normalization method --- ca_gazebo/include/ca_gazebo/create_bumper_plugin.h | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/ca_gazebo/include/ca_gazebo/create_bumper_plugin.h b/ca_gazebo/include/ca_gazebo/create_bumper_plugin.h index 9339d4d98..4312c45ac 100644 --- a/ca_gazebo/include/ca_gazebo/create_bumper_plugin.h +++ b/ca_gazebo/include/ca_gazebo/create_bumper_plugin.h @@ -63,6 +63,13 @@ namespace gazebo private: + inline float normalizeAngle(float angle) { + angle = std::fmod(angle + M_PI,2*M_PI); + if (angle < 0) + angle += 2*M_PI; + return angle - M_PI; + }; + /// \brief pointer to ros node std::shared_ptr rosnode_; ros::Publisher contact_pub_; From 9eae267d622121d868c0bdedf10021158d406e04 Mon Sep 17 00:00:00 2001 From: Emiliano Borghi Date: Fri, 30 Aug 2019 18:58:15 -0300 Subject: [PATCH 8/9] Fix bumper detection - Added angle normalization --- ca_gazebo/src/create_bumper_plugin.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ca_gazebo/src/create_bumper_plugin.cpp b/ca_gazebo/src/create_bumper_plugin.cpp index 58a9e5940..de0bb2c4a 100644 --- a/ca_gazebo/src/create_bumper_plugin.cpp +++ b/ca_gazebo/src/create_bumper_plugin.cpp @@ -127,7 +127,7 @@ void CreateBumperPlugin::OnUpdate() // using the force normals below, since the contact position is given in world coordinates // also negating the normal, because it points from contact to robot centre const double global_contact_angle = std::atan2(-contacts.contact(i).normal(0).y(), -contacts.contact(i).normal(0).x()); - const double relative_contact_angle = global_contact_angle - this->robot_heading_; + const double relative_contact_angle = normalizeAngle(global_contact_angle - this->robot_heading_); if ((relative_contact_angle <= (M_PI/2)) && (relative_contact_angle > (M_PI/6))) { From 316caf5a73751213aa5f88ac529558fb6f1a6e58 Mon Sep 17 00:00:00 2001 From: Emiliano Borghi Date: Fri, 30 Aug 2019 18:58:32 -0300 Subject: [PATCH 9/9] Add dependencies --- docker/create_kinetic_nvidia/Dockerfile | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/docker/create_kinetic_nvidia/Dockerfile b/docker/create_kinetic_nvidia/Dockerfile index e0b19be80..3f9400d87 100644 --- a/docker/create_kinetic_nvidia/Dockerfile +++ b/docker/create_kinetic_nvidia/Dockerfile @@ -16,6 +16,15 @@ USER root RUN apt-get update RUN apt-get install -y \ - ros-$ROS1_DISTRO-image-proc + ros-$ROS1_DISTRO-dwa-local-planner \ + ros-$ROS1_DISTRO-image-proc \ + ros-$ROS1_DISTRO-joy \ + ros-$ROS1_DISTRO-joy-teleop \ + ros-$ROS1_DISTRO-map-server \ + ros-$ROS1_DISTRO-mbf-costmap-core \ + ros-$ROS1_DISTRO-mbf-msgs \ + ros-$ROS1_DISTRO-move-base-flex \ + ros-$ROS1_DISTRO-slam-gmapping \ + ros-$ROS1_DISTRO-smach-viewer USER create