Add Ranking Controller node

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<ros::NodeHandle> rosnode_;
       ros::Publisher contact_pub_;

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)))
     {

ca_gazebo/worlds/stage_1.world

@@ -52,7 +52,10 @@
           <surface>
             <bounce/>
             <friction>
-              <ode/>
+              <ode>
+                <mu>10</mu>
+                <mu2>10</mu2>
+              </ode>
             </friction>
             <contact>
               <ode/>
@@ -88,7 +91,10 @@
           <surface>
             <bounce/>
             <friction>
-              <ode/>
+              <ode>
+                <mu>10</mu>
+                <mu2>10</mu2>
+              </ode>
             </friction>
             <contact>
               <ode/>
@@ -124,7 +130,10 @@
           <surface>
             <bounce/>
             <friction>
-              <ode/>
+              <ode>
+                <mu>10</mu>
+                <mu2>10</mu2>
+              </ode>
             </friction>
             <contact>
               <ode/>
@@ -160,7 +169,10 @@
           <surface>
             <bounce/>
             <friction>
-              <ode/>
+              <ode>
+                <mu>10</mu>
+                <mu2>10</mu2>
+              </ode>
             </friction>
             <contact>
               <ode/>

ca_gazebo/worlds/stage_2.world

@@ -48,6 +48,19 @@
               <length>0.25</length>
             </cylinder>
           </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <bounce/>
+            <friction>
+              <ode>
+                <mu>10</mu>
+                <mu2>10</mu2>
+              </ode>
+            </friction>
+            <contact>
+              <ode/>
+            </contact>
+          </surface>
         </collision>
 
         <visual name="visual">
@@ -71,6 +84,19 @@
               <length>0.25</length>
             </cylinder>
           </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <bounce/>
+            <friction>
+              <ode>
+                <mu>10</mu>
+                <mu2>10</mu2>
+              </ode>
+            </friction>
+            <contact>
+              <ode/>
+            </contact>
+          </surface>
         </collision>
 
         <visual name="visual">

ca_node/CMakeLists.txt

@@ -4,41 +4,28 @@ 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)
 
 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_ros
+    smach_viewer
+    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})

ca_node/package.xml

@@ -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" />

ca_node/scripts/ranking_controller.py

@@ -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()