diff --git a/magni_nav/README.txt b/magni_nav/README.txt new file mode 100644 index 00000000..dda18950 --- /dev/null +++ b/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]' diff --git a/magni_nav/scripts/localization_monitor/move_base_square.py b/magni_nav/scripts/localization_monitor/move_base_square.py new file mode 100644 index 00000000..42537006 --- /dev/null +++ b/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.") diff --git a/magni_nav/scripts/localization_monitor/simple_move.py b/magni_nav/scripts/localization_monitor/simple_move.py new file mode 100644 index 00000000..3e0e250f --- /dev/null +++ b/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 + forward +""" % 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() diff --git a/magni_nav/scripts/localization_monitor/timed_out_and_back.py b/magni_nav/scripts/localization_monitor/timed_out_and_back.py new file mode 100644 index 00000000..344cef5a --- /dev/null +++ b/magni_nav/scripts/localization_monitor/timed_out_and_back.py @@ -0,0 +1,115 @@ +#!/usr/bin/env python3 + +""" timed_out_and_back.py - Version 1.2 2014-12-14 + + A basic demo of the using odometry data to move the robot along + and out-and-back trajectory. + + 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.html + +""" + +import rospy +from geometry_msgs.msg import Twist +from math import pi + +class OutAndBack(): + def __init__(self): + # Give the node a name + rospy.init_node('out_and_back', anonymous=False) + + # Set rospy to execute a shutdown function when exiting + rospy.on_shutdown(self.shutdown) + + # Publisher to control the robot's speed + self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=1) + + # How fast will we update the robot's movement? + rate = 50 + + # Set the equivalent ROS rate variable + r = rospy.Rate(rate) + + # Set the forward linear speed to 0.2 meters per second + linear_speed = 0.2 + + # Set the travel distance to 1.0 meters + goal_distance = 1.0 + + # How long should it take us to get there? + linear_duration = goal_distance / linear_speed + + # Set the rotation speed to 1.0 radians per second + angular_speed = 1.0 + + # Set the rotation angle to Pi radians (180 degrees) + goal_angle = pi + + # How long should it take to rotate? + angular_duration = goal_angle / angular_speed + + # Loop through the two legs of the trip + for i in range(2): + # Initialize the movement command + move_cmd = Twist() + + # Set the forward speed + move_cmd.linear.x = linear_speed + + # Move forward for a time to go the desired distance + ticks = int(linear_duration * rate) + + for t in range(ticks): + self.cmd_vel.publish(move_cmd) + r.sleep() + + # Stop the robot before the rotation + move_cmd = Twist() + self.cmd_vel.publish(move_cmd) + rospy.sleep(1) + + # Now rotate left roughly 180 degrees + + # Set the angular speed + move_cmd.angular.z = angular_speed + + # Rotate for a time to go 180 degrees + ticks = int(goal_angle * rate) + + for t in range(ticks): + self.cmd_vel.publish(move_cmd) + r.sleep() + + # Stop the robot before the next leg + move_cmd = Twist() + self.cmd_vel.publish(move_cmd) + rospy.sleep(1) + + # Stop the robot + self.cmd_vel.publish(Twist()) + + def shutdown(self): + # Always stop the robot when shutting down the node. + rospy.loginfo("Stopping the robot...") + self.cmd_vel.publish(Twist()) + rospy.sleep(1) + +if __name__ == '__main__': + try: + OutAndBack() + except: + rospy.loginfo("Out-and-Back node terminated.") +