diff --git a/ca_gazebo/include/ca_gazebo/create_bumper_plugin.h b/ca_gazebo/include/ca_gazebo/create_bumper_plugin.h index 9339d4d9..4312c45a 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_; diff --git a/ca_gazebo/src/create_bumper_plugin.cpp b/ca_gazebo/src/create_bumper_plugin.cpp index 58a9e594..de0bb2c4 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))) { diff --git a/ca_gazebo/worlds/stage_1.world b/ca_gazebo/worlds/stage_1.world index bf2ee4ff..faadd326 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 8946bc98..febd5747 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 + + + + + + diff --git a/ca_node/CMakeLists.txt b/ca_node/CMakeLists.txt index 9a8eb364..1314d75a 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_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}) diff --git a/ca_node/package.xml b/ca_node/package.xml index 5f07edd9..5258911e 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_ros + smach_viewer + std_msgs diff --git a/ca_node/scripts/ranking_controller.py b/ca_node/scripts/ranking_controller.py new file mode 100755 index 00000000..201969c8 --- /dev/null +++ b/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() diff --git a/ca_node/scripts/smach_ranking_controller.py b/ca_node/scripts/smach_ranking_controller.py new file mode 100755 index 00000000..8b0e95c9 --- /dev/null +++ b/ca_node/scripts/smach_ranking_controller.py @@ -0,0 +1,154 @@ +#!/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 + # 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 + 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_RIGHT_SEQ', + 'right_pressed':'ROTATE_LEFT_SEQ', + # 'both_pressed': 'BACKWARD'}) + 'both_pressed': 'ROTATE_LEFT_SEQ'}) + smach.StateMachine.add('BACKWARD', Backward(), + transitions={'done':'FORWARD'}) + + right_sq = smach.Sequence( + outcomes = ['done','aborted'], + connector_outcome = 'done') + + with right_sq: + smach.Sequence.add('BACKWARD', Backward()) + smach.Sequence.add('ROTATE_RIGHT', RotateRight()) + + left_sq = smach.Sequence( + outcomes = ['done','aborted'], + connector_outcome = 'done') + + with left_sq: + smach.Sequence.add('BACKWARD', Backward()) + smach.Sequence.add('ROTATE_LEFT', RotateLeft()) + + smach.StateMachine.add('ROTATE_RIGHT_SEQ', right_sq, + transitions={'done':'FORWARD'}) + smach.StateMachine.add('ROTATE_LEFT_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__': + try: + while not rospy.is_shutdown(): + main() + except rospy.ROSInterruptException: + pass diff --git a/docker/create_kinetic_nvidia/Dockerfile b/docker/create_kinetic_nvidia/Dockerfile index e0b19be8..3f9400d8 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