forked from eborghi10/create_autonomy
-
-
Notifications
You must be signed in to change notification settings - Fork 38
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
* Add ranking_controller * Implement duration field * Add friction to stage worlds * Add ranking controller using smach * Fix state machine * Fix indentation and add smach_viewer as dependency * Add angle normalization method * Fix bumper detection - Added angle normalization * Add dependencies
- Loading branch information
Showing
9 changed files
with
331 additions
and
62 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
|
|
@@ -2,7 +2,7 @@ | |
| <package> | ||
| <name>ca_node</name> | ||
| <version>1.1.0</version> | ||
| <description>ROS driver for iRobot's Create 1 and 2, based on libcreate</description> | ||
| <description>ROS driver for iRobot Create 2, based on libcreate</description> | ||
|
|
||
| <maintainer email="[email protected]">Jacob Perron</maintainer> | ||
|
|
||
|
|
@@ -11,37 +11,24 @@ | |
| <author email="[email protected]">Jacob Perron</author> | ||
|
|
||
| <buildtool_depend>catkin</buildtool_depend> | ||
| <build_depend>git</build_depend> | ||
| <build_depend>roscpp</build_depend> | ||
| <build_depend>std_msgs</build_depend> | ||
| <build_depend>geometry_msgs</build_depend> | ||
| <build_depend>nav_msgs</build_depend> | ||
| <build_depend>sensor_msgs</build_depend> | ||
| <build_depend>tf</build_depend> | ||
| <build_depend>ca_msgs</build_depend> | ||
| <build_depend>diagnostic_msgs</build_depend> | ||
| <build_depend>diagnostic_updater</build_depend> | ||
| <build_depend>libcreate</build_depend> | ||
|
|
||
| <build_depend>ca_driver</build_depend> | ||
| <build_depend>ecl_threads</build_depend> | ||
| <build_depend>nodelet</build_depend> | ||
| <build_depend>pluginlib</build_depend> | ||
| <build_depend>ecl_threads</build_depend> | ||
| <build_depend>ca_driver</build_depend> | ||
|
|
||
| <run_depend>roscpp</run_depend> | ||
| <run_depend>std_msgs</run_depend> | ||
| <run_depend>geometry_msgs</run_depend> | ||
| <run_depend>nav_msgs</run_depend> | ||
| <run_depend>sensor_msgs</run_depend> | ||
| <run_depend>tf</run_depend> | ||
| <run_depend>ca_msgs</run_depend> | ||
| <run_depend>ca_description</run_depend> | ||
| <run_depend>diagnostic_msgs</run_depend> | ||
| <run_depend>diagnostic_updater</run_depend> | ||
| <run_depend>libcreate</run_depend> | ||
| <run_depend>ca_driver</run_depend> | ||
| <run_depend>ca_msgs</run_depend> | ||
| <run_depend>ecl_threads</run_depend> | ||
| <run_depend>geometry_msgs</run_depend> | ||
| <run_depend>nodelet</run_depend> | ||
| <run_depend>pluginlib</run_depend> | ||
| <run_depend>ecl_threads</run_depend> | ||
| <run_depend>ca_driver</run_depend> | ||
| <run_depend>rospy</run_depend> | ||
| <run_depend>smach</run_depend> | ||
| <run_depend>smach_ros</run_depend> | ||
| <run_depend>smach_viewer</run_depend> | ||
| <run_depend>std_msgs</run_depend> | ||
|
|
||
| <export> | ||
| <nodelet plugin="${prefix}/plugins/nodelet_plugins.xml" /> | ||
|
|
||
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,90 @@ | ||
| #!/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.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.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.}) | ||
|
|
||
| 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() | ||
| 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) | ||
|
|
||
| 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() |
Oops, something went wrong.