Demo scripts

magni_nav/README.txt

@@ -0,0 +1,16 @@
+To do simple moves:
+
+rosrun magni_nav simple_move.py forward 1  # moves robot 1 meter forward,  -1 will turn 180, go back, turn 180
+
+rosrun magni_nav simple_move.py rotate 90   # ritates 90 degrees
+
+by altenateing in a script could draw a square, etc.
+
+timed_out_and_back  = to  forward -1
+
+move_base_square = to script 
+
+if for some reason move_base isn't working corectly, with the robot or magni_gazebo running 
+in separate ssh window try:
+
+ rostopic pub -r 1 /cmd_vel geometry_msgs/Twist -- '[1.0, 0.0, 0.0]' '[0.0, 0.0, 1.0]'

magni_nav/scripts/localization_monitor/move_base_square.py

@@ -0,0 +1,172 @@
+#!/usr/bin/env python
+
+""" move_base_square.py - Version 0.1 2012-01-10
+
+    Command a robot to move in a square using move_base actions..
+
+    Created for the Pi Robot Project: http://www.pirobot.org
+    Copyright (c) 2012 Patrick Goebel.  All rights reserved.
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.5
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details at:
+
+    http://www.gnu.org/licenses/gpl.htmlPoint
+
+"""
+
+import roslib; roslib.load_manifest('rbx1_nav')
+import rospy
+import actionlib
+from actionlib_msgs.msg import *
+from geometry_msgs.msg import Pose, Point, Quaternion, Twist
+from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
+from tf.transformations import quaternion_from_euler
+from visualization_msgs.msg import Marker
+from math import radians, pi
+
+class MoveBaseSquare():
+    def __init__(self):
+        rospy.init_node('nav_test', anonymous=False)
+
+        rospy.on_shutdown(self.shutdown)
+
+        # How big is the square we want the robot to navigate?
+        square_size = rospy.get_param("~square_size", 1.0) # meters
+
+        # Create a list to hold the target quaternions (orientations)
+        quaternions = list()
+
+        # First define the corner orientations as Euler angles
+        euler_angles = (pi/2, pi, 3*pi/2, 0)
+
+        # Then convert the angles to quaternions
+        for angle in euler_angles:
+            q_angle = quaternion_from_euler(0, 0, angle, axes='sxyz')
+            q = Quaternion(*q_angle)
+            quaternions.append(q)
+
+        # Create a list to hold the waypoint poses
+        waypoints = list()
+
+        # Append each of the four waypoints to the list.  Each waypoint
+        # is a pose consisting of a position and orientation in the map frame.
+        waypoints.append(Pose(Point(square_size, 0.0, 0.0), quaternions[0]))
+        waypoints.append(Pose(Point(square_size, square_size, 0.0), quaternions[1]))
+        waypoints.append(Pose(Point(0.0, square_size, 0.0), quaternions[2]))
+        waypoints.append(Pose(Point(0.0, 0.0, 0.0), quaternions[3]))
+
+        # Initialize the visualization markers for RViz
+        self.init_markers()
+
+        # Set a visualization marker at each waypoint        
+        for waypoint in waypoints:           
+            p = Point()
+            p = waypoint.position
+            self.markers.points.append(p)
+
+        # Publisher to manually control the robot (e.g. to stop it)
+        self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist)
+
+        # Subscribe to the move_base action server
+        self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
+
+        rospy.loginfo("Waiting for move_base action server...")
+
+        # Wait 60 seconds for the action server to become available
+        self.move_base.wait_for_server(rospy.Duration(60))
+
+        rospy.loginfo("Connected to move base server")
+        rospy.loginfo("Starting navigation test")
+
+        # Initialize a counter to track waypoints
+        i = 0
+
+        # Cycle through the four waypoints
+        while i < 4 and not rospy.is_shutdown():
+            # Update the marker display
+            self.marker_pub.publish(self.markers)
+
+            # Intialize the waypoint goal
+            goal = MoveBaseGoal()
+
+            # Use the map frame to define goal poses
+            goal.target_pose.header.frame_id = 'map'
+
+            # Set the time stamp to "now"
+            goal.target_pose.header.stamp = rospy.Time.now()
+
+            # Set the goal pose to the i-th waypoint
+            goal.target_pose.pose = waypoints[i]
+
+            # Start the robot moving toward the goal
+            self.move(goal)
+
+            i += 1
+
+    def move(self, goal):
+            # Send the goal pose to the MoveBaseAction server
+            self.move_base.send_goal(goal)
+
+            # Allow 1 minute to get there
+            finished_within_time = self.move_base.wait_for_result(rospy.Duration(60)) 
+
+            # If we don't get there in time, abort the goal
+            if not finished_within_time:
+                self.move_base.cancel_goal()
+                rospy.loginfo("Timed out achieving goal")
+            else:
+                # We made it!
+                state = self.move_base.get_state()
+                if state == GoalStatus.SUCCEEDED:
+                    rospy.loginfo("Goal succeeded!")
+
+    def init_markers(self):
+        # Set up our waypoint markers
+        marker_scale = 0.2
+        marker_lifetime = 0 # 0 is forever
+        marker_ns = 'waypoints'
+        marker_id = 0
+        marker_color = {'r': 1.0, 'g': 0.7, 'b': 1.0, 'a': 1.0}
+
+        # Define a marker publisher.
+        self.marker_pub = rospy.Publisher('waypoint_markers', Marker)
+
+        # Initialize the marker points list.
+        self.markers = Marker()
+        self.markers.ns = marker_ns
+        self.markers.id = marker_id
+        self.markers.type = Marker.SPHERE_LIST
+        self.markers.action = Marker.ADD
+        self.markers.lifetime = rospy.Duration(marker_lifetime)
+        self.markers.scale.x = marker_scale
+        self.markers.scale.y = marker_scale
+        self.markers.color.r = marker_color['r']
+        self.markers.color.g = marker_color['g']
+        self.markers.color.b = marker_color['b']
+        self.markers.color.a = marker_color['a']
+
+        self.markers.header.frame_id = 'map'
+        self.markers.header.stamp = rospy.Time.now()
+        self.markers.points = list()
+
+    def shutdown(self):
+        rospy.loginfo("Stopping the robot...")
+        # Cancel any active goals
+        self.move_base.cancel_goal()
+        rospy.sleep(2)
+        # Stop the robot
+        self.cmd_vel_pub.publish(Twist())
+        rospy.sleep(1)
+
+if __name__ == '__main__':
+    try:
+        MoveBaseSquare()
+    except rospy.ROSInterruptException:
+        rospy.loginfo("Navigation test finished.")

magni_nav/scripts/localization_monitor/simple_move.py

@@ -0,0 +1,134 @@
+#!/usr/bin/python3
+
+"""
+Copyright (c) 2018, Ubiquity Robotics
+All rights reserved.
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+* Neither the name of fiducial_follow nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+"""
+
+"""
+Send a goal to move_basic to move a specified amount
+"""
+
+import rospy
+import actionlib
+from actionlib_msgs.msg import *
+import move_base_msgs.msg
+import tf.transformations
+import tf2_ros
+import tf2_geometry_msgs
+import math
+import traceback
+from geometry_msgs.msg import Quaternion, Point, PoseStamped
+
+# Utility function to convert radians to degrees
+def degrees(r):
+    return 180.0 * r / math.pi
+
+# Utility function to convert degrees to radians
+def radians(r):
+    return math.pi * r / 180.0
+
+# Utility function to return an identiy quaternion representing zero rotation
+def identity_quaternion():
+    return Quaternion(0,0,0,1)
+
+class Move:
+    """
+    Constructor for our class
+    """
+    def __init__(self):
+        # Set up transform buffer and listener
+        self.tf_buffer = tf2_ros.Buffer()
+        self.listener = tf2_ros.TransformListener(self.tf_buffer)
+
+        # Create a proxy object for the move action server
+        self.move = actionlib.SimpleActionClient('/move_base',
+                                                 move_base_msgs.msg.MoveBaseAction)
+        rospy.loginfo("Waiting for move service to be available")
+        if not self.move.wait_for_server(rospy.Duration(10.0)):
+           rospy.logerr("Move service not available")
+           self.move = None
+           return
+
+    # Go to a goal
+    def goto_goal(self, orientation=identity_quaternion(), position=Point()):
+        if self.move is None:
+            rospy.logerr("Move service not available")
+            return False
+        # Transform goal into the odom fram
+        # Create goal and actionlib call to rotate
+        pose_base = PoseStamped()
+        pose_base.header.frame_id = "base_link"
+        pose_base.pose.orientation = orientation
+        pose_base.pose.position = position
+
+        try:
+            pose_odom = self.tf_buffer.transform(pose_base, "odom",
+                                                 rospy.Duration(1.0))
+        except:
+            rospy.logerr("Unable to transform goal into odom frame")
+            return
+        goal = move_base_msgs.msg.MoveBaseGoal()
+        goal.target_pose.header.frame_id = "odom"
+        goal.target_pose = pose_odom
+        self.move.send_goal(goal)
+        self.move.wait_for_result(rospy.Duration(60.0))
+
+        if self.move.get_state() == GoalStatus.SUCCEEDED:
+           rospy.loginfo("Success")
+           return True
+        else:
+           rospy.loginfo("Failure")
+           return False
+
+    def rotate(self, angle):
+        rospy.loginfo("Rotating %f degrees" % angle)
+        q = tf.transformations.quaternion_from_euler(0, 0, radians(angle))
+        self.goto_goal(orientation=Quaternion(*q))
+
+    def forward(self, dist):
+        rospy.loginfo("Moving %f meters" % dist)
+        self.goto_goal(position=Point(dist, 0, 0))
+
+def usage():
+    print( \
+"""Usage: %s rotate|forward amount
+  rotate <angle in degrees>
+  forward <distance in meters>
+""" % sys.argv[0])
+    sys.exit(1)
+
+
+if __name__ == "__main__":
+    rospy.init_node("rotate")
+    if len(sys.argv) != 3:
+        usage()
+
+    if sys.argv[1] == "rotate":
+        node = Move()
+        node.rotate(float(sys.argv[2]))
+    elif sys.argv[1] == "forward":
+        node = Move()
+        node.forward(float(sys.argv[2]))
+    else:
+        usage()