├── Robotic AGV.pdf ├── video Demo └── Robotic AGV.mp4 ├── mybot_navigation ├── config │ ├── joint_names_final.yaml │ ├── local_costmap_params.yaml │ ├── base_local_planner_params.yaml │ ├── global_costmap_params.yaml │ ├── costmap_common_params.yaml │ └── control.yaml ├── launch │ ├── simple_tele_top.launch │ ├── gmapping_demo.launch │ ├── final6_gmapping_demo.launch │ └── amcl_demo.launch ├── scripts │ ├── controller.py │ ├── bluetooth_robot_teleop.py │ ├── odom_pub.py │ ├── talk.py │ └── robot_teleop.py ├── package.xml ├── map_saver.cpp └── CMakeLists.txt ├── mybot_description ├── launch │ ├── mybot_rviz_amcl.launch │ ├── mybot_rviz.launch │ ├── final6_rviz_gmapping.launch │ └── mybot_rviz_gmapping.launch ├── urdf │ ├── materials.xacro │ ├── macros.xacro │ ├── mybot.gazebo │ ├── final6.gazebo │ ├── mybot.xacro │ └── final6.xacro ├── package.xml ├── rviz │ ├── mapping.rviz │ └── amcl.rviz └── CMakeLists.txt ├── mybot_gazebo ├── worlds │ ├── mybot.world │ ├── tutorial_testing.world │ └── turtlebot_playground.world ├── launch │ ├── mybot_world.launch │ └── final6_world.launch ├── package.xml └── CMakeLists.txt ├── README.md └── CMakeLists.txt /Robotic AGV.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sudarshan-s-harithas/Robotic-AGV/HEAD/Robotic AGV.pdf -------------------------------------------------------------------------------- /video Demo/Robotic AGV.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sudarshan-s-harithas/Robotic-AGV/HEAD/video Demo/Robotic AGV.mp4 -------------------------------------------------------------------------------- /mybot_navigation/config/joint_names_final.yaml: -------------------------------------------------------------------------------- 1 | controller_joint_names: ['coupler_front_left_joint', 'coupler_front_right_joint', 'coupler_back_left_joint', 'coupler_back_right_joint', 'wheel_front_left_joint', 'wheel_front_right_joint', 'wheel_back_left_joint', 'wheel_back_right_joint', ] 2 | -------------------------------------------------------------------------------- /mybot_navigation/config/local_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | local_costmap: 2 | global_frame: odom 3 | robot_base_frame: chassis 4 | update_frequency: 2.0 5 | publish_frequency: 1.0 6 | static_map: false 7 | rolling_window: true 8 | width: 6.0 9 | height: 6.0 10 | resolution: 0.05 11 | -------------------------------------------------------------------------------- /mybot_navigation/config/base_local_planner_params.yaml: -------------------------------------------------------------------------------- 1 | TrajectoryPlannerROS: 2 | max_vel_x: 0.5 3 | min_vel_x: 0.01 4 | max_vel_theta: 1.5 5 | min_in_place_vel_theta: 0.01 6 | 7 | acc_lim_theta: 1.5 8 | acc_lim_x: 0.5 9 | acc_lim_y: 0.5 10 | 11 | 12 | holonomic_robot: false 13 | -------------------------------------------------------------------------------- /mybot_navigation/config/global_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | # static_map - True if using existing map 2 | 3 | global_costmap: 4 | global_frame: odom 5 | robot_base_frame: chassis_link 6 | update_frequency: 1.0 7 | publish_frequency: 1.0 8 | resolution: 0.05 9 | static_map: true 10 | width: 10.0 11 | height: 10.0 12 | -------------------------------------------------------------------------------- /mybot_navigation/launch/simple_tele_top.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /mybot_description/launch/mybot_rviz_amcl.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /mybot_navigation/config/costmap_common_params.yaml: -------------------------------------------------------------------------------- 1 | obstacle_range: 2.5 2 | raytrace_range: 3.0 3 | #footprint: [[x0, y0], [x1, y1], ... [xn, yn]] 4 | #robot_radius: ir_of_robot 5 | robot_radius: 0.5 # distance a circular robot should be clear of the obstacle 6 | inflation_radius: 3.0 7 | 8 | observation_sources: laser_scan_sensor #point_cloud_sensor 9 | 10 | # marking - add obstacle information to cost map 11 | # clearing - clear obstacle information to cost map 12 | laser_scan_sensor: {sensor_frame: lidar_Link, data_type: LaserScan, topic: /scan1, marking: true, clearing: true} 13 | 14 | #point_cloud_sensor: {sensor_frame: frame_name, data_type: PointCloud, topic: topic_name, marking: true, clearing: true} 15 | -------------------------------------------------------------------------------- /mybot_description/launch/mybot_rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /mybot_description/launch/final6_rviz_gmapping.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /mybot_description/launch/mybot_rviz_gmapping.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /mybot_gazebo/worlds/mybot.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 9 | 10 | 11 | model://ground_plane 12 | 13 | 14 | 15 | 16 | model://sun 17 | 18 | 19 | 20 | 21 | 22 | 4.927360 -4.376610 3.740080 0.000000 0.275643 2.356190 23 | orbit 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /mybot_description/urdf/materials.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /mybot_navigation/launch/gmapping_demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /mybot_navigation/launch/final6_gmapping_demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /mybot_gazebo/launch/mybot_world.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /mybot_gazebo/launch/final6_world.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /mybot_navigation/scripts/controller.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/python 2 | 3 | import rospy 4 | from tf.transformations import euler_from_quaternion 5 | from nav_msgs.msg import Odometry 6 | from geometry_msgs.msg import Point, Twist 7 | from math import atan2 8 | 9 | x = 0.0 10 | y = 0.0 11 | theta = 0.0 12 | 13 | def moveRobot(x, y): 14 | goal = Point() 15 | goal.x = x 16 | goal.y = y 17 | return goal 18 | 19 | 20 | def newOdom (msg): 21 | global x 22 | global y 23 | global theta 24 | 25 | x = msg.pose.pose.position.x 26 | y = msg.pose.pose.position.y 27 | 28 | rot_q = msg.pose.pose.orientation 29 | 30 | (roll, pitch, theta) = euler_from_quaternion([rot_q.x, rot_q.y, rot_q.z, rot_q.w]) 31 | 32 | 33 | rospy.init_node("speed_controller") 34 | 35 | sub = rospy.Subscriber("odom", Odometry, newOdom) 36 | pub = rospy.Publisher("cmd_vel", Twist, queue_size = 1) 37 | 38 | 39 | r = rospy.Rate(4) 40 | 41 | speed = Twist() 42 | 43 | goal = moveRobot(-1,1) 44 | 45 | while not rospy.is_shutdown(): 46 | inc_x = goal.x - x 47 | inc_y = goal.y - y 48 | 49 | angle_to_goal = atan2(inc_y, inc_x) 50 | 51 | print angle_to_goal 52 | 53 | if abs(angle_to_goal - theta) > 0.1: 54 | speed.linear.x = 0.0 55 | speed.angular.z = 0.2 56 | else: 57 | speed.linear.x = 0.1 58 | speed.angular.z = 0.0 59 | 60 | if x - goal.x < inc_x and y - goal.y < inc_y: 61 | speed.linear.x = 0.0 62 | speed.angular.z = 0.0 63 | 64 | pub.publish(speed) 65 | r.sleep() 66 | -------------------------------------------------------------------------------- /mybot_navigation/scripts/bluetooth_robot_teleop.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | 4 | import rospy 5 | from std_msgs.msg import String 6 | 7 | from geometry_msgs.msg import Twist 8 | from time import sleep 9 | 10 | 11 | 12 | speed=5 13 | 14 | 15 | rospy.init_node('rotate') 16 | 17 | 18 | def callback(data): 19 | 20 | hi=data.data 21 | 22 | msg=Twist() 23 | global speed 24 | if hi=="q": 25 | 26 | speed=speed*2 27 | print(speed) 28 | 29 | 30 | 31 | 32 | if hi =="i": 33 | 34 | 35 | 36 | 37 | 38 | msg.linear.x = speed; msg.linear.y = 0.0; msg.linear.z = 0 39 | msg.angular.x = 0; msg.angular.y = 0; msg.angular.z = 0 40 | publisher.publish(msg) 41 | 42 | 43 | 44 | 45 | if hi == "j" : 46 | 47 | msg.linear.x = 0.0; msg.linear.y = 0; msg.linear.z = 0 48 | msg.angular.x = 0; msg.angular.y = 0; msg.angular.z = 0.5 49 | publisher.publish(msg) 50 | 51 | if hi == "o": 52 | 53 | msg.linear.x = 0.0; msg.linear.y = 0; msg.linear.z = 0 54 | msg.angular.x = 0; msg.angular.y = 0; msg.angular.z = 0.0 55 | publisher.publish(msg) 56 | 57 | 58 | 59 | 60 | publisher=rospy.Publisher('/cmd_vel',Twist,queue_size=1) 61 | 62 | 63 | rotate_right = True 64 | 65 | if __name__ == '__main__': 66 | 67 | 68 | rospy.Subscriber("chatter", String, callback) 69 | 70 | 71 | 72 | 73 | 74 | 75 | rospy.spin() 76 | -------------------------------------------------------------------------------- /mybot_navigation/config/control.yaml: -------------------------------------------------------------------------------- 1 | final: 2 | joint_state_controller: 3 | type: joint_state_controller/JointStateController 4 | publish_rate: 50 5 | 6 | wheel_front_left_joint_position_controller: 7 | type: velocity_controllers/JointVelocityController 8 | joint: wheel_front_left_joint 9 | pid: {p: 100.0, i: 0.01, d: 10.0} 10 | 11 | wheel_front_right_joint_position_controller: 12 | type: velocity_controllers/JointVelocityController 13 | joint: wheel_front_right_joint 14 | pid: {p: 100.0, i: 0.01, d: 10.0} 15 | 16 | wheel_back_left_joint_position_controller: 17 | type: velocity_controllers/JointVelocityController 18 | joint: wheel_back_left_joint 19 | pid: {p: 100.0, i: 0.01, d: 10.0} 20 | 21 | coupler_back_right_joint_position_controller: 22 | type: velocity_controllers/JointVelocityController 23 | joint: coupler_back_right_joint 24 | pid: {p: 100.0, i: 0.01, d: 10.0} 25 | 26 | coupler_front_left_joint_position_controller: 27 | type: velocity_controllers/JointVelocityController 28 | joint: coupler_front_left_joint 29 | pid: {p: 100.0, i: 0.01, d: 10.0} 30 | 31 | coupler_front_right_joint_position_controller: 32 | type: velocity_controllers/JointVelocityController 33 | joint: coupler_front_right_joint 34 | pid: {p: 100.0, i: 0.01, d: 10.0} 35 | 36 | coupler_back_left_joint_position_controller: 37 | type: velocity_controllers/JointVelocityController 38 | joint: coupler_back_left_joint 39 | pid: {p: 100.0, i: 0.01, d: 10.0} 40 | 41 | wheel_back_right_joint_position_controller: 42 | type: velocity_controllers/JointVelocityController 43 | joint: wheel_back_right_joint 44 | pid: {p: 100.0, i: 0.01, d: 10.0} 45 | -------------------------------------------------------------------------------- /mybot_navigation/scripts/odom_pub.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | import math 4 | from math import sin, cos, pi 5 | 6 | import rospy 7 | import tf 8 | from nav_msgs.msg import Odometry 9 | from geometry_msgs.msg import Point, Pose, Quaternion, Twist, Vector3 10 | 11 | rospy.init_node('odometry_publisher') 12 | 13 | odom_pub = rospy.Publisher("odom", Odometry, queue_size=50) 14 | odom_broadcaster = tf.TransformBroadcaster() 15 | 16 | x = 0.0 17 | y = 0.0 18 | th = 0.0 19 | 20 | vx = 0.1 21 | vy = -0.1 22 | vth = 0.1 23 | 24 | current_time = rospy.Time.now() 25 | last_time = rospy.Time.now() 26 | 27 | r = rospy.Rate(1.0) 28 | while not rospy.is_shutdown(): 29 | current_time = rospy.Time.now() 30 | 31 | # compute odometry in a typical way given the velocities of the robot 32 | dt = (current_time - last_time).to_sec() 33 | delta_x = (vx * cos(th) - vy * sin(th)) * dt 34 | delta_y = (vx * sin(th) + vy * cos(th)) * dt 35 | delta_th = vth * dt 36 | 37 | x += delta_x 38 | y += delta_y 39 | th += delta_th 40 | 41 | # since all odometry is 6DOF we'll need a quaternion created from yaw 42 | odom_quat = tf.transformations.quaternion_from_euler(0, 0, th) 43 | 44 | # first, we'll publish the transform over tf 45 | odom_broadcaster.sendTransform( 46 | (x, y, 0.), 47 | odom_quat, 48 | current_time, 49 | "odom", 50 | "chassis_link" 51 | ) 52 | 53 | # next, we'll publish the odometry message over ROS 54 | odom = Odometry() 55 | odom.header.stamp = current_time 56 | odom.header.frame_id = "odom" 57 | 58 | # set the position 59 | odom.pose.pose = Pose(Point(x, y, 0.), Quaternion(*odom_quat)) 60 | 61 | # set the velocity 62 | odom.child_frame_id = "chassis_link" 63 | odom.twist.twist = Twist(Vector3(vx, vy, 0), Vector3(0, 0, vth)) 64 | 65 | # publish the message 66 | odom_pub.publish(odom) 67 | 68 | last_time = current_time 69 | r.sleep() -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Robotic AGV 2 | 3 | The modelling of the robot has been done using solidworks. The model consists of a chassis to which four motors with quadrature encoders have been attached. **Mecanum wheels have been used to provide Omni-directional movement**, a BeagleBone Blue is placed on the chassis, and the LIDAR is mounted on the top. A Video Demo of the Omni directional Movement has been givn in the Video Demo folder. 4 | 5 | The URDF (Universal Robot Description Format) model is a collection of files that describe a robots physical description to ROS. These files are used by ROS to tell the computer what the robot actually looks like in real life. URDF files are needed in order for ROS to understand and be able to simulate situations with the robot before a researcher or engineer actually acquires the robot. It is the standard ROS XML representation of the robot model.Here individual parts have been designed and assembled as a whole, and once the model is ready a SW2URDF addin was installed for exporting the model to a URDF file (Unified Robot Description Format), this file is used to perform simulation in ROS. 6 | 7 | Here is the guide to run the SLAM Simulation of the Robotic AGV on ROS 8 | 9 | 1. **roslaunch mybot_gazebo final6_world.launch** 10 | The Gazebo world opens 11 | 2. **roslaunch mybot_navigation final6_gmapping_demo.launch** 12 | Launch Gmapping SLAM 13 | 3. **roslaunch mybot_description final6_rviz_gmapping.launch** 14 | Launch RVIZ and obtain Laser scan data and map in Rviz 15 | 16 | 4. **roslaunch mybot_navigation simple_tele_top.launch** 17 | launch teletop and move the robot through the world and generate the complete map 18 | 19 | 5. **rosrun map_server map_saver -f ~/{ws_name}/src/mybot_navigation/{add_remanaing_path}** 20 | Save the map using the above command 21 | 22 | The locomotion of the Robot can be controlled remotely using mobile phone using bletooth refer to the Robotic AGV.pdf for the details. 23 | 24 | -------------------------------------------------------------------------------- /mybot_navigation/launch/amcl_demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | -------------------------------------------------------------------------------- /mybot_navigation/scripts/talk.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import bluetooth, time 4 | import rospy 5 | from std_msgs.msg import String 6 | 7 | search_time = 10 8 | addr = None 9 | 10 | 11 | print("Welcome to the Bluetooth Detection Demo! \nMake sure your desired Bluetooth-capable device is turned on and discoverable.") 12 | 13 | if addr == None: 14 | try: 15 | input("When you are ready to begin, press the Enter key to continue...") 16 | except SyntaxError: 17 | pass 18 | 19 | print("Searching for devices...") 20 | 21 | nearby_devices = bluetooth.discover_devices(duration=search_time, flush_cache=True, lookup_names=True) 22 | 23 | if len(nearby_devices) > 0: 24 | print("Found %d devices!" % len(nearby_devices)) 25 | else: 26 | print("No devices found! Please check your Bluetooth device and restart the demo!") 27 | exit(0) 28 | 29 | i = 0 # Just an incrementer for labeling the list entries 30 | # Print out a list of all the discovered Bluetooth Devices 31 | for addr, name in nearby_devices: 32 | print("%s. %s - %s" % (i, addr, name)) 33 | i =+ 1 34 | 35 | device_num = input("Please specify the number of the device you want to track: ") 36 | 37 | # extract out the useful info on the desired device for use later 38 | addr, name = nearby_devices[device_num][0], nearby_devices[device_num][1] 39 | 40 | 41 | server_socket=bluetooth.BluetoothSocket( bluetooth.RFCOMM ) 42 | port = 1 43 | server_socket.bind(("",port)) 44 | server_socket.listen(1) 45 | 46 | client_socket,addr = server_socket.accept() 47 | print "Accepted connection from ",addr 48 | 49 | data="" 50 | while 1: 51 | data= client_socket.recv(1024) 52 | print "Received: %s" % data 53 | pub = rospy.Publisher('chatter',String , queue_size=1) 54 | rospy.init_node('talk', anonymous=True) 55 | # while not rospy.is_shutdown(): 56 | hello_str = "%s" % data 57 | ##rospy.loginfo(hello_str) 58 | pub.publish(hello_str) 59 | 60 | 61 | 62 | 63 | 64 | client_socket.close() 65 | server_socket.close() 66 | 67 | 68 | -------------------------------------------------------------------------------- /mybot_description/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | mybot_description 4 | 0.0.0 5 | The mybot_description package 6 | 7 | 8 | 9 | 10 | Sudarshan S Harithas 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | -------------------------------------------------------------------------------- /mybot_navigation/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | mybot_navigation 4 | 0.0.0 5 | The mybot_navigation package 6 | 7 | 8 | 9 | 10 | Sudarshan S Harithas 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | -------------------------------------------------------------------------------- /mybot_gazebo/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | mybot_gazebo 4 | 0.0.0 5 | The mybot_gazebo package 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | TODO 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | catkin 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # toplevel CMakeLists.txt for a catkin workspace 2 | # catkin/cmake/toplevel.cmake 3 | 4 | cmake_minimum_required(VERSION 2.8.3) 5 | 6 | set(CATKIN_TOPLEVEL TRUE) 7 | 8 | # search for catkin within the workspace 9 | set(_cmd "catkin_find_pkg" "catkin" "${CMAKE_SOURCE_DIR}") 10 | execute_process(COMMAND ${_cmd} 11 | RESULT_VARIABLE _res 12 | OUTPUT_VARIABLE _out 13 | ERROR_VARIABLE _err 14 | OUTPUT_STRIP_TRAILING_WHITESPACE 15 | ERROR_STRIP_TRAILING_WHITESPACE 16 | ) 17 | if(NOT _res EQUAL 0 AND NOT _res EQUAL 2) 18 | # searching fot catkin resulted in an error 19 | string(REPLACE ";" " " _cmd_str "${_cmd}") 20 | message(FATAL_ERROR "Search for 'catkin' in workspace failed (${_cmd_str}): ${_err}") 21 | endif() 22 | 23 | # include catkin from workspace or via find_package() 24 | if(_res EQUAL 0) 25 | set(catkin_EXTRAS_DIR "${CMAKE_SOURCE_DIR}/${_out}/cmake") 26 | # include all.cmake without add_subdirectory to let it operate in same scope 27 | include(${catkin_EXTRAS_DIR}/all.cmake NO_POLICY_SCOPE) 28 | add_subdirectory("${_out}") 29 | 30 | else() 31 | # use either CMAKE_PREFIX_PATH explicitly passed to CMake as a command line argument 32 | # or CMAKE_PREFIX_PATH from the environment 33 | if(NOT DEFINED CMAKE_PREFIX_PATH) 34 | if(NOT "$ENV{CMAKE_PREFIX_PATH}" STREQUAL "") 35 | string(REPLACE ":" ";" CMAKE_PREFIX_PATH $ENV{CMAKE_PREFIX_PATH}) 36 | endif() 37 | endif() 38 | 39 | # list of catkin workspaces 40 | set(catkin_search_path "") 41 | foreach(path ${CMAKE_PREFIX_PATH}) 42 | if(EXISTS "${path}/.catkin") 43 | list(FIND catkin_search_path ${path} _index) 44 | if(_index EQUAL -1) 45 | list(APPEND catkin_search_path ${path}) 46 | endif() 47 | endif() 48 | endforeach() 49 | 50 | # search for catkin in all workspaces 51 | set(CATKIN_TOPLEVEL_FIND_PACKAGE TRUE) 52 | find_package(catkin QUIET 53 | NO_POLICY_SCOPE 54 | PATHS ${catkin_search_path} 55 | NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH) 56 | unset(CATKIN_TOPLEVEL_FIND_PACKAGE) 57 | 58 | if(NOT catkin_FOUND) 59 | message(FATAL_ERROR "find_package(catkin) failed. catkin was neither found in the workspace nor in the CMAKE_PREFIX_PATH. One reason may be that no ROS setup.sh was sourced before.") 60 | endif() 61 | endif() 62 | 63 | catkin_workspace() 64 | -------------------------------------------------------------------------------- /mybot_description/urdf/macros.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | -------------------------------------------------------------------------------- /mybot_navigation/scripts/robot_teleop.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | 5 | from geometry_msgs.msg import Twist 6 | 7 | import sys, select, termios, tty 8 | 9 | msg = """ 10 | Control Your Robot! 11 | --------------------------- 12 | Moving around: 13 | u i o 14 | j k l 15 | m , . 16 | 17 | q/z : increase/decrease max speeds by 10% 18 | w/x : increase/decrease only linear speed by 10% 19 | e/c : increase/decrease only angular speed by 10% 20 | space key, k : force stop 21 | anything else : stop smoothly 22 | 23 | CTRL-C to quit 24 | """ 25 | 26 | moveBindings = { 27 | 'i':(1,0), 28 | 'o':(1,-1), 29 | 'j':(0,1), 30 | 'l':(0,-1), 31 | 'u':(1,1), 32 | ',':(-1,0), 33 | '.':(-1,1), 34 | 'm':(-1,-1), 35 | } 36 | 37 | speedBindings={ 38 | 'q':(1.1,1.1), 39 | 'z':(.9,.9), 40 | 'w':(1.1,1), 41 | 'x':(.9,1), 42 | 'e':(1,1.1), 43 | 'c':(1,.9), 44 | } 45 | 46 | def getKey(): 47 | tty.setraw(sys.stdin.fileno()) 48 | rlist, _, _ = select.select([sys.stdin], [], [], 0.1) 49 | if rlist: 50 | key = sys.stdin.read(1) 51 | else: 52 | key = '' 53 | 54 | termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings) 55 | return key 56 | 57 | speed = .2 58 | turn = 1 59 | 60 | def vels(speed,turn): 61 | return "currently:\tspeed %s\tturn %s " % (speed,turn) 62 | 63 | if __name__=="__main__": 64 | settings = termios.tcgetattr(sys.stdin) 65 | 66 | rospy.init_node('simple_robot_teleop_key') 67 | pub = rospy.Publisher('/cmd_vel', Twist, queue_size=5) 68 | 69 | x = 0 70 | th = 0 71 | status = 0 72 | count = 0 73 | acc = 0.1 74 | target_speed = 0 75 | target_turn = 0 76 | control_speed = 0 77 | control_turn = 0 78 | try: 79 | print msg 80 | print vels(speed,turn) 81 | while(1): 82 | key = getKey() 83 | if key in moveBindings.keys(): 84 | x = moveBindings[key][0] 85 | th = moveBindings[key][1] 86 | count = 0 87 | elif key in speedBindings.keys(): 88 | speed = speed * speedBindings[key][0] 89 | turn = turn * speedBindings[key][1] 90 | count = 0 91 | 92 | print vels(speed,turn) 93 | if (status == 14): 94 | print msg 95 | status = (status + 1) % 15 96 | elif key == ' ' or key == 'k' : 97 | x = 0 98 | th = 0 99 | control_speed = 0 100 | control_turn = 0 101 | else: 102 | count = count + 1 103 | if count > 4: 104 | x = 0 105 | th = 0 106 | if (key == '\x03'): 107 | break 108 | 109 | target_speed = speed * x 110 | target_turn = turn * th 111 | 112 | if target_speed > control_speed: 113 | control_speed = min( target_speed, control_speed + 0.02 ) 114 | elif target_speed < control_speed: 115 | control_speed = max( target_speed, control_speed - 0.02 ) 116 | else: 117 | control_speed = target_speed 118 | 119 | if target_turn > control_turn: 120 | control_turn = min( target_turn, control_turn + 0.1 ) 121 | elif target_turn < control_turn: 122 | control_turn = max( target_turn, control_turn - 0.1 ) 123 | else: 124 | control_turn = target_turn 125 | 126 | twist = Twist() 127 | twist.linear.x = control_speed; twist.linear.y = 0; twist.linear.z = 0 128 | twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = control_turn 129 | pub.publish(twist) 130 | 131 | #print("loop: {0}".format(count)) 132 | #print("target: vx: {0}, wz: {1}".format(target_speed, target_turn)) 133 | #print("publihsed: vx: {0}, wz: {1}".format(twist.linear.x, twist.angular.z)) 134 | 135 | except: 136 | print e 137 | 138 | finally: 139 | twist = Twist() 140 | twist.linear.x = 0; twist.linear.y = 0; twist.linear.z = 0 141 | twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0 142 | pub.publish(twist) 143 | 144 | termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings) 145 | 146 | 147 | -------------------------------------------------------------------------------- /mybot_description/urdf/mybot.gazebo: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 20 | 21 | 22 | 23 | false 24 | true 25 | 10 26 | wheel_front_left_joint 27 | wheel_front_right_joint 28 | 0.4 29 | 0.2 30 | 0 31 | 10 32 | cmd_vel 33 | odom 34 | odom 35 | chassis_link 36 | 37 | 38 | 39 | 40 | 41 | false 42 | true 43 | 10 44 | wheel_front_left_joint 45 | wheel_front_right_joint 46 | 0.4 47 | 0.2 48 | 0 49 | 10 50 | cmd_vel 51 | odom 52 | odom 53 | chassis_link 54 | 55 | 56 | 57 | 58 | 59 | 60 | Gazebo/Green 61 | 62 | 30.0 63 | 64 | 1.3962634 65 | 66 | 800 67 | 800 68 | R8G8B8 69 | 70 | 71 | 0.02 72 | 300 73 | 74 | 75 | 76 | true 77 | 0.0 78 | mybot/camera1 79 | image_raw 80 | camera_info 81 | camera 82 | 0.07 83 | 0.0 84 | 0.0 85 | 0.0 86 | 0.0 87 | 0.0 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | Gazebo/Blue 96 | false 97 | 98 | 99 | 100 | 0.025 0 0 0 0 0 101 | true 102 | 100 103 | 104 | 105 | 106 | 107 | 700 108 | 1 109 | -2.170796 110 | 2.170796 111 | 112 | 113 | 114 | 115 | 0.1 116 | 30 117 | 0.01 118 | 119 | 120 | 121 | 122 | 123 | /scan1 124 | sharp1 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | Gazebo/Blue 134 | false 135 | 136 | 137 | 138 | Gazebo/Black 139 | false 140 | 141 | 142 | 143 | Gazebo/Black 144 | false 145 | 146 | 147 | 148 | Gazebo/Black 149 | false 150 | 151 | 152 | 153 | Gazebo/Black 154 | false 155 | 156 | 157 | 158 | 159 | 160 | -------------------------------------------------------------------------------- /mybot_navigation/map_saver.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * map_saver 3 | * Copyright (c) 2008, Willow Garage, Inc. 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 9 | * * Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * * Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in the 13 | * documentation and/or other materials provided with the distribution. 14 | * * Neither the name of the nor the names of its 15 | * contributors may be used to endorse or promote products derived from 16 | * this software without specific prior written permission. 17 | * 18 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 22 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | * POSSIBILITY OF SUCH DAMAGE. 29 | */ 30 | 31 | #include 32 | #include "ros/ros.h" 33 | #include "ros/console.h" 34 | #include "nav_msgs/GetMap.h" 35 | #include "tf/LinearMath/Matrix3x3.h" 36 | #include "geometry_msgs/Quaternion.h" 37 | 38 | using namespace std; 39 | 40 | /** 41 | * @brief Map generation node. 42 | */ 43 | class MapGenerator 44 | { 45 | 46 | public: 47 | MapGenerator(const std::string& mapname) : mapname_(mapname), saved_map_(false) 48 | { 49 | ros::NodeHandle n; 50 | ROS_INFO("Waiting for the map"); 51 | map_sub_ = n.subscribe("map", 1, &MapGenerator::mapCallback, this); 52 | } 53 | 54 | void mapCallback(const nav_msgs::OccupancyGridConstPtr& map) 55 | { 56 | ROS_INFO("Received a %d X %d map @ %.3f m/pix", 57 | map->info.width, 58 | map->info.height, 59 | map->info.resolution); 60 | 61 | 62 | std::string mapdatafile = mapname_ + ".pgm"; 63 | ROS_INFO("Writing map occupancy data to %s", mapdatafile.c_str()); 64 | FILE* out = fopen(mapdatafile.c_str(), "w"); 65 | if (!out) 66 | { 67 | ROS_ERROR("Couldn't save map file to %s", mapdatafile.c_str()); 68 | return; 69 | } 70 | 71 | fprintf(out, "P5\n# CREATOR: Map_generator.cpp %.3f m/pix\n%d %d\n255\n", 72 | map->info.resolution, map->info.width, map->info.height); 73 | for(unsigned int y = 0; y < map->info.height; y++) { 74 | for(unsigned int x = 0; x < map->info.width; x++) { 75 | unsigned int i = x + (map->info.height - y - 1) * map->info.width; 76 | if (map->data[i] == 0) { //occ [0,0.1) 77 | fputc(254, out); 78 | } else if (map->data[i] == +100) { //occ (0.65,1] 79 | fputc(000, out); 80 | } else { //occ [0.1,0.65] 81 | fputc(205, out); 82 | } 83 | } 84 | } 85 | 86 | fclose(out); 87 | 88 | 89 | std::string mapmetadatafile = mapname_ + ".yaml"; 90 | ROS_INFO("Writing map occupancy data to %s", mapmetadatafile.c_str()); 91 | FILE* yaml = fopen(mapmetadatafile.c_str(), "w"); 92 | 93 | 94 | /* 95 | resolution: 0.100000 96 | origin: [0.000000, 0.000000, 0.000000] 97 | # 98 | negate: 0 99 | occupied_thresh: 0.65 100 | free_thresh: 0.196 101 | 102 | */ 103 | 104 | geometry_msgs::Quaternion orientation = map->info.origin.orientation; 105 | tf::Matrix3x3 mat(tf::Quaternion(orientation.x, orientation.y, orientation.z, orientation.w)); 106 | double yaw, pitch, roll; 107 | mat.getEulerYPR(yaw, pitch, roll); 108 | 109 | fprintf(yaml, "image: %s\nresolution: %f\norigin: [%f, %f, %f]\nnegate: 0\noccupied_thresh: 0.65\nfree_thresh: 0.196\n\n", 110 | mapdatafile.c_str(), map->info.resolution, map->info.origin.position.x, map->info.origin.position.y, yaw); 111 | 112 | fclose(yaml); 113 | 114 | ROS_INFO("Done\n"); 115 | saved_map_ = true; 116 | } 117 | 118 | std::string mapname_; 119 | ros::Subscriber map_sub_; 120 | bool saved_map_; 121 | 122 | }; 123 | 124 | #define USAGE "Usage: \n" \ 125 | " map_saver -h\n"\ 126 | " map_saver [-f ] [ROS remapping args]" 127 | 128 | int main(int argc, char** argv) 129 | { 130 | ros::init(argc, argv, "map_saver"); 131 | std::string mapname = "map"; 132 | 133 | for(int i=1; i 2 | 3 | 4 | 20 | 21 | 41 | 42 | 43 | 44 | 45 | 46 | false 47 | true 48 | 10 49 | wheel_front_left_joint 50 | wheel_front_right_joint 51 | 0.4 52 | 0.2 53 | 0 54 | 10 55 | cmd_vel 56 | odom 57 | odom 58 | chassis_link 59 | 60 | 61 | 62 | 63 | 64 | false 65 | true 66 | 10 67 | wheel_back_left_joint 68 | wheel_back_right_joint 69 | 0.4 70 | 0.2 71 | 0 72 | 10 73 | cmd_vel 74 | odom 75 | odom 76 | chassis_link 77 | 78 | 79 | 80 | 81 | 116 | 117 | 118 | 0.025 0 0 0 0 0 119 | true 120 | 40 121 | 122 | 123 | 124 | 720 125 | 1 126 | -3.14 127 | 0.00 128 | 129 | 130 | 131 | 0.10 132 | 30.0 133 | 0.01 134 | 135 | 136 | gaussian 137 | 141 | 0.0 142 | 0.01 143 | 144 | 145 | 146 | /scan1 147 | lidar_Link 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | Gazebo/Blue 157 | false 158 | 159 | 160 | 161 | Gazebo/Black 162 | false 163 | 164 | 165 | 166 | Gazebo/Black 167 | false 168 | 169 | 170 | 171 | Gazebo/Black 172 | false 173 | 174 | 175 | 176 | Gazebo/Black 177 | false 178 | 179 | 180 | 181 | 182 | -------------------------------------------------------------------------------- /mybot_description/rviz/mapping.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Map1 10 | - /LaserScan1 11 | Splitter Ratio: 0.5 12 | Tree Height: 747 13 | - Class: rviz/Selection 14 | Name: Selection 15 | - Class: rviz/Tool Properties 16 | Expanded: 17 | - /2D Pose Estimate1 18 | - /2D Nav Goal1 19 | - /Publish Point1 20 | Name: Tool Properties 21 | Splitter Ratio: 0.588679016 22 | - Class: rviz/Views 23 | Expanded: 24 | - /Current View1 25 | Name: Views 26 | Splitter Ratio: 0.5 27 | - Class: rviz/Time 28 | Experimental: false 29 | Name: Time 30 | SyncMode: 0 31 | SyncSource: LaserScan 32 | Visualization Manager: 33 | Class: "" 34 | Displays: 35 | - Alpha: 0.5 36 | Cell Size: 1 37 | Class: rviz/Grid 38 | Color: 160; 160; 164 39 | Enabled: true 40 | Line Style: 41 | Line Width: 0.0299999993 42 | Value: Lines 43 | Name: Grid 44 | Normal Cell Count: 0 45 | Offset: 46 | X: 0 47 | Y: 0 48 | Z: 0 49 | Plane: XY 50 | Plane Cell Count: 10 51 | Reference Frame: 52 | Value: true 53 | - Alpha: 1 54 | Class: rviz/RobotModel 55 | Collision Enabled: false 56 | Enabled: true 57 | Links: 58 | All Links Enabled: true 59 | Expand Joint Details: false 60 | Expand Link Details: false 61 | Expand Tree: false 62 | Link Tree Style: Links in Alphabetic Order 63 | camera: 64 | Alpha: 1 65 | Show Axes: false 66 | Show Trail: false 67 | Value: true 68 | chassis: 69 | Alpha: 1 70 | Show Axes: false 71 | Show Trail: false 72 | Value: true 73 | hokuyo: 74 | Alpha: 1 75 | Show Axes: false 76 | Show Trail: false 77 | Value: true 78 | left_wheel: 79 | Alpha: 1 80 | Show Axes: false 81 | Show Trail: false 82 | Value: true 83 | right_wheel: 84 | Alpha: 1 85 | Show Axes: false 86 | Show Trail: false 87 | Value: true 88 | Name: RobotModel 89 | Robot Description: robot_description 90 | TF Prefix: "" 91 | Update Interval: 0 92 | Value: true 93 | Visual Enabled: true 94 | - Alpha: 0.699999988 95 | Class: rviz/Map 96 | Color Scheme: map 97 | Draw Behind: false 98 | Enabled: true 99 | Name: Map 100 | Topic: /map 101 | Unreliable: false 102 | Use Timestamp: false 103 | Value: true 104 | - Alpha: 1 105 | Autocompute Intensity Bounds: true 106 | Autocompute Value Bounds: 107 | Max Value: 10 108 | Min Value: -10 109 | Value: true 110 | Axis: Z 111 | Channel Name: intensity 112 | Class: rviz/LaserScan 113 | Color: 255; 255; 255 114 | Color Transformer: Intensity 115 | Decay Time: 0 116 | Enabled: true 117 | Invert Rainbow: false 118 | Max Color: 255; 255; 255 119 | Max Intensity: 0 120 | Min Color: 0; 0; 0 121 | Min Intensity: 0 122 | Name: LaserScan 123 | Position Transformer: XYZ 124 | Queue Size: 10 125 | Selectable: true 126 | Size (Pixels): 3 127 | Size (m): 0.0500000007 128 | Style: Flat Squares 129 | Topic: /mybot/laser/scan 130 | Unreliable: false 131 | Use Fixed Frame: true 132 | Use rainbow: true 133 | Value: true 134 | Enabled: true 135 | Global Options: 136 | Background Color: 48; 48; 48 137 | Default Light: true 138 | Fixed Frame: map 139 | Frame Rate: 30 140 | Name: root 141 | Tools: 142 | - Class: rviz/Interact 143 | Hide Inactive Objects: true 144 | - Class: rviz/MoveCamera 145 | - Class: rviz/Select 146 | - Class: rviz/FocusCamera 147 | - Class: rviz/Measure 148 | - Class: rviz/SetInitialPose 149 | Topic: /initialpose 150 | - Class: rviz/SetGoal 151 | Topic: /move_base_simple/goal 152 | - Class: rviz/PublishPoint 153 | Single click: true 154 | Topic: /clicked_point 155 | Value: true 156 | Views: 157 | Current: 158 | Class: rviz/Orbit 159 | Distance: 9.82933807 160 | Enable Stereo Rendering: 161 | Stereo Eye Separation: 0.0599999987 162 | Stereo Focal Distance: 1 163 | Swap Stereo Eyes: false 164 | Value: false 165 | Focal Point: 166 | X: 0.135420367 167 | Y: 0.424718022 168 | Z: -0.153741345 169 | Focal Shape Fixed Size: true 170 | Focal Shape Size: 0.0500000007 171 | Invert Z Axis: false 172 | Name: Current View 173 | Near Clip Distance: 0.00999999978 174 | Pitch: 1.23479664 175 | Target Frame: 176 | Value: Orbit (rviz) 177 | Yaw: 1.42039514 178 | Saved: ~ 179 | Window Geometry: 180 | Displays: 181 | collapsed: true 182 | Height: 1028 183 | Hide Left Dock: true 184 | Hide Right Dock: true 185 | QMainWindow State: 000000ff00000000fd00000004000000000000016a0000037afc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007300000000280000037a000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000037afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000280000037a000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003a00000003efc0100000002fb0000000800540069006d00650100000000000003a00000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000003a00000037a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 186 | Selection: 187 | collapsed: false 188 | Time: 189 | collapsed: false 190 | Tool Properties: 191 | collapsed: false 192 | Views: 193 | collapsed: true 194 | Width: 928 195 | X: 779 196 | Y: 74 197 | -------------------------------------------------------------------------------- /mybot_gazebo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(mybot_gazebo) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED) 8 | 9 | ## System dependencies are found with CMake's conventions 10 | # find_package(Boost REQUIRED COMPONENTS system) 11 | 12 | 13 | ## Uncomment this if the package has a setup.py. This macro ensures 14 | ## modules and global scripts declared therein get installed 15 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 16 | # catkin_python_setup() 17 | 18 | ################################################ 19 | ## Declare ROS messages, services and actions ## 20 | ################################################ 21 | 22 | ## To declare and build messages, services or actions from within this 23 | ## package, follow these steps: 24 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 25 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 26 | ## * In the file package.xml: 27 | ## * add a build_depend tag for "message_generation" 28 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 29 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 30 | ## but can be declared for certainty nonetheless: 31 | ## * add a run_depend tag for "message_runtime" 32 | ## * In this file (CMakeLists.txt): 33 | ## * add "message_generation" and every package in MSG_DEP_SET to 34 | ## find_package(catkin REQUIRED COMPONENTS ...) 35 | ## * add "message_runtime" and every package in MSG_DEP_SET to 36 | ## catkin_package(CATKIN_DEPENDS ...) 37 | ## * uncomment the add_*_files sections below as needed 38 | ## and list every .msg/.srv/.action file to be processed 39 | ## * uncomment the generate_messages entry below 40 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 41 | 42 | ## Generate messages in the 'msg' folder 43 | # add_message_files( 44 | # FILES 45 | # Message1.msg 46 | # Message2.msg 47 | # ) 48 | 49 | ## Generate services in the 'srv' folder 50 | # add_service_files( 51 | # FILES 52 | # Service1.srv 53 | # Service2.srv 54 | # ) 55 | 56 | ## Generate actions in the 'action' folder 57 | # add_action_files( 58 | # FILES 59 | # Action1.action 60 | # Action2.action 61 | # ) 62 | 63 | ## Generate added messages and services with any dependencies listed here 64 | # generate_messages( 65 | # DEPENDENCIES 66 | # std_msgs # Or other packages containing msgs 67 | # ) 68 | 69 | ################################################ 70 | ## Declare ROS dynamic reconfigure parameters ## 71 | ################################################ 72 | 73 | ## To declare and build dynamic reconfigure parameters within this 74 | ## package, follow these steps: 75 | ## * In the file package.xml: 76 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 77 | ## * In this file (CMakeLists.txt): 78 | ## * add "dynamic_reconfigure" to 79 | ## find_package(catkin REQUIRED COMPONENTS ...) 80 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 81 | ## and list every .cfg file to be processed 82 | 83 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 84 | # generate_dynamic_reconfigure_options( 85 | # cfg/DynReconf1.cfg 86 | # cfg/DynReconf2.cfg 87 | # ) 88 | 89 | ################################### 90 | ## catkin specific configuration ## 91 | ################################### 92 | ## The catkin_package macro generates cmake config files for your package 93 | ## Declare things to be passed to dependent projects 94 | ## INCLUDE_DIRS: uncomment this if you package contains header files 95 | ## LIBRARIES: libraries you create in this project that dependent projects also need 96 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 97 | ## DEPENDS: system dependencies of this project that dependent projects also need 98 | catkin_package( 99 | # INCLUDE_DIRS include 100 | # LIBRARIES mybot_gazebo 101 | # CATKIN_DEPENDS other_catkin_pkg 102 | # DEPENDS system_lib 103 | ) 104 | 105 | ########### 106 | ## Build ## 107 | ########### 108 | 109 | ## Specify additional locations of header files 110 | ## Your package locations should be listed before other locations 111 | # include_directories(include) 112 | 113 | ## Declare a C++ library 114 | # add_library(mybot_gazebo 115 | # src/${PROJECT_NAME}/mybot_gazebo.cpp 116 | # ) 117 | 118 | ## Add cmake target dependencies of the library 119 | ## as an example, code may need to be generated before libraries 120 | ## either from message generation or dynamic reconfigure 121 | # add_dependencies(mybot_gazebo ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 122 | 123 | ## Declare a C++ executable 124 | # add_executable(mybot_gazebo_node src/mybot_gazebo_node.cpp) 125 | 126 | ## Add cmake target dependencies of the executable 127 | ## same as for the library above 128 | # add_dependencies(mybot_gazebo_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 129 | 130 | ## Specify libraries to link a library or executable target against 131 | # target_link_libraries(mybot_gazebo_node 132 | # ${catkin_LIBRARIES} 133 | # ) 134 | 135 | ############# 136 | ## Install ## 137 | ############# 138 | 139 | # all install targets should use catkin DESTINATION variables 140 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 141 | 142 | ## Mark executable scripts (Python etc.) for installation 143 | ## in contrast to setup.py, you can choose the destination 144 | # install(PROGRAMS 145 | # scripts/my_python_script 146 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 147 | # ) 148 | 149 | ## Mark executables and/or libraries for installation 150 | # install(TARGETS mybot_gazebo mybot_gazebo_node 151 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 152 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 153 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 154 | # ) 155 | 156 | ## Mark cpp header files for installation 157 | # install(DIRECTORY include/${PROJECT_NAME}/ 158 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 159 | # FILES_MATCHING PATTERN "*.h" 160 | # PATTERN ".svn" EXCLUDE 161 | # ) 162 | 163 | ## Mark other files for installation (e.g. launch and bag files, etc.) 164 | # install(FILES 165 | # # myfile1 166 | # # myfile2 167 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 168 | # ) 169 | 170 | ############# 171 | ## Testing ## 172 | ############# 173 | 174 | ## Add gtest based cpp test target and link libraries 175 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_mybot_gazebo.cpp) 176 | # if(TARGET ${PROJECT_NAME}-test) 177 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 178 | # endif() 179 | 180 | ## Add folders to be run by python nosetests 181 | # catkin_add_nosetests(test) 182 | -------------------------------------------------------------------------------- /mybot_navigation/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(mybot_navigation) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED) 8 | 9 | ## System dependencies are found with CMake's conventions 10 | # find_package(Boost REQUIRED COMPONENTS system) 11 | 12 | 13 | ## Uncomment this if the package has a setup.py. This macro ensures 14 | ## modules and global scripts declared therein get installed 15 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 16 | # catkin_python_setup() 17 | 18 | ################################################ 19 | ## Declare ROS messages, services and actions ## 20 | ################################################ 21 | 22 | ## To declare and build messages, services or actions from within this 23 | ## package, follow these steps: 24 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 25 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 26 | ## * In the file package.xml: 27 | ## * add a build_depend tag for "message_generation" 28 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 29 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 30 | ## but can be declared for certainty nonetheless: 31 | ## * add a run_depend tag for "message_runtime" 32 | ## * In this file (CMakeLists.txt): 33 | ## * add "message_generation" and every package in MSG_DEP_SET to 34 | ## find_package(catkin REQUIRED COMPONENTS ...) 35 | ## * add "message_runtime" and every package in MSG_DEP_SET to 36 | ## catkin_package(CATKIN_DEPENDS ...) 37 | ## * uncomment the add_*_files sections below as needed 38 | ## and list every .msg/.srv/.action file to be processed 39 | ## * uncomment the generate_messages entry below 40 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 41 | 42 | ## Generate messages in the 'msg' folder 43 | # add_message_files( 44 | # FILES 45 | # Message1.msg 46 | # Message2.msg 47 | # ) 48 | 49 | ## Generate services in the 'srv' folder 50 | # add_service_files( 51 | # FILES 52 | # Service1.srv 53 | # Service2.srv 54 | # ) 55 | 56 | ## Generate actions in the 'action' folder 57 | # add_action_files( 58 | # FILES 59 | # Action1.action 60 | # Action2.action 61 | # ) 62 | 63 | ## Generate added messages and services with any dependencies listed here 64 | # generate_messages( 65 | # DEPENDENCIES 66 | # std_msgs # Or other packages containing msgs 67 | # ) 68 | 69 | ################################################ 70 | ## Declare ROS dynamic reconfigure parameters ## 71 | ################################################ 72 | 73 | ## To declare and build dynamic reconfigure parameters within this 74 | ## package, follow these steps: 75 | ## * In the file package.xml: 76 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 77 | ## * In this file (CMakeLists.txt): 78 | ## * add "dynamic_reconfigure" to 79 | ## find_package(catkin REQUIRED COMPONENTS ...) 80 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 81 | ## and list every .cfg file to be processed 82 | 83 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 84 | # generate_dynamic_reconfigure_options( 85 | # cfg/DynReconf1.cfg 86 | # cfg/DynReconf2.cfg 87 | # ) 88 | 89 | ################################### 90 | ## catkin specific configuration ## 91 | ################################### 92 | ## The catkin_package macro generates cmake config files for your package 93 | ## Declare things to be passed to dependent projects 94 | ## INCLUDE_DIRS: uncomment this if you package contains header files 95 | ## LIBRARIES: libraries you create in this project that dependent projects also need 96 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 97 | ## DEPENDS: system dependencies of this project that dependent projects also need 98 | catkin_package( 99 | # INCLUDE_DIRS include 100 | # LIBRARIES mybot_navigation 101 | # CATKIN_DEPENDS other_catkin_pkg 102 | # DEPENDS system_lib 103 | ) 104 | 105 | ########### 106 | ## Build ## 107 | ########### 108 | 109 | ## Specify additional locations of header files 110 | ## Your package locations should be listed before other locations 111 | # include_directories(include) 112 | 113 | ## Declare a C++ library 114 | # add_library(mybot_navigation 115 | # src/${PROJECT_NAME}/mybot_navigation.cpp 116 | # ) 117 | 118 | ## Add cmake target dependencies of the library 119 | ## as an example, code may need to be generated before libraries 120 | ## either from message generation or dynamic reconfigure 121 | # add_dependencies(mybot_navigation ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 122 | 123 | ## Declare a C++ executable 124 | # add_executable(mybot_navigation_node src/mybot_navigation_node.cpp) 125 | 126 | ## Add cmake target dependencies of the executable 127 | ## same as for the library above 128 | # add_dependencies(mybot_navigation_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 129 | 130 | ## Specify libraries to link a library or executable target against 131 | # target_link_libraries(mybot_navigation_node 132 | # ${catkin_LIBRARIES} 133 | # ) 134 | 135 | ############# 136 | ## Install ## 137 | ############# 138 | 139 | # all install targets should use catkin DESTINATION variables 140 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 141 | 142 | ## Mark executable scripts (Python etc.) for installation 143 | ## in contrast to setup.py, you can choose the destination 144 | # install(PROGRAMS 145 | # scripts/my_python_script 146 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 147 | # ) 148 | 149 | ## Mark executables and/or libraries for installation 150 | # install(TARGETS mybot_navigation mybot_navigation_node 151 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 152 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 153 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 154 | # ) 155 | 156 | ## Mark cpp header files for installation 157 | # install(DIRECTORY include/${PROJECT_NAME}/ 158 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 159 | # FILES_MATCHING PATTERN "*.h" 160 | # PATTERN ".svn" EXCLUDE 161 | # ) 162 | 163 | ## Mark other files for installation (e.g. launch and bag files, etc.) 164 | # install(FILES 165 | # # myfile1 166 | # # myfile2 167 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 168 | # ) 169 | 170 | ############# 171 | ## Testing ## 172 | ############# 173 | 174 | ## Add gtest based cpp test target and link libraries 175 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_mybot_navigation.cpp) 176 | # if(TARGET ${PROJECT_NAME}-test) 177 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 178 | # endif() 179 | 180 | ## Add folders to be run by python nosetests 181 | # catkin_add_nosetests(test) 182 | -------------------------------------------------------------------------------- /mybot_description/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(mybot_description) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED) 8 | 9 | ## System dependencies are found with CMake's conventions 10 | # find_package(Boost REQUIRED COMPONENTS system) 11 | 12 | 13 | ## Uncomment this if the package has a setup.py. This macro ensures 14 | ## modules and global scripts declared therein get installed 15 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 16 | # catkin_python_setup() 17 | 18 | ################################################ 19 | ## Declare ROS messages, services and actions ## 20 | ################################################ 21 | 22 | ## To declare and build messages, services or actions from within this 23 | ## package, follow these steps: 24 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 25 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 26 | ## * In the file package.xml: 27 | ## * add a build_depend tag for "message_generation" 28 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 29 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 30 | ## but can be declared for certainty nonetheless: 31 | ## * add a run_depend tag for "message_runtime" 32 | ## * In this file (CMakeLists.txt): 33 | ## * add "message_generation" and every package in MSG_DEP_SET to 34 | ## find_package(catkin REQUIRED COMPONENTS ...) 35 | ## * add "message_runtime" and every package in MSG_DEP_SET to 36 | ## catkin_package(CATKIN_DEPENDS ...) 37 | ## * uncomment the add_*_files sections below as needed 38 | ## and list every .msg/.srv/.action file to be processed 39 | ## * uncomment the generate_messages entry below 40 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 41 | 42 | ## Generate messages in the 'msg' folder 43 | # add_message_files( 44 | # FILES 45 | # Message1.msg 46 | # Message2.msg 47 | # ) 48 | 49 | ## Generate services in the 'srv' folder 50 | # add_service_files( 51 | # FILES 52 | # Service1.srv 53 | # Service2.srv 54 | # ) 55 | 56 | ## Generate actions in the 'action' folder 57 | # add_action_files( 58 | # FILES 59 | # Action1.action 60 | # Action2.action 61 | # ) 62 | 63 | ## Generate added messages and services with any dependencies listed here 64 | # generate_messages( 65 | # DEPENDENCIES 66 | # std_msgs # Or other packages containing msgs 67 | # ) 68 | 69 | ################################################ 70 | ## Declare ROS dynamic reconfigure parameters ## 71 | ################################################ 72 | 73 | ## To declare and build dynamic reconfigure parameters within this 74 | ## package, follow these steps: 75 | ## * In the file package.xml: 76 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 77 | ## * In this file (CMakeLists.txt): 78 | ## * add "dynamic_reconfigure" to 79 | ## find_package(catkin REQUIRED COMPONENTS ...) 80 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 81 | ## and list every .cfg file to be processed 82 | 83 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 84 | # generate_dynamic_reconfigure_options( 85 | # cfg/DynReconf1.cfg 86 | # cfg/DynReconf2.cfg 87 | # ) 88 | 89 | ################################### 90 | ## catkin specific configuration ## 91 | ################################### 92 | ## The catkin_package macro generates cmake config files for your package 93 | ## Declare things to be passed to dependent projects 94 | ## INCLUDE_DIRS: uncomment this if you package contains header files 95 | ## LIBRARIES: libraries you create in this project that dependent projects also need 96 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 97 | ## DEPENDS: system dependencies of this project that dependent projects also need 98 | catkin_package( 99 | # INCLUDE_DIRS include 100 | # LIBRARIES mybot_description 101 | # CATKIN_DEPENDS other_catkin_pkg 102 | # DEPENDS system_lib 103 | ) 104 | 105 | ########### 106 | ## Build ## 107 | ########### 108 | 109 | ## Specify additional locations of header files 110 | ## Your package locations should be listed before other locations 111 | # include_directories(include) 112 | 113 | ## Declare a C++ library 114 | # add_library(mybot_description 115 | # src/${PROJECT_NAME}/mybot_description.cpp 116 | # ) 117 | 118 | ## Add cmake target dependencies of the library 119 | ## as an example, code may need to be generated before libraries 120 | ## either from message generation or dynamic reconfigure 121 | # add_dependencies(mybot_description ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 122 | 123 | ## Declare a C++ executable 124 | # add_executable(mybot_description_node src/mybot_description_node.cpp) 125 | 126 | ## Add cmake target dependencies of the executable 127 | ## same as for the library above 128 | # add_dependencies(mybot_description_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 129 | 130 | ## Specify libraries to link a library or executable target against 131 | # target_link_libraries(mybot_description_node 132 | # ${catkin_LIBRARIES} 133 | # ) 134 | 135 | ############# 136 | ## Install ## 137 | ############# 138 | 139 | # all install targets should use catkin DESTINATION variables 140 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 141 | 142 | ## Mark executable scripts (Python etc.) for installation 143 | ## in contrast to setup.py, you can choose the destination 144 | # install(PROGRAMS 145 | # scripts/my_python_script 146 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 147 | # ) 148 | 149 | ## Mark executables and/or libraries for installation 150 | # install(TARGETS mybot_description mybot_description_node 151 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 152 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 153 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 154 | # ) 155 | 156 | ## Mark cpp header files for installation 157 | # install(DIRECTORY include/${PROJECT_NAME}/ 158 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 159 | # FILES_MATCHING PATTERN "*.h" 160 | # PATTERN ".svn" EXCLUDE 161 | # ) 162 | 163 | ## Mark other files for installation (e.g. launch and bag files, etc.) 164 | # install(FILES 165 | # # myfile1 166 | # # myfile2 167 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 168 | # ) 169 | 170 | ############# 171 | ## Testing ## 172 | ############# 173 | 174 | ## Add gtest based cpp test target and link libraries 175 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_mybot_description.cpp) 176 | # if(TARGET ${PROJECT_NAME}-test) 177 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 178 | # endif() 179 | 180 | ## Add folders to be run by python nosetests 181 | # catkin_add_nosetests(test) 182 | -------------------------------------------------------------------------------- /mybot_description/rviz/amcl.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /LaserScan1 10 | - /Path (local)1 11 | - /Path (global)1 12 | Splitter Ratio: 0.5 13 | Tree Height: 747 14 | - Class: rviz/Selection 15 | Name: Selection 16 | - Class: rviz/Tool Properties 17 | Expanded: 18 | - /2D Pose Estimate1 19 | - /2D Nav Goal1 20 | - /Publish Point1 21 | Name: Tool Properties 22 | Splitter Ratio: 0.588679016 23 | - Class: rviz/Views 24 | Expanded: 25 | - /Current View1 26 | Name: Views 27 | Splitter Ratio: 0.5 28 | - Class: rviz/Time 29 | Experimental: false 30 | Name: Time 31 | SyncMode: 0 32 | SyncSource: LaserScan 33 | Visualization Manager: 34 | Class: "" 35 | Displays: 36 | - Alpha: 0.5 37 | Cell Size: 1 38 | Class: rviz/Grid 39 | Color: 160; 160; 164 40 | Enabled: true 41 | Line Style: 42 | Line Width: 0.0299999993 43 | Value: Lines 44 | Name: Grid 45 | Normal Cell Count: 0 46 | Offset: 47 | X: 0 48 | Y: 0 49 | Z: 0 50 | Plane: XY 51 | Plane Cell Count: 10 52 | Reference Frame: 53 | Value: true 54 | - Alpha: 1 55 | Class: rviz/RobotModel 56 | Collision Enabled: false 57 | Enabled: true 58 | Links: 59 | All Links Enabled: true 60 | Expand Joint Details: false 61 | Expand Link Details: false 62 | Expand Tree: false 63 | Link Tree Style: Links in Alphabetic Order 64 | camera: 65 | Alpha: 1 66 | Show Axes: false 67 | Show Trail: false 68 | Value: true 69 | chassis: 70 | Alpha: 1 71 | Show Axes: false 72 | Show Trail: false 73 | Value: true 74 | hokuyo: 75 | Alpha: 1 76 | Show Axes: false 77 | Show Trail: false 78 | Value: true 79 | left_wheel: 80 | Alpha: 1 81 | Show Axes: false 82 | Show Trail: false 83 | Value: true 84 | right_wheel: 85 | Alpha: 1 86 | Show Axes: false 87 | Show Trail: false 88 | Value: true 89 | Name: RobotModel 90 | Robot Description: robot_description 91 | TF Prefix: "" 92 | Update Interval: 0 93 | Value: true 94 | Visual Enabled: true 95 | - Alpha: 0.699999988 96 | Class: rviz/Map 97 | Color Scheme: map 98 | Draw Behind: false 99 | Enabled: true 100 | Name: Map 101 | Topic: /move_base/global_costmap/costmap 102 | Unreliable: false 103 | Value: true 104 | - Alpha: 1 105 | Autocompute Intensity Bounds: true 106 | Autocompute Value Bounds: 107 | Max Value: 10 108 | Min Value: -10 109 | Value: true 110 | Axis: Z 111 | Channel Name: intensity 112 | Class: rviz/LaserScan 113 | Color: 255; 255; 255 114 | Color Transformer: Intensity 115 | Decay Time: 0 116 | Enabled: true 117 | Invert Rainbow: false 118 | Max Color: 255; 255; 255 119 | Max Intensity: 0 120 | Min Color: 0; 0; 0 121 | Min Intensity: 0 122 | Name: LaserScan 123 | Position Transformer: XYZ 124 | Queue Size: 10 125 | Selectable: true 126 | Size (Pixels): 3 127 | Size (m): 0.0500000007 128 | Style: Flat Squares 129 | Topic: /mybot/laser/scan 130 | Unreliable: false 131 | Use Fixed Frame: true 132 | Use rainbow: true 133 | Value: true 134 | - Alpha: 1 135 | Buffer Length: 1 136 | Class: rviz/Path 137 | Color: 25; 255; 0 138 | Enabled: true 139 | Line Style: Lines 140 | Line Width: 0.0299999993 141 | Name: Path (local) 142 | Offset: 143 | X: 0 144 | Y: 0 145 | Z: 0 146 | Topic: /move_base/TrajectoryPlannerROS/local_plan 147 | Unreliable: false 148 | Value: true 149 | - Alpha: 1 150 | Buffer Length: 1 151 | Class: rviz/Path 152 | Color: 0; 0; 255 153 | Enabled: true 154 | Line Style: Lines 155 | Line Width: 0.0299999993 156 | Name: Path (global) 157 | Offset: 158 | X: 0 159 | Y: 0 160 | Z: 0 161 | Topic: /move_base/TrajectoryPlannerROS/global_plan 162 | Unreliable: false 163 | Value: true 164 | Enabled: true 165 | Global Options: 166 | Background Color: 48; 48; 48 167 | Fixed Frame: odom 168 | Frame Rate: 30 169 | Name: root 170 | Tools: 171 | - Class: rviz/Interact 172 | Hide Inactive Objects: true 173 | - Class: rviz/MoveCamera 174 | - Class: rviz/Select 175 | - Class: rviz/FocusCamera 176 | - Class: rviz/Measure 177 | - Class: rviz/SetInitialPose 178 | Topic: /initialpose 179 | - Class: rviz/SetGoal 180 | Topic: /move_base_simple/goal 181 | - Class: rviz/PublishPoint 182 | Single click: true 183 | Topic: /clicked_point 184 | Value: true 185 | Views: 186 | Current: 187 | Class: rviz/Orbit 188 | Distance: 16.6426525 189 | Enable Stereo Rendering: 190 | Stereo Eye Separation: 0.0599999987 191 | Stereo Focal Distance: 1 192 | Swap Stereo Eyes: false 193 | Value: false 194 | Focal Point: 195 | X: -2.73463702 196 | Y: -0.407819152 197 | Z: 0.260570765 198 | Name: Current View 199 | Near Clip Distance: 0.00999999978 200 | Pitch: 1.20479667 201 | Target Frame: 202 | Value: Orbit (rviz) 203 | Yaw: 1.47039497 204 | Saved: ~ 205 | Window Geometry: 206 | Displays: 207 | collapsed: false 208 | Height: 1028 209 | Hide Left Dock: false 210 | Hide Right Dock: true 211 | QMainWindow State: 000000ff00000000fd00000004000000000000016a0000037afc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000280000037a000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000037afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000280000037a000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003a00000003efc0100000002fb0000000800540069006d00650100000000000003a00000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000002300000037a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 212 | Selection: 213 | collapsed: false 214 | Time: 215 | collapsed: false 216 | Tool Properties: 217 | collapsed: false 218 | Views: 219 | collapsed: true 220 | Width: 928 221 | X: 992 222 | Y: 24 223 | -------------------------------------------------------------------------------- /mybot_gazebo/worlds/tutorial_testing.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 1 6 | 0 0 10 0 -0 0 7 | 0.8 0.8 0.8 1 8 | 0.2 0.2 0.2 1 9 | 10 | 1000 11 | 0.9 12 | 0.01 13 | 0.001 14 | 15 | -0.5 0.1 -0.9 16 | 17 | 18 | 1 19 | 20 | 21 | 22 | 23 | 0 0 1 24 | 100 100 25 | 26 | 27 | 28 | 29 | 30 | 100 31 | 50 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 10 40 | 41 | 42 | 0 43 | 44 | 45 | 0 0 1 46 | 100 100 47 | 48 | 49 | 50 | 54 | 55 | 56 | 57 | 0 58 | 0 59 | 60 | 0 61 | 0 62 | 1 63 | 64 | 65 | 66 | 0.001 67 | 1 68 | 1000 69 | 0 0 -9.8 70 | 71 | 72 | 0.4 0.4 0.4 1 73 | 0.7 0.7 0.7 1 74 | 1 75 | 76 | 77 | EARTH_WGS84 78 | 0 79 | 0 80 | 0 81 | 0 82 | 83 | 84 | 1 85 | 86 | 87 | 88 | 89 | 7.32 0.2 1 90 | 91 | 92 | 0 0 0.5 0 -0 0 93 | 10 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 0 0 0.5 0 -0 0 106 | 107 | 108 | 7.32 0.2 1 109 | 110 | 111 | 112 | 116 | 117 | 118 | 119 | 0 120 | 0 121 | 122 | 1.73 -0.68 0 0 -0 0 123 | 0 124 | 0 125 | 1 126 | 127 | 128 | 129 | 130 | 131 | 7.46839 0.2 1 132 | 133 | 134 | 0 0 0.5 0 -0 0 135 | 10 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 0 0 0.5 0 -0 0 148 | 149 | 150 | 7.46839 0.2 1 151 | 152 | 153 | 154 | 158 | 159 | 160 | 161 | 0 162 | 0 163 | 164 | 5.29 -4.3142 0 0 0 -1.5708 165 | 0 166 | 0 167 | 1 168 | 169 | 170 | 171 | 172 | 173 | 7.30049 0.2 1 174 | 175 | 176 | 0 0 0.5 0 -0 0 177 | 10 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 0 0 0.5 0 -0 0 190 | 191 | 192 | 7.30049 0.2 1 193 | 194 | 195 | 196 | 200 | 201 | 202 | 203 | 0 204 | 0 205 | 206 | 1.74 -7.9482 0 0 -0 3.14154 207 | 0 208 | 0 209 | 1 210 | 211 | 212 | 213 | 214 | 215 | 7.43591 0.2 1 216 | 217 | 218 | 0 0 0.5 0 -0 0 219 | 10 220 | 221 | 222 | 223 | 224 | 225 | 226 | 227 | 228 | 229 | 230 | 231 | 0 0 0.5 0 -0 0 232 | 233 | 234 | 7.43591 0.2 1 235 | 236 | 237 | 238 | 242 | 243 | 244 | 245 | 0 246 | 0 247 | 248 | -1.81 -4.33 0 0 -0 1.5708 249 | 0 250 | 0 251 | 1 252 | 253 | 1 254 | 1 2 0 0 -0 0 255 | 256 | 257 | -1.54318 1.53246 0.5 0 -0 0 258 | 259 | 260 | 1 261 | 262 | 1 263 | 0 264 | 0 265 | 1 266 | 0 267 | 1 268 | 269 | 270 | 271 | 272 | 273 | 0.47498 0.475909 0.410949 274 | 275 | 276 | 10 277 | 278 | 279 | 280 | 281 | 282 | 283 | 284 | 285 | 286 | 287 | 288 | 289 | 290 | 1 1 1 291 | 292 | 293 | 294 | 298 | 299 | 300 | 301 | 0 302 | 0 303 | 304 | 0 305 | 0 306 | 1 307 | 308 | 0 309 | 310 | 311 | 2 -2 0.5 0 -0 0 312 | 313 | 314 | 1 315 | 316 | 1 317 | 0 318 | 0 319 | 1 320 | 0 321 | 1 322 | 323 | 324 | 325 | 326 | 327 | 0.64314 0.57215 0.4641 328 | 329 | 330 | 10 331 | 332 | 333 | 334 | 335 | 336 | 337 | 338 | 339 | 340 | 341 | 342 | 343 | 344 | 1 1 1 345 | 346 | 347 | 348 | 352 | 353 | 354 | 355 | 0 356 | 0 357 | 358 | 0 359 | 0 360 | 1 361 | 362 | 0 363 | 364 | 365 | 220 966000000 366 | 221 622821638 367 | 1471337482 157527144 368 | 369 | 0 0 0 0 -0 0 370 | 371 | 0 0 0 0 -0 0 372 | 0 0 0 0 -0 0 373 | 0 0 0 0 -0 0 374 | 0 0 0 0 -0 0 375 | 376 | 377 | 378 | -1.52886 3.95841 0 0 -0 0 379 | 380 | 0.201137 3.27841 0 0 -0 0 381 | 0 0 0 0 -0 0 382 | 0 0 0 0 -0 0 383 | 0 0 0 0 -0 0 384 | 385 | 386 | 3.76114 -0.355786 0 0 0 -1.5708 387 | 0 0 0 0 -0 0 388 | 0 0 0 0 -0 0 389 | 0 0 0 0 -0 0 390 | 391 | 392 | 0.211137 -3.98979 0 0 -0 3.14154 393 | 0 0 0 0 -0 0 394 | 0 0 0 0 -0 0 395 | 0 0 0 0 -0 0 396 | 397 | 398 | -3.33886 -0.371586 0 0 -0 1.5708 399 | 0 0 0 0 -0 0 400 | 0 0 0 0 -0 0 401 | 0 0 0 0 -0 0 402 | 403 | 404 | 405 | -1.34523 1.48192 0.205475 8.07794e-28 1e-06 2e-06 406 | 407 | -1.34523 1.48192 0.205475 8.07794e-28 1e-06 2e-06 408 | 0 0 0 0 -0 0 409 | 0 0 0 0 -0 0 410 | 0 0 0 0 -0 0 411 | 412 | 413 | 414 | 2 -2 0.23205 0 -0 0 415 | 416 | 2 -2 0.23205 0 -0 0 417 | 0 0 0 0 -0 0 418 | 0 0 0 0 -0 0 419 | 0 0 0 0 -0 0 420 | 421 | 422 | 423 | 424 | 425 | 6.61911 -3.99852 4.01441 0.001214 0.643031 2.55451 426 | orbit 427 | 428 | 429 | 430 | 431 | -------------------------------------------------------------------------------- /mybot_description/urdf/mybot.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 16 | 17 | 20 | 22 | 29 | 30 | 31 | 34 | 35 | 37 | 38 | 40 | 42 | 43 | 44 | 45 | 48 | 49 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 61 | 63 | 65 | 66 | 67 | 68 | 69 | 71 | 72 | 75 | 77 | 84 | 85 | 86 | 89 | 90 | 92 | 93 | 95 | 97 | 98 | 99 | 100 | 103 | 104 | 106 | 107 | 108 | 109 | 112 | 115 | 117 | 119 | 121 | 122 | 124 | 125 | 128 | 130 | 137 | 138 | 139 | 142 | 143 | 145 | 146 | 148 | 150 | 151 | 152 | 153 | 156 | 157 | 159 | 160 | 161 | 162 | 165 | 168 | 170 | 172 | 174 | 175 | 177 | 178 | 181 | 183 | 190 | 191 | 192 | 195 | 196 | 198 | 199 | 201 | 203 | 204 | 205 | 206 | 209 | 210 | 212 | 213 | 214 | 215 | 218 | 221 | 223 | 225 | 227 | 228 | 230 | 231 | 234 | 236 | 243 | 244 | 245 | 248 | 249 | 251 | 252 | 254 | 256 | 257 | 258 | 259 | 262 | 263 | 265 | 266 | 267 | 268 | 271 | 274 | 276 | 278 | 280 | 281 | 283 | 284 | 287 | 289 | 296 | 297 | 298 | 301 | 302 | 304 | 305 | 307 | 309 | 310 | 311 | 312 | 315 | 316 | 318 | 319 | 320 | 321 | 324 | 327 | 329 | 331 | 333 | 334 | 336 | 337 | 340 | 342 | 349 | 350 | 351 | 354 | 355 | 357 | 358 | 360 | 362 | 363 | 364 | 365 | 368 | 369 | 371 | 372 | 373 | 374 | 377 | 380 | 382 | 384 | 386 | 387 | 389 | 390 | 393 | 395 | 402 | 403 | 404 | 407 | 408 | 410 | 411 | 413 | 415 | 416 | 417 | 418 | 421 | 422 | 424 | 425 | 426 | 427 | 430 | 433 | 435 | 437 | 439 | 440 | 442 | 443 | 446 | 448 | 455 | 456 | 457 | 460 | 461 | 463 | 464 | 466 | 468 | 469 | 470 | 471 | 474 | 475 | 477 | 478 | 479 | 480 | 483 | 486 | 488 | 490 | 492 | 493 | 494 | 495 | 496 | 497 | 498 | 499 | 500 | 501 | 502 | 503 | 504 | 505 | 506 | 507 | 508 | 509 | 510 | 511 | 512 | 513 | 514 | 515 | 516 | 517 | 518 | 519 | 520 | 521 | 522 | 523 | 524 | 525 | 526 | 527 | 528 | 529 | 530 | 531 | 532 | 533 | 534 | 535 | 536 | 537 | 538 | 539 | 540 | 541 | 542 | 543 | 544 | 545 | 546 | -------------------------------------------------------------------------------- /mybot_gazebo/worlds/turtlebot_playground.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 1 5 | 0 0 10 0 -0 0 6 | 0.8 0.8 0.8 1 7 | 0.2 0.2 0.2 1 8 | 9 | 1000 10 | 0.9 11 | 0.01 12 | 0.001 13 | 14 | 0.5 0.1 -0.9 15 | 16 | 17 | 0.4 0.4 0.4 1 18 | 0.7 0.7 0.7 1 19 | 1 20 | 21 | 22 | 0.01 23 | 1 24 | 100 25 | 0 0 -9.8 26 | 27 | 28 | 1 29 | 30 | 31 | 1 32 | 33 | 34 | 0 0.005 0.6 0 -0 0 35 | 36 | 37 | 0.9 0.01 1.2 38 | 39 | 40 | 10 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 0 0.005 0.6 0 -0 0 53 | 54 | 55 | 0.9 0.01 1.2 56 | 57 | 58 | 59 | 63 | 64 | 65 | 66 | 0.45 -0.195 0.6 0 -0 0 67 | 68 | 69 | 0.02 0.4 1.2 70 | 71 | 72 | 10 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 0.45 -0.195 0.6 0 -0 0 85 | 86 | 87 | 0.02 0.4 1.2 88 | 89 | 90 | 91 | 95 | 96 | 97 | 98 | -0.45 -0.195 0.6 0 -0 0 99 | 100 | 101 | 0.02 0.4 1.2 102 | 103 | 104 | 10 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | -0.45 -0.195 0.6 0 -0 0 117 | 118 | 119 | 0.02 0.4 1.2 120 | 121 | 122 | 123 | 127 | 128 | 129 | 130 | 0 -0.195 0.03 0 -0 0 131 | 132 | 133 | 0.88 0.4 0.06 134 | 135 | 136 | 10 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 0 -0.195 0.03 0 -0 0 149 | 150 | 151 | 0.88 0.4 0.06 152 | 153 | 154 | 155 | 159 | 160 | 161 | 162 | 0 -0.195 1.19 0 -0 0 163 | 164 | 165 | 0.88 0.4 0.02 166 | 167 | 168 | 10 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 0 -0.195 1.19 0 -0 0 181 | 182 | 183 | 0.88 0.4 0.02 184 | 185 | 186 | 187 | 191 | 192 | 193 | 194 | 0 -0.195 0.43 0 -0 0 195 | 196 | 197 | 0.88 0.4 0.02 198 | 199 | 200 | 10 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | 209 | 210 | 211 | 212 | 0 -0.195 0.43 0 -0 0 213 | 214 | 215 | 0.88 0.4 0.02 216 | 217 | 218 | 219 | 223 | 224 | 225 | 226 | 0 -0.195 0.8 0 -0 0 227 | 228 | 229 | 0.88 0.4 0.02 230 | 231 | 232 | 10 233 | 234 | 235 | 236 | 237 | 238 | 239 | 240 | 241 | 242 | 243 | 244 | 0 -0.195 0.8 0 -0 0 245 | 246 | 247 | 0.88 0.4 0.02 248 | 249 | 250 | 251 | 255 | 256 | 257 | 258 | 0 259 | 0 260 | 261 | 0 262 | 0 263 | 1 264 | 265 | 0 1.53026 0 0 -0 0 266 | 267 | 268 | 1 269 | 270 | 271 | 272 | 273 | model://jersey_barrier/meshes/jersey_barrier.dae 274 | 275 | 276 | 277 | 278 | 0 0 0.5715 0 -0 0 279 | 280 | 281 | 4.06542 0.3063 1.143 282 | 283 | 284 | 10 285 | 286 | 287 | 288 | 289 | 290 | 291 | 292 | 293 | 294 | 295 | 296 | 0 0 0.032258 0 -0 0 297 | 298 | 299 | 4.06542 0.8107 0.064516 300 | 301 | 302 | 10 303 | 304 | 305 | 306 | 307 | 308 | 309 | 310 | 311 | 312 | 313 | 314 | 0 0 0.1 0 -0 0 315 | 316 | 317 | 4.06542 0.65 0.1 318 | 319 | 320 | 10 321 | 322 | 323 | 324 | 325 | 326 | 327 | 328 | 329 | 330 | 331 | 332 | 0 0 0.2 0 -0 0 333 | 334 | 335 | 4.06542 0.5 0.1 336 | 337 | 338 | 10 339 | 340 | 341 | 342 | 343 | 344 | 345 | 346 | 347 | 348 | 349 | 350 | 0 -0.224 0.2401 0.9 -0 0 351 | 352 | 353 | 4.06542 0.5 0.064516 354 | 355 | 356 | 10 357 | 358 | 359 | 360 | 361 | 362 | 363 | 364 | 365 | 366 | 367 | 368 | 0 0.224 0.2401 -0.9 0 0 369 | 370 | 371 | 4.06542 0.5 0.064516 372 | 373 | 374 | 10 375 | 376 | 377 | 378 | 379 | 380 | 381 | 382 | 383 | 384 | 385 | 386 | 0 387 | 0 388 | 389 | 0 390 | 0 391 | 1 392 | 393 | -4 -1 0 0 -0 -0.7 394 | 395 | 396 | 1 397 | 398 | 399 | 400 | 401 | 0 0 1 402 | 100 100 403 | 404 | 405 | 406 | 407 | 408 | 100 409 | 50 410 | 411 | 412 | 413 | 414 | 415 | 416 | 417 | 10 418 | 419 | 420 | 0 421 | 422 | 423 | 0 0 1 424 | 100 100 425 | 426 | 427 | 428 | 432 | 433 | 434 | 435 | 0 436 | 0 437 | 438 | 0 439 | 0 440 | 1 441 | 442 | 0.497681 0 0 0 -0 0 443 | 444 | 445 | 0 0 446 | 0 44986 447 | 1377677575 940727583 448 | 449 | 1 -3.44458 0 0 -0 0 450 | 451 | 1 -3.44458 0 0 -0 0 452 | 0 0 0 0 -0 0 453 | 0 0 0 0 -0 0 454 | 0 0 0 0 -0 0 455 | 456 | 457 | 458 | 1.41131 -1 0 0 -0 0.9 459 | 460 | 1.41131 -1 0.5 0 -0 0.9 461 | 0 0 0 0 -0 0 462 | 0 0 0 0 -0 0 463 | 0 0 0 0 -0 0 464 | 465 | 466 | 467 | -2 -3.4888 0.5 0 -0 0 468 | 469 | -2 -3.4888 0.5 0 -0 0 470 | 0 0 0 0 -0 0 471 | 0 0 0 0 -0 0 472 | 0 0 0 0 -0 0 473 | 474 | 475 | 476 | 477 | 478 | 1.33336 -0.422442 27.6101 3e-06 1.5698 3.04015 479 | orbit 480 | 481 | 482 | 483 | -2 -3.4888 0.5 0 -0 0 484 | 485 | 486 | 1 487 | 488 | 1 489 | 0 490 | 0 491 | 1 492 | 0 493 | 1 494 | 495 | 496 | 497 | 498 | 499 | 0.5 500 | 1 501 | 502 | 503 | 10 504 | 505 | 506 | 507 | 508 | 509 | 510 | 511 | 512 | 513 | 514 | 515 | 516 | 517 | 0.5 518 | 1 519 | 520 | 521 | 522 | 526 | 527 | 528 | 529 | 0 530 | 0 531 | 532 | 0 533 | 0 534 | 1 535 | 536 | 0 537 | 538 | 539 | 540 | 541 | 542 | 543 | model://dumpster/meshes/dumpster.dae 544 | 545 | 546 | 10 547 | 548 | 549 | 550 | 551 | 552 | 553 | 554 | 555 | 556 | 557 | 558 | 559 | 560 | model://dumpster/meshes/dumpster.dae 561 | 562 | 563 | 564 | 569 | 570 | 571 | 572 | 0 573 | 0 574 | 575 | 0 576 | 577 | 578 | 1 579 | 0 580 | 0 581 | 1 582 | 0 583 | 1 584 | 585 | 1 586 | 587 | 0 588 | 1 589 | 590 | 1 -3.44458 0 0 -0 -0.3 591 | 0 592 | 593 | 594 | 595 | 0 0 0.5 0 -0 0 596 | 597 | 598 | 599 | model://cube_20k/meshes/cube_20k.stl 600 | 0.5 0.5 0.5 601 | 602 | 603 | 10 604 | 605 | 606 | 607 | 608 | 609 | 610 | 611 | 612 | 613 | 614 | 615 | 616 | 617 | model://cube_20k/meshes/cube_20k.stl 618 | 0.5 0.5 0.5 619 | 620 | 621 | 622 | 623 | 0 624 | 0 625 | 626 | 0 627 | 628 | 629 | 1 630 | 0 631 | 0 632 | 1 633 | 0 634 | 1 635 | 636 | 1 637 | 638 | 0 639 | 1 640 | 641 | 1.41131 -1 0 0 -0 0.9 642 | 0 643 | 644 | 645 | 646 | -------------------------------------------------------------------------------- /mybot_description/urdf/final6.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 17 | 18 | 21 | 23 | 30 | 31 | 32 | 35 | 36 | 38 | 39 | 41 | 43 | 44 | 45 | 46 | 49 | 50 | 52 | 53 | 54 | 55 | 56 | 59 | 61 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 73 | 74 | 77 | 79 | 86 | 87 | 88 | 91 | 92 | 94 | 95 | 97 | 99 | 100 | 101 | 102 | 105 | 106 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 118 | 121 | 123 | 125 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 137 | 138 | 141 | 143 | 150 | 151 | 152 | 155 | 156 | 158 | 159 | 161 | 163 | 164 | 165 | 166 | 169 | 170 | 172 | 173 | 174 | 175 | 178 | 181 | 183 | 185 | 187 | 188 | 190 | 191 | 194 | 196 | 203 | 204 | 205 | 208 | 209 | 211 | 212 | 214 | 216 | 217 | 218 | 219 | 222 | 223 | 225 | 226 | 227 | 228 | 231 | 234 | 236 | 238 | 240 | 241 | 243 | 244 | 247 | 249 | 256 | 257 | 258 | 261 | 262 | 264 | 265 | 267 | 269 | 270 | 271 | 272 | 275 | 276 | 278 | 279 | 280 | 281 | 284 | 287 | 289 | 291 | 293 | 294 | 296 | 297 | 300 | 302 | 309 | 310 | 311 | 314 | 315 | 317 | 318 | 320 | 322 | 323 | 324 | 325 | 328 | 329 | 331 | 332 | 333 | 334 | 337 | 340 | 342 | 344 | 346 | 347 | 349 | 350 | 353 | 355 | 362 | 363 | 364 | 367 | 368 | 370 | 371 | 373 | 375 | 376 | 377 | 378 | 381 | 382 | 384 | 385 | 386 | 387 | 390 | 393 | 395 | 397 | 399 | 400 | 402 | 403 | 406 | 408 | 415 | 416 | 417 | 420 | 421 | 423 | 424 | 426 | 428 | 429 | 430 | 431 | 434 | 435 | 437 | 438 | 439 | 440 | 443 | 446 | 448 | 450 | 452 | 455 | 456 | 458 | 459 | 462 | 464 | 471 | 472 | 473 | 476 | 477 | 479 | 480 | 482 | 484 | 485 | 486 | 487 | 490 | 491 | 493 | 494 | 495 | 496 | 499 | 502 | 504 | 506 | 508 | 511 | 512 | 514 | 515 | 518 | 520 | 527 | 528 | 529 | 532 | 533 | 535 | 536 | 538 | 540 | 541 | 542 | 543 | 546 | 547 | 549 | 550 | 551 | 552 | 555 | 558 | 560 | 562 | 564 | 567 | 568 | 570 | 571 | 574 | 576 | 583 | 584 | 585 | 588 | 589 | 591 | 592 | 594 | 596 | 597 | 598 | 599 | 602 | 603 | 605 | 606 | 607 | 608 | 611 | 614 | 616 | 618 | 620 | 623 | 624 | 626 | 627 | 630 | 632 | 639 | 640 | 641 | 644 | 645 | 647 | 648 | 650 | 652 | 653 | 654 | 655 | 658 | 659 | 661 | 662 | 663 | 664 | 667 | 670 | 672 | 674 | 676 | 679 | 680 | 682 | 683 | 686 | 688 | 695 | 696 | 697 | 700 | 701 | 703 | 704 | 706 | 708 | 709 | 710 | 711 | 714 | 715 | 717 | 718 | 719 | 720 | 723 | 726 | 728 | 730 | 732 | 735 | 736 | 738 | 739 | 742 | 744 | 751 | 752 | 753 | 756 | 757 | 759 | 760 | 762 | 764 | 765 | 766 | 767 | 770 | 771 | 773 | 774 | 775 | 776 | 779 | 782 | 784 | 786 | 788 | 791 | 792 | 794 | 795 | 798 | 800 | 807 | 808 | 809 | 812 | 813 | 815 | 816 | 818 | 820 | 821 | 822 | 823 | 826 | 827 | 829 | 830 | 831 | 832 | 835 | 838 | 840 | 842 | 844 | 847 | 848 | 850 | 851 | 854 | 856 | 863 | 864 | 865 | 868 | 869 | 871 | 872 | 874 | 876 | 877 | 878 | 879 | 882 | 883 | 885 | 886 | 887 | 888 | 891 | 894 | 896 | 898 | 900 | 901 | 903 | 904 | 907 | 909 | 916 | 917 | 918 | 921 | 922 | 924 | 925 | 927 | 929 | 930 | 931 | 932 | 935 | 936 | 938 | 939 | 940 | 941 | 944 | 947 | 949 | 951 | 953 | 956 | 957 | 959 | 960 | 963 | 965 | 972 | 973 | 974 | 977 | 978 | 980 | 981 | 983 | 985 | 986 | 987 | 988 | 991 | 992 | 994 | 995 | 996 | 997 | 1000 | 1003 | 1005 | 1007 | 1009 | 1012 | 1013 | 1015 | 1016 | 1019 | 1021 | 1028 | 1029 | 1030 | 1033 | 1034 | 1036 | 1037 | 1039 | 1041 | 1042 | 1043 | 1044 | 1047 | 1048 | 1050 | 1051 | 1052 | 1053 | 1056 | 1059 | 1061 | 1063 | 1065 | 1068 | 1069 | 1071 | 1072 | 1075 | 1077 | 1084 | 1085 | 1086 | 1089 | 1090 | 1092 | 1093 | 1095 | 1097 | 1098 | 1099 | 1100 | 1103 | 1104 | 1106 | 1107 | 1108 | 1109 | 1112 | 1115 | 1117 | 1119 | 1121 | 1124 | 1125 | 1127 | 1128 | 1131 | 1133 | 1140 | 1141 | 1142 | 1145 | 1146 | 1148 | 1149 | 1151 | 1153 | 1154 | 1155 | 1156 | 1159 | 1160 | 1162 | 1163 | 1164 | 1165 | 1168 | 1171 | 1173 | 1175 | 1177 | 1180 | 1181 | 1183 | 1184 | 1187 | 1189 | 1196 | 1197 | 1198 | 1201 | 1202 | 1204 | 1205 | 1207 | 1209 | 1210 | 1211 | 1212 | 1215 | 1216 | 1218 | 1219 | 1220 | 1221 | 1224 | 1227 | 1229 | 1231 | 1233 | 1236 | 1237 | 1239 | 1240 | 1243 | 1245 | 1252 | 1253 | 1254 | 1257 | 1258 | 1260 | 1261 | 1263 | 1265 | 1266 | 1267 | 1268 | 1271 | 1272 | 1274 | 1275 | 1276 | 1277 | 1280 | 1283 | 1285 | 1287 | 1289 | 1292 | 1293 | 1295 | 1296 | 1299 | 1301 | 1308 | 1309 | 1310 | 1313 | 1314 | 1316 | 1317 | 1319 | 1321 | 1322 | 1323 | 1324 | 1327 | 1328 | 1330 | 1331 | 1332 | 1333 | 1336 | 1339 | 1341 | 1343 | 1345 | 1348 | 1349 | 1351 | 1352 | 1355 | 1357 | 1364 | 1365 | 1366 | 1369 | 1370 | 1372 | 1373 | 1375 | 1377 | 1378 | 1379 | 1380 | 1383 | 1384 | 1386 | 1387 | 1388 | 1389 | 1392 | 1395 | 1397 | 1399 | 1401 | 1402 | 1404 | 1405 | 1408 | 1410 | 1417 | 1418 | 1419 | 1422 | 1423 | 1425 | 1426 | 1428 | 1430 | 1431 | 1432 | 1433 | 1436 | 1437 | 1439 | 1440 | 1441 | 1442 | 1445 | 1448 | 1450 | 1452 | 1454 | 1457 | 1458 | 1460 | 1461 | 1464 | 1466 | 1473 | 1474 | 1475 | 1478 | 1479 | 1481 | 1482 | 1484 | 1486 | 1487 | 1488 | 1489 | 1492 | 1493 | 1495 | 1496 | 1497 | 1498 | 1501 | 1504 | 1506 | 1508 | 1510 | 1513 | 1514 | 1516 | 1517 | 1520 | 1522 | 1529 | 1530 | 1531 | 1534 | 1535 | 1537 | 1538 | 1540 | 1542 | 1543 | 1544 | 1545 | 1548 | 1549 | 1551 | 1552 | 1553 | 1554 | 1557 | 1560 | 1562 | 1564 | 1566 | 1569 | 1570 | 1572 | 1573 | 1576 | 1578 | 1585 | 1586 | 1587 | 1590 | 1591 | 1593 | 1594 | 1596 | 1598 | 1599 | 1600 | 1601 | 1604 | 1605 | 1607 | 1608 | 1609 | 1610 | 1613 | 1616 | 1618 | 1620 | 1622 | 1625 | 1626 | 1628 | 1629 | 1632 | 1634 | 1641 | 1642 | 1643 | 1646 | 1647 | 1649 | 1650 | 1652 | 1654 | 1655 | 1656 | 1657 | 1660 | 1661 | 1663 | 1664 | 1665 | 1666 | 1669 | 1672 | 1674 | 1676 | 1678 | 1681 | 1682 | 1684 | 1685 | 1688 | 1690 | 1697 | 1698 | 1699 | 1702 | 1703 | 1705 | 1706 | 1708 | 1710 | 1711 | 1712 | 1713 | 1716 | 1717 | 1719 | 1720 | 1721 | 1722 | 1725 | 1728 | 1730 | 1732 | 1734 | 1737 | 1738 | 1740 | 1741 | 1744 | 1746 | 1753 | 1754 | 1755 | 1758 | 1759 | 1761 | 1762 | 1764 | 1766 | 1767 | 1768 | 1769 | 1772 | 1773 | 1775 | 1776 | 1777 | 1778 | 1781 | 1784 | 1786 | 1788 | 1790 | 1793 | 1794 | 1796 | 1797 | 1800 | 1802 | 1809 | 1810 | 1811 | 1814 | 1815 | 1817 | 1818 | 1820 | 1822 | 1823 | 1824 | 1825 | 1828 | 1829 | 1831 | 1832 | 1833 | 1834 | 1837 | 1840 | 1842 | 1844 | 1846 | 1849 | 1850 | 1852 | 1853 | 1856 | 1858 | 1865 | 1866 | 1867 | 1870 | 1871 | 1873 | 1874 | 1876 | 1878 | 1879 | 1880 | 1881 | 1884 | 1885 | 1887 | 1888 | 1889 | 1890 | 1893 | 1896 | 1898 | 1900 | 1902 | 1903 | 1905 | 1906 | 1909 | 1911 | 1918 | 1919 | 1920 | 1923 | 1924 | 1926 | 1927 | 1929 | 1931 | 1932 | 1933 | 1934 | 1937 | 1938 | 1940 | 1941 | 1942 | 1943 | 1946 | 1949 | 1951 | 1953 | 1955 | 1958 | 1959 | 1961 | 1962 | 1965 | 1967 | 1974 | 1975 | 1976 | 1979 | 1980 | 1982 | 1983 | 1985 | 1987 | 1988 | 1989 | 1990 | 1993 | 1994 | 1996 | 1997 | 1998 | 1999 | 2002 | 2005 | 2007 | 2009 | 2011 | 2014 | 2015 | 2017 | 2018 | 2021 | 2023 | 2030 | 2031 | 2032 | 2035 | 2036 | 2038 | 2039 | 2041 | 2043 | 2044 | 2045 | 2046 | 2049 | 2050 | 2052 | 2053 | 2054 | 2055 | 2058 | 2061 | 2063 | 2065 | 2067 | 2070 | 2071 | 2073 | 2074 | 2077 | 2079 | 2086 | 2087 | 2088 | 2091 | 2092 | 2094 | 2095 | 2097 | 2099 | 2100 | 2101 | 2102 | 2105 | 2106 | 2108 | 2109 | 2110 | 2111 | 2114 | 2117 | 2119 | 2121 | 2123 | 2126 | 2127 | 2129 | 2130 | 2133 | 2135 | 2142 | 2143 | 2144 | 2147 | 2148 | 2150 | 2151 | 2153 | 2155 | 2156 | 2157 | 2158 | 2161 | 2162 | 2164 | 2165 | 2166 | 2167 | 2170 | 2173 | 2175 | 2177 | 2179 | 2182 | 2183 | 2185 | 2186 | 2189 | 2191 | 2198 | 2199 | 2200 | 2203 | 2204 | 2206 | 2207 | 2209 | 2211 | 2212 | 2213 | 2214 | 2217 | 2218 | 2220 | 2221 | 2222 | 2223 | 2226 | 2229 | 2231 | 2233 | 2235 | 2238 | 2239 | 2241 | 2242 | 2245 | 2247 | 2254 | 2255 | 2256 | 2259 | 2260 | 2262 | 2263 | 2265 | 2267 | 2268 | 2269 | 2270 | 2273 | 2274 | 2276 | 2277 | 2278 | 2279 | 2282 | 2285 | 2287 | 2289 | 2291 | 2294 | 2295 | 2297 | 2298 | 2301 | 2303 | 2310 | 2311 | 2312 | 2315 | 2316 | 2318 | 2319 | 2321 | 2323 | 2324 | 2325 | 2326 | 2329 | 2330 | 2332 | 2333 | 2334 | 2335 | 2338 | 2341 | 2343 | 2345 | 2347 | 2350 | 2351 | 2352 | 2386 | 2387 | 2388 | 2389 | --------------------------------------------------------------------------------