├── .gitattributes ├── Contributing.md ├── LICENSE.txt ├── README.md ├── chapter_1 └── README.md ├── chapter_2 ├── add_prjct_cmake │ ├── CMakeLists.txt │ ├── add.cpp │ ├── add.h │ └── main.cpp ├── add_prjct_make │ ├── Makefile │ ├── add.cpp │ ├── add.h │ └── main.cpp ├── class_inherit.cpp ├── class_struct.cpp ├── exception.cpp ├── hello_world.cpp ├── namespace.cpp ├── read_write_file.cpp └── sum.cpp ├── chapter_3 ├── prog1.py ├── prog2.py ├── prog3.py ├── prog4.py ├── prog5.py ├── prog6.py ├── prog7.py ├── prog8.py └── prog9.py ├── chapter_4 └── README.md ├── chapter_5 └── hello_world │ ├── CMakeLists.txt │ ├── launch │ ├── talker_listener.launch │ └── talker_listener_python.launch │ ├── package.xml │ ├── scripts │ ├── listener.py │ ├── move_distance.py │ ├── move_turtle.py │ ├── move_turtle_get_pose.py │ ├── move_turtlebot.py │ ├── talker.py │ └── turtle_service_param.py │ └── src │ ├── listener.cpp │ └── talker.cpp ├── chapter_6 ├── Arduino_Firmware │ └── final_code │ │ └── final_code.ino ├── mobile_robot_description │ ├── CMakeLists.txt │ ├── config │ │ ├── model.rviz │ │ └── robot.rviz │ ├── launch │ │ ├── view_model.launch │ │ └── view_robot.launch │ ├── meshes │ │ └── body │ │ │ └── chasis.dae │ ├── package.xml │ └── urdf │ │ └── robot_model.xacro └── mobile_robot_pkg │ ├── CMakeLists.txt │ ├── launch │ ├── keyboard_teleop.launch │ └── robot_standalone.launch │ ├── package.xml │ └── scripts │ ├── dead_reckoning.py │ ├── diff_tf.py │ ├── pid_velocity.py │ ├── robot_teleop_key │ └── twist_to_motors.py └── cover_edition_2.jpg /.gitattributes: -------------------------------------------------------------------------------- 1 | # Auto detect text files and perform LF normalization 2 | * text=auto 3 | -------------------------------------------------------------------------------- /Contributing.md: -------------------------------------------------------------------------------- 1 | # Contributing to Apress Source Code 2 | 3 | Copyright for Apress source code belongs to the author(s). However, under fair use you are encouraged to fork and contribute minor corrections and updates for the benefit of the author(s) and other readers. 4 | 5 | ## How to Contribute 6 | 7 | 1. Make sure you have a GitHub account. 8 | 2. Fork the repository for the relevant book. 9 | 3. Create a new branch on which to make your change, e.g. 10 | `git checkout -b my_code_contribution` 11 | 4. Commit your change. Include a commit message describing the correction. Please note that if your commit message is not clear, the correction will not be accepted. 12 | 5. Submit a pull request. 13 | 14 | Thank you for your contribution! -------------------------------------------------------------------------------- /LICENSE.txt: -------------------------------------------------------------------------------- 1 | Freeware License, some rights reserved 2 | 3 | Copyright (c) 2018 Lentin Joseph 4 | 5 | Permission is hereby granted, free of charge, to anyone obtaining a copy 6 | of this software and associated documentation files (the "Software"), 7 | to work with the Software within the limits of freeware distribution and fair use. 8 | This includes the rights to use, copy, and modify the Software for personal use. 9 | Users are also allowed and encouraged to submit corrections and modifications 10 | to the Software for the benefit of other users. 11 | 12 | It is not allowed to reuse, modify, or redistribute the Software for 13 | commercial use in any way, or for a user’s educational materials such as books 14 | or blog articles without prior permission from the copyright holder. 15 | 16 | The above copyright notice and this permission notice need to be included 17 | in all copies or substantial portions of the software. 18 | 19 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 20 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 21 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 22 | AUTHORS OR COPYRIGHT HOLDERS OR APRESS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 23 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 24 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 25 | SOFTWARE. 26 | 27 | 28 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Apress Source Code 2 | 3 | This repository accompanies [*Robot Operating System (ROS) for Absolute Beginners:Robotics Programming Made Easy, Second Edition*](https://link.springer.com/book/10.1007/978-1-4842-7750-8) by [Lentin Joseph](https://www.linkedin.com/in/lentinjoseph/) and [Aleena Johny](https://www.linkedin.com/in/aleena-johny-83a3161a5/) (Apress, 2022). 4 | 5 | [comment]: #cover 6 | ![Cover image1](cover_edition_2.jpg) 7 | 8 | Download the files as a zip using the green button, or clone the repository to your machine using Git. 9 | 10 | ## Releases 11 | 12 | Release v1.0 corresponds to the code in the published book, without corrections or updates. 13 | 14 | ## Contributions 15 | 16 | See the file Contributing.md for more information on how you can contribute to this repository. -------------------------------------------------------------------------------- /chapter_1/README.md: -------------------------------------------------------------------------------- 1 | No code for this chapter 2 | -------------------------------------------------------------------------------- /chapter_2/add_prjct_cmake/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0) 2 | 3 | set(CMAKE_BUILD_TYPE Release) 4 | 5 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14") 6 | 7 | project(main) 8 | 9 | add_executable( 10 | main 11 | add.cpp 12 | main.cpp 13 | ) 14 | 15 | -------------------------------------------------------------------------------- /chapter_2/add_prjct_cmake/add.cpp: -------------------------------------------------------------------------------- 1 | #include "add.h" 2 | 3 | int add::compute(int a, int b) 4 | { 5 | 6 | return(a+b); 7 | 8 | 9 | } 10 | -------------------------------------------------------------------------------- /chapter_2/add_prjct_cmake/add.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | 4 | class add 5 | { 6 | 7 | public: 8 | int compute(int no_1,int no_2); 9 | 10 | 11 | }; 12 | -------------------------------------------------------------------------------- /chapter_2/add_prjct_cmake/main.cpp: -------------------------------------------------------------------------------- 1 | #include "add.h" 2 | using namespace std; 3 | int main() 4 | { 5 | add obj; 6 | int result = obj.compute(43,34); 7 | cout<<"The Result:="< 2 | 3 | 4 | class add 5 | { 6 | 7 | public: 8 | int compute(int no_1,int no_2); 9 | 10 | 11 | }; 12 | -------------------------------------------------------------------------------- /chapter_2/add_prjct_make/main.cpp: -------------------------------------------------------------------------------- 1 | #include "add.h" 2 | using namespace std; 3 | int main() 4 | { 5 | add obj; 6 | int result = obj.compute(43,34); 7 | cout<<"The Result:="< 2 | #include 3 | using namespace std; 4 | 5 | class Robot_Class 6 | { 7 | public: 8 | int id; 9 | int no_wheels; 10 | string robot_name; 11 | void move_robot(); 12 | void stop_robot(); 13 | }; 14 | 15 | class Robot_Class_Derived: public Robot_Class 16 | { 17 | public: 18 | void turn_left(); 19 | void turn_right(); 20 | }; 21 | 22 | void Robot_Class::move_robot() 23 | { 24 | cout<<"Moving Robot"< 2 | 3 | using namespace std; 4 | 5 | int main() 6 | 7 | { 8 | 9 | try 10 | { 11 | 12 | 13 | int no_1 = 1; 14 | int no_2 = 0; 15 | 16 | if(no_2 == 0) 17 | { 18 | throw no_1; 19 | } 20 | 21 | } 22 | 23 | catch(int e) 24 | 25 | { 26 | 27 | cout<<"Exception found:"< 2 | 3 | using namespace std; 4 | 5 | int main() 6 | { 7 | 8 | int num_1 = 3; 9 | 10 | int num_2 = 4; 11 | 12 | 13 | int sum = num_1 + num_2; 14 | cout<<"Sum="< 2 | 3 | using namespace std; 4 | namespace robot { 5 | void process(void) 6 | { 7 | cout<<"Processing by Robot"< 2 | 3 | #include 4 | 5 | #include 6 | 7 | using namespace std; 8 | 9 | 10 | int main() 11 | 12 | { 13 | 14 | 15 | ofstream out_file; 16 | string data = "Robot_ID=0"; 17 | 18 | cout<<"Write data:"<> data; 29 | 30 | cout<<"Read data:"< 2 | 3 | using namespace std; 4 | 5 | int main() 6 | { 7 | 8 | int num_1 = 3; 9 | 10 | int num_2 = 4; 11 | 12 | 13 | int sum = num_1 + num_2; 14 | cout<<"Sum="< ") 5 | 6 | if(robot_command == "move_left"): 7 | print ("Robot is moving Left") 8 | elif(robot_command == "move_right"): 9 | print ("Robot is moving right") 10 | elif(robot_command == "move_forward"): 11 | print ("Robot is moving forward") 12 | elif(robot_command == "move_backward"): 13 | print ("Robot is moving backward") 14 | else: 15 | print ("Invalid command") 16 | -------------------------------------------------------------------------------- /chapter_3/prog3.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | 4 | robot_x = 0.1 5 | robot_y = 0.1 6 | 7 | while (robot_x < 2 and robot_y < 2): 8 | robot_x += 0.1 9 | robot_y += 0.1 10 | 11 | print ("Current Position ",robot_x,robot_y) 12 | 13 | 14 | print ("Reached destination") 15 | -------------------------------------------------------------------------------- /chapter_3/prog4.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | 4 | robot_x = 0.1 5 | robot_y = 0.1 6 | 7 | 8 | for i in range(0,100): 9 | robot_x += 0.1 10 | robot_y += 0.1 11 | 12 | print ("Current Position ",robot_x,robot_y) 13 | 14 | if(robot_x > 2 and robot_y > 2): 15 | print ("Reached destination") 16 | break 17 | -------------------------------------------------------------------------------- /chapter_3/prog5.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | def forward(): 3 | print ("Robot moving forward") 4 | def backward(): 5 | print ("Robot moving backward") 6 | def left(): 7 | print ("Robot moving left") 8 | def right(): 9 | print ("Robot moving right") 10 | 11 | def main(): 12 | ''' 13 | This is the main function 14 | ''' 15 | robot_command = input("Enter the command:> ") 16 | if(robot_command == "move_left"): 17 | left() 18 | elif(robot_command == "move_right"): 19 | right() 20 | elif(robot_command == "move_forward"): 21 | forward() 22 | elif(robot_command == "move_backward"): 23 | backward() 24 | else: 25 | print ("Invalid command") 26 | if __name__ == "__main__": 27 | while True: 28 | main() 29 | -------------------------------------------------------------------------------- /chapter_3/prog6.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | def divide(a, b): 4 | try: 5 | result = a// b 6 | print("Your answer is :", result) 7 | except ZeroDivisionError: 8 | print("You are dividing by zero") 9 | 10 | # Look at the parameters and note the working of Program 11 | divide(3, 0) 12 | 13 | -------------------------------------------------------------------------------- /chapter_3/prog7.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | class Robot: 3 | def __init__(self): 4 | print ("Started robot") 5 | def move_forward(self,distance): 6 | print ("Robot moving forward: "+str(distance)+"m") 7 | def move_backward(self,distance): 8 | print ("Robot moving backward: "+str(distance)+"m") 9 | def move_left(self,distance): 10 | print ("Robot moving left: "+str(distance)+"m") 11 | def move_right(self,distance): 12 | print ("Robot moving right: "+str(distance)+"m") 13 | def __del__(self): 14 | print ("Robot stopped") 15 | def main(): 16 | obj = Robot() 17 | obj.move_forward(2) 18 | obj.move_backward(2) 19 | obj.move_left(2) 20 | obj.move_right(2) 21 | if __name__ == "__main__": 22 | 23 | main() 24 | -------------------------------------------------------------------------------- /chapter_3/prog8.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | 4 | text = input("Enter the text:> ") 5 | file_obj = open("test.txt","w+") 6 | file_obj.write(text) 7 | file_obj.close() 8 | 9 | file_obj = open("test.txt","r") 10 | text = file_obj.readline() 11 | print ("Read text: ",text) 12 | -------------------------------------------------------------------------------- /chapter_3/prog9.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | class Test: 4 | 5 | def __init__(self): 6 | print ("Object created") 7 | 8 | def execute(self,text): 9 | print ("Input text:> ",text) 10 | -------------------------------------------------------------------------------- /chapter_4/README.md: -------------------------------------------------------------------------------- 1 | No code for this chapter 2 | -------------------------------------------------------------------------------- /chapter_5/hello_world/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(hello_world) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | roscpp 6 | rospy 7 | std_msgs 8 | ) 9 | catkin_package() 10 | 11 | include_directories( 12 | ${catkin_INCLUDE_DIRS} 13 | ) 14 | 15 | 16 | 17 | add_executable(talker src/talker.cpp) 18 | target_link_libraries(talker 19 | ${catkin_LIBRARIES} 20 | ) 21 | 22 | add_executable(listener src/listener.cpp) 23 | target_link_libraries(listener 24 | ${catkin_LIBRARIES} 25 | ) 26 | 27 | -------------------------------------------------------------------------------- /chapter_5/hello_world/launch/talker_listener.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /chapter_5/hello_world/launch/talker_listener_python.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /chapter_5/hello_world/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | hello_world 4 | 0.0.0 5 | The hello_world package 6 | 7 | Lentin Joseph 8 | 9 | BSD 10 | 11 | catkin 12 | 13 | roscpp 14 | rospy 15 | std_msgs 16 | 17 | roscpp 18 | rospy 19 | std_msgs 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /chapter_5/hello_world/scripts/listener.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import rospy 3 | from std_msgs.msg import String 4 | 5 | def callback(data): 6 | rospy.loginfo(rospy.get_caller_id() + "I heard %s", data.data) 7 | 8 | def listener(): 9 | 10 | # In ROS, nodes are uniquely named. If two nodes with the same 11 | # name are launched, the previous one is kicked off. The 12 | # anonymous=True flag means that rospy will choose a unique 13 | # name for our 'listener' node so that multiple listeners can 14 | # run simultaneously. 15 | rospy.init_node('listener', anonymous=True) 16 | 17 | rospy.Subscriber("chatter", String, callback) 18 | 19 | # spin() simply keeps python from exiting until this node is stopped 20 | rospy.spin() 21 | 22 | if __name__ == '__main__': 23 | listener() 24 | 25 | -------------------------------------------------------------------------------- /chapter_5/hello_world/scripts/move_distance.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import rospy 4 | 5 | from geometry_msgs.msg import Twist 6 | 7 | from turtlesim.msg import Pose 8 | 9 | import sys 10 | 11 | robot_x = 0 12 | 13 | 14 | def pose_callback(pose): 15 | global robot_x 16 | 17 | rospy.loginfo("Robot X = %f\n",pose.x) 18 | 19 | robot_x = pose.x 20 | 21 | 22 | def move_turtle(lin_vel,ang_vel,distance): 23 | 24 | global robot_x 25 | 26 | rospy.init_node('move_turtle', anonymous=False) 27 | pub = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10) 28 | 29 | rospy.Subscriber('/turtle1/pose',Pose, pose_callback) 30 | 31 | rate = rospy.Rate(10) # 10hz 32 | 33 | vel = Twist() 34 | while not rospy.is_shutdown(): 35 | 36 | vel.linear.x = lin_vel 37 | vel.linear.y = 0 38 | vel.linear.z = 0 39 | 40 | vel.angular.x = 0 41 | vel.angular.y = 0 42 | vel.angular.z = ang_vel 43 | 44 | #rospy.loginfo("Linear Vel = %f: Angular Vel = %f",lin_vel,ang_vel) 45 | 46 | if(robot_x >= distance): 47 | rospy.loginfo("Robot Reached destination") 48 | rospy.logwarn("Stopping robot") 49 | 50 | break 51 | 52 | pub.publish(vel) 53 | 54 | rate.sleep() 55 | 56 | if __name__ == '__main__': 57 | try: 58 | move_turtle(float(sys.argv[1]),float(sys.argv[2]),float(sys.argv[3])) 59 | except rospy.ROSInterruptException: 60 | pass 61 | 62 | -------------------------------------------------------------------------------- /chapter_5/hello_world/scripts/move_turtle.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import rospy 4 | 5 | from geometry_msgs.msg import Twist 6 | 7 | import sys 8 | 9 | def move_turtle(lin_vel,ang_vel): 10 | 11 | rospy.init_node('move_turtle', anonymous=True) 12 | pub = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10) 13 | rate = rospy.Rate(10) # 10hz 14 | 15 | vel = Twist() 16 | while not rospy.is_shutdown(): 17 | 18 | vel.linear.x = lin_vel 19 | vel.linear.y = 0 20 | vel.linear.z = 0 21 | 22 | vel.angular.x = 0 23 | vel.angular.y = 0 24 | vel.angular.z = ang_vel 25 | 26 | 27 | 28 | rospy.loginfo("Linear Vel = %f: Angular Vel = %f",lin_vel,ang_vel) 29 | 30 | pub.publish(vel) 31 | 32 | rate.sleep() 33 | 34 | if __name__ == '__main__': 35 | try: 36 | move_turtle(float(sys.argv[1]),float(sys.argv[2])) 37 | except rospy.ROSInterruptException: 38 | pass 39 | -------------------------------------------------------------------------------- /chapter_5/hello_world/scripts/move_turtle_get_pose.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import rospy 4 | 5 | from geometry_msgs.msg import Twist 6 | from turtlesim.msg import Pose 7 | import sys 8 | 9 | robot_x = 0 10 | 11 | 12 | def pose_callback(pose): 13 | global robot_x 14 | rospy.loginfo("Robot X = %f\n",pose.x) 15 | robot_x = pose.x 16 | 17 | 18 | def move_turtle(lin_vel,ang_vel): 19 | 20 | global robot_x 21 | rospy.init_node('move_turtle', anonymous=False) 22 | pub = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10) 23 | rospy.Subscriber('/turtle1/pose',Pose, pose_callback) 24 | rate = rospy.Rate(10) # 10hz 25 | 26 | vel = Twist() 27 | while not rospy.is_shutdown(): 28 | 29 | vel.linear.x = lin_vel 30 | vel.linear.y = 0 31 | vel.linear.z = 0 32 | 33 | vel.angular.x = 0 34 | vel.angular.y = 0 35 | vel.angular.z = ang_vel 36 | 37 | rospy.loginfo("Linear Vel = %f: Angular Vel = %f",lin_vel,ang_vel) 38 | 39 | pub.publish(vel) 40 | 41 | rate.sleep() 42 | 43 | if __name__ == '__main__': 44 | try: 45 | move_turtle(float(sys.argv[1]),float(sys.argv[2])) 46 | except rospy.ROSInterruptException: 47 | pass 48 | 49 | -------------------------------------------------------------------------------- /chapter_5/hello_world/scripts/move_turtlebot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import rospy 3 | from geometry_msgs.msg import Twist 4 | from nav_msgs.msg import Odometry 5 | import sys 6 | robot_x = 0 7 | def pose_callback(msg): 8 | global robot_x 9 | #Reading x position from the Odometry message 10 | robot_x = msg.pose.pose.position.x 11 | rospy.loginfo("Robot X = %f\n",robot_x) 12 | def move_turtle(lin_vel,ang_vel,distance): 13 | global robot_x 14 | rospy.init_node('move_turtlebot', anonymous=False) 15 | #The Twist topic is /cmd_vel_muc/input/teleop 16 | pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10) 17 | #Position topic is /odom 18 | rospy.Subscriber('/odom',Odometry, pose_callback) 19 | rate = rospy.Rate(10) # 10hz 20 | vel = Twist() 21 | while not rospy.is_shutdown(): 22 | vel.linear.x = lin_vel 23 | vel.linear.y = 0 24 | vel.linear.z = 0 25 | vel.angular.x = 0 26 | vel.angular.y = 0 27 | vel.angular.z = ang_vel 28 | #rospy.loginfo("Linear Vel = %f: Angular Vel = %f",lin_vel,ang_vel) 29 | if(robot_x >= distance): 30 | rospy.loginfo("Robot Reached destination") 31 | rospy.logwarn("Stopping robot") 32 | vel.linear.x = 0 33 | vel.linear.z = 0 34 | break 35 | pub.publish(vel) 36 | rate.sleep() 37 | if __name__ == '__main__': 38 | try: 39 | move_turtle(float(sys.argv[1]),float(sys.argv[2]),float(sys.argv[3])) 40 | except rospy.ROSInterruptException: 41 | pass 42 | 43 | -------------------------------------------------------------------------------- /chapter_5/hello_world/scripts/talker.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # license removed for brevity 3 | import rospy 4 | from std_msgs.msg import String 5 | 6 | def talker(): 7 | rospy.init_node('talker', anonymous=True) 8 | pub = rospy.Publisher('chatter', String, queue_size=10) 9 | rate = rospy.Rate(10) # 10hz 10 | while not rospy.is_shutdown(): 11 | hello_str = "hello world %s" % rospy.get_time() 12 | rospy.loginfo(hello_str) 13 | pub.publish(hello_str) 14 | rate.sleep() 15 | 16 | if __name__ == '__main__': 17 | try: 18 | talker() 19 | except rospy.ROSInterruptException: 20 | pass 21 | 22 | -------------------------------------------------------------------------------- /chapter_5/hello_world/scripts/turtle_service_param.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import rospy 4 | 5 | import random 6 | 7 | from std_srvs.srv import Empty 8 | 9 | 10 | def change_color(): 11 | 12 | rospy.init_node('change_color', anonymous=False) 13 | rospy.set_param('/turtlesim/background_b',random.randint(0,255)) 14 | rospy.set_param('/turtlesim/background_g',random.randint(0,255)) 15 | rospy.set_param('/turtlesim/background_r',random.randint(0,255)) 16 | 17 | rospy.wait_for_service('/reset') 18 | 19 | try: 20 | 21 | serv = rospy.ServiceProxy('/reset',Empty) 22 | resp = serv() 23 | rospy.loginfo("Executed service") 24 | 25 | except rospy.ServiceException as e: 26 | rospy.loginfo("Service call failed: %s" %e) 27 | 28 | 29 | rospy.spin() 30 | 31 | if __name__ == '__main__': 32 | try: 33 | change_color() 34 | except rospy.ROSInterruptException: 35 | pass 36 | 37 | -------------------------------------------------------------------------------- /chapter_5/hello_world/src/listener.cpp: -------------------------------------------------------------------------------- 1 | #include "ros/ros.h" 2 | #include "std_msgs/String.h" 3 | 4 | void chatterCallback(const std_msgs::String::ConstPtr& msg) 5 | { 6 | 7 | ROS_INFO("I heard: [%s]", msg->data.c_str()); 8 | 9 | } 10 | 11 | int main(int argc, char **argv) 12 | { 13 | 14 | ros::init(argc, argv, "listener"); 15 | 16 | ros::NodeHandle n; 17 | 18 | ros::Subscriber sub = n.subscribe("chatter", 1000, chatterCallback); 19 | 20 | ros::spin(); 21 | 22 | return 0; 23 | } 24 | 25 | -------------------------------------------------------------------------------- /chapter_5/hello_world/src/talker.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "ros/ros.h" 3 | #include "std_msgs/String.h" 4 | #include 5 | 6 | 7 | int main(int argc, char **argv) 8 | { 9 | ros::init(argc, argv, "talker"); 10 | 11 | ros::NodeHandle n; 12 | 13 | ros::Publisher chatter_pub = n.advertise("chatter", 1000); 14 | 15 | ros::Rate loop_rate(10); 16 | 17 | int count = 0; 18 | 19 | while (ros::ok()) 20 | { 21 | std_msgs::String msg; 22 | 23 | std::stringstream ss; 24 | 25 | ss << "hello world " << count; 26 | 27 | msg.data = ss.str(); 28 | 29 | ROS_INFO("%s", msg.data.c_str()); 30 | 31 | chatter_pub.publish(msg); 32 | 33 | ros::spinOnce(); 34 | 35 | loop_rate.sleep(); 36 | 37 | ++count; 38 | } 39 | 40 | 41 | return 0; 42 | } 43 | 44 | 45 | -------------------------------------------------------------------------------- /chapter_6/Arduino_Firmware/final_code/final_code.ino: -------------------------------------------------------------------------------- 1 | 2 | //Name: robot_firmware 3 | 4 | //Author: Lentin Joseph 5 | 6 | //ROS Arduino code publishing sensor data and subscribing to motor commands 7 | 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | 15 | 16 | //////////////////////////////////////////////////////////////////////////////// 17 | //Motor, Encoder and IR sensor pin definition 18 | 19 | int encoder_pinA = 2; 20 | int encoder_pinB = 3; 21 | 22 | volatile int pulses1 = 0; 23 | volatile int pulses2 = 0; 24 | 25 | #define IR_PIN A0 26 | 27 | //Motor A 28 | int enableA = 5; 29 | int MotorA1 = 6; 30 | int MotorA2 = 7; 31 | 32 | //Motor B 33 | int enableB = 8; 34 | int MotorB1 = 9; 35 | int MotorB2 = 10; 36 | 37 | ///////////////////////////////////////////////////////////////////////////////////// 38 | 39 | //ROS Node handle 40 | ros::NodeHandle nh; 41 | //Left and right speed 42 | int r_speed = 0, l_speed = 0; 43 | 44 | //Direction flag for encoder 45 | int left_direction = 1; 46 | int right_direction = 1; 47 | 48 | 49 | ///////////////////////////////////////////////////////////////////////////////////////////////////////// 50 | 51 | void left_speed_cb(const std_msgs::Int32& msg) // cmd_vel callback function definition 52 | { 53 | 54 | digitalWrite(LED_BUILTIN, HIGH-digitalRead(LED_BUILTIN)); // blink the led 55 | l_speed = msg.data; 56 | 57 | } 58 | 59 | 60 | void right_speed_cb(const std_msgs::Int32& msg) // cmd_vel callback function definition 61 | { 62 | digitalWrite(LED_BUILTIN, HIGH-digitalRead(LED_BUILTIN)); // blink the led 63 | r_speed = msg.data; 64 | 65 | } 66 | 67 | void reset_cb(const std_msgs::Bool& msg) 68 | { 69 | 70 | l_speed = 0; 71 | r_speed = 0; 72 | 73 | pulses1 = 0; 74 | pulses2 = 0; 75 | 76 | 77 | } 78 | 79 | //////////////////////////////////////////////////////////////////////////////////////////////// 80 | //Mapping function one range to another range 81 | 82 | float mapFloat(float value, float fromLow, float fromHigh, float toLow, float toHigh) { 83 | return (value - fromLow) * (toHigh - toLow) / (fromHigh - fromLow) + toLow; 84 | } 85 | 86 | 87 | //////////////////////////////////////////////////////////////////////////////////////////////// 88 | //Publisher for Left and Right encoder 89 | 90 | std_msgs::Int32 l_encoder_msg; 91 | ros::Publisher l_enc_pub("left_ticks", &l_encoder_msg); 92 | 93 | std_msgs::Int32 r_encoder_msg; 94 | ros::Publisher r_enc_pub("right_ticks", &r_encoder_msg); 95 | 96 | //Sharp distance publisher 97 | 98 | std_msgs::Float32 sharp_msg; 99 | ros::Publisher sharp_distance_pub("obstacle_distance", &sharp_msg); 100 | 101 | //Subscribers for left and right speed 102 | 103 | ros::Subscriber left_speed_sub("set_left_speed",&left_speed_cb); // creation of subscriber object sub for recieving the cmd_vel 104 | ros::Subscriber right_speed_sub("set_right_speed",&right_speed_cb); // creation of subscriber object sub for recieving the cmd_vel 105 | ros::Subscriber reset_sub("reset",&reset_cb); // creation of subscriber object sub for recieving the cmd_vel 106 | 107 | 108 | 109 | //////////////////////////////////////////////////////////////////////////////////////////////// 110 | 111 | 112 | // The value will quickly become too large for an int to store 113 | unsigned long previousMillis = 0; 114 | 115 | // Loop frequency: 100 ms 116 | const long interval = 50; 117 | 118 | 119 | ////////////////////////////////////////////////////////////////////////////////////////////////////// 120 | //ISR for two encoders 121 | 122 | void counter1(){ 123 | 124 | pulses1 = pulses1 + left_direction; 125 | 126 | } 127 | 128 | void counter2(){ 129 | 130 | pulses2 = pulses2 + right_direction; 131 | 132 | } 133 | 134 | ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 135 | //Setting encoder pins as interrupts 136 | 137 | void setup_wheelencoder() 138 | { 139 | 140 | pinMode(encoder_pinA, INPUT); 141 | attachInterrupt(digitalPinToInterrupt (encoder_pinA), counter1, RISING); 142 | pulses1 = 0; 143 | pinMode(encoder_pinB, INPUT); 144 | attachInterrupt(digitalPinToInterrupt (encoder_pinB), counter2, RISING); 145 | pulses2 = 0; 146 | 147 | } 148 | 149 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 150 | //Read IR values and publish 151 | 152 | void update_IR() 153 | { 154 | 155 | float volts = analogRead(IR_PIN)*0.0048828125; // value from sensor * (5/1024) 156 | float distance = 13*pow(volts, -1); // worked out from datasheet graph 157 | 158 | sharp_msg.data = distance; 159 | sharp_distance_pub.publish(&sharp_msg); 160 | 161 | } 162 | 163 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 164 | 165 | void update_Motor() 166 | { 167 | 168 | //If left speed is greater than zero 169 | if(l_speed >= 0) 170 | { 171 | digitalWrite (MotorA1, LOW); 172 | digitalWrite (MotorA2, HIGH); 173 | 174 | analogWrite(enableA,abs(l_speed)); 175 | 176 | left_direction = 1; 177 | 178 | } 179 | else 180 | { 181 | 182 | digitalWrite (MotorA1, HIGH); 183 | digitalWrite (MotorA2, LOW); 184 | 185 | analogWrite(enableA,abs(l_speed)); 186 | 187 | left_direction = -1; 188 | 189 | 190 | } 191 | 192 | if(r_speed >= 0) 193 | { 194 | 195 | digitalWrite (MotorB1, HIGH); 196 | digitalWrite (MotorB2, LOW); 197 | analogWrite(enableB,abs(r_speed)); 198 | 199 | right_direction = 1; 200 | 201 | 202 | } 203 | else 204 | { 205 | 206 | digitalWrite (MotorB1, LOW); 207 | digitalWrite (MotorB2, HIGH); 208 | analogWrite(enableB,abs(r_speed)); 209 | 210 | right_direction = -1; 211 | 212 | 213 | } 214 | 215 | 216 | 217 | } 218 | 219 | void setup() 220 | { 221 | //Setting Serial1 and bluetooth as default serial port for communication via Bluetooth 222 | 223 | nh.getHardware()->setPort(&Serial1); 224 | nh.getHardware()->setBaud(9600); 225 | 226 | pinMode (enableA, OUTPUT); 227 | pinMode (MotorA1, OUTPUT); 228 | pinMode (MotorA2, OUTPUT); 229 | 230 | pinMode (enableB, OUTPUT); 231 | pinMode (MotorB1, OUTPUT); 232 | pinMode (MotorB2, OUTPUT); 233 | 234 | pinMode(LED_BUILTIN, OUTPUT); 235 | 236 | //Setup wheel encoders 237 | setup_wheelencoder(); 238 | 239 | //Initialize ROS node 240 | nh.initNode(); 241 | 242 | //Setup publisher 243 | nh.advertise(l_enc_pub); 244 | nh.advertise(r_enc_pub); 245 | nh.advertise(sharp_distance_pub); 246 | 247 | //Setup subscriber 248 | nh.subscribe(left_speed_sub); 249 | nh.subscribe(right_speed_sub); 250 | nh.subscribe(reset_sub); 251 | 252 | } 253 | 254 | void loop() 255 | { 256 | 257 | unsigned long currentMillis = millis(); 258 | 259 | if (currentMillis - previousMillis >= interval) 260 | { 261 | previousMillis = currentMillis; 262 | 263 | l_encoder_msg.data = pulses1; 264 | r_encoder_msg.data = pulses2; 265 | 266 | l_enc_pub.publish(&l_encoder_msg); 267 | r_enc_pub.publish(&r_encoder_msg); 268 | 269 | update_IR(); 270 | 271 | } 272 | 273 | update_Motor(); 274 | nh.spinOnce(); 275 | 276 | delay(20); 277 | } 278 | -------------------------------------------------------------------------------- /chapter_6/mobile_robot_description/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(mobile_robot_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 COMPONENTS 8 | urdf 9 | xacro 10 | ) 11 | 12 | ## System dependencies are found with CMake's conventions 13 | # find_package(Boost REQUIRED COMPONENTS system) 14 | 15 | 16 | ## Uncomment this if the package has a setup.py. This macro ensures 17 | ## modules and global scripts declared therein get installed 18 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 19 | # catkin_python_setup() 20 | 21 | ################################################ 22 | ## Declare ROS messages, services and actions ## 23 | ################################################ 24 | 25 | ## To declare and build messages, services or actions from within this 26 | ## package, follow these steps: 27 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 28 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 29 | ## * In the file package.xml: 30 | ## * add a build_depend tag for "message_generation" 31 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 32 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 33 | ## but can be declared for certainty nonetheless: 34 | ## * add a run_depend tag for "message_runtime" 35 | ## * In this file (CMakeLists.txt): 36 | ## * add "message_generation" and every package in MSG_DEP_SET to 37 | ## find_package(catkin REQUIRED COMPONENTS ...) 38 | ## * add "message_runtime" and every package in MSG_DEP_SET to 39 | ## catkin_package(CATKIN_DEPENDS ...) 40 | ## * uncomment the add_*_files sections below as needed 41 | ## and list every .msg/.srv/.action file to be processed 42 | ## * uncomment the generate_messages entry below 43 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 44 | 45 | ## Generate messages in the 'msg' folder 46 | # add_message_files( 47 | # FILES 48 | # Message1.msg 49 | # Message2.msg 50 | # ) 51 | 52 | ## Generate services in the 'srv' folder 53 | # add_service_files( 54 | # FILES 55 | # Service1.srv 56 | # Service2.srv 57 | # ) 58 | 59 | ## Generate actions in the 'action' folder 60 | # add_action_files( 61 | # FILES 62 | # Action1.action 63 | # Action2.action 64 | # ) 65 | 66 | ## Generate added messages and services with any dependencies listed here 67 | # generate_messages( 68 | # DEPENDENCIES 69 | # std_msgs # Or other packages containing msgs 70 | # ) 71 | 72 | ################################################ 73 | ## Declare ROS dynamic reconfigure parameters ## 74 | ################################################ 75 | 76 | ## To declare and build dynamic reconfigure parameters within this 77 | ## package, follow these steps: 78 | ## * In the file package.xml: 79 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 80 | ## * In this file (CMakeLists.txt): 81 | ## * add "dynamic_reconfigure" to 82 | ## find_package(catkin REQUIRED COMPONENTS ...) 83 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 84 | ## and list every .cfg file to be processed 85 | 86 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 87 | # generate_dynamic_reconfigure_options( 88 | # cfg/DynReconf1.cfg 89 | # cfg/DynReconf2.cfg 90 | # ) 91 | 92 | ################################### 93 | ## catkin specific configuration ## 94 | ################################### 95 | ## The catkin_package macro generates cmake config files for your package 96 | ## Declare things to be passed to dependent projects 97 | ## INCLUDE_DIRS: uncomment this if you package contains header files 98 | ## LIBRARIES: libraries you create in this project that dependent projects also need 99 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 100 | ## DEPENDS: system dependencies of this project that dependent projects also need 101 | catkin_package( 102 | # INCLUDE_DIRS include 103 | # LIBRARIES mobile_robot_description 104 | # CATKIN_DEPENDS urdf xacro 105 | # DEPENDS system_lib 106 | ) 107 | 108 | ########### 109 | ## Build ## 110 | ########### 111 | 112 | ## Specify additional locations of header files 113 | ## Your package locations should be listed before other locations 114 | # include_directories(include) 115 | include_directories( 116 | ${catkin_INCLUDE_DIRS} 117 | ) 118 | 119 | ## Declare a C++ library 120 | # add_library(mobile_robot_description 121 | # src/${PROJECT_NAME}/mobile_robot_description.cpp 122 | # ) 123 | 124 | ## Add cmake target dependencies of the library 125 | ## as an example, code may need to be generated before libraries 126 | ## either from message generation or dynamic reconfigure 127 | # add_dependencies(mobile_robot_description ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 128 | 129 | ## Declare a C++ executable 130 | # add_executable(mobile_robot_description_node src/mobile_robot_description_node.cpp) 131 | 132 | ## Add cmake target dependencies of the executable 133 | ## same as for the library above 134 | # add_dependencies(mobile_robot_description_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 135 | 136 | ## Specify libraries to link a library or executable target against 137 | # target_link_libraries(mobile_robot_description_node 138 | # ${catkin_LIBRARIES} 139 | # ) 140 | 141 | ############# 142 | ## Install ## 143 | ############# 144 | 145 | # all install targets should use catkin DESTINATION variables 146 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 147 | 148 | ## Mark executable scripts (Python etc.) for installation 149 | ## in contrast to setup.py, you can choose the destination 150 | # install(PROGRAMS 151 | # scripts/my_python_script 152 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 153 | # ) 154 | 155 | ## Mark executables and/or libraries for installation 156 | # install(TARGETS mobile_robot_description mobile_robot_description_node 157 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 158 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 159 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 160 | # ) 161 | 162 | ## Mark cpp header files for installation 163 | # install(DIRECTORY include/${PROJECT_NAME}/ 164 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 165 | # FILES_MATCHING PATTERN "*.h" 166 | # PATTERN ".svn" EXCLUDE 167 | # ) 168 | 169 | ## Mark other files for installation (e.g. launch and bag files, etc.) 170 | # install(FILES 171 | # # myfile1 172 | # # myfile2 173 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 174 | # ) 175 | 176 | ############# 177 | ## Testing ## 178 | ############# 179 | 180 | ## Add gtest based cpp test target and link libraries 181 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_mobile_robot_description.cpp) 182 | # if(TARGET ${PROJECT_NAME}-test) 183 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 184 | # endif() 185 | 186 | ## Add folders to be run by python nosetests 187 | # catkin_add_nosetests(test) 188 | -------------------------------------------------------------------------------- /chapter_6/mobile_robot_description/config/model.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 | - /TF1 10 | - /TF1/Frames1 11 | - /RobotModel1 12 | - /RobotModel1/Links1 13 | - /Axes1 14 | - /Pose1 15 | Splitter Ratio: 0.5 16 | Tree Height: 651 17 | - Class: rviz/Selection 18 | Name: Selection 19 | - Class: rviz/Tool Properties 20 | Expanded: 21 | - /2D Pose Estimate1 22 | - /2D Nav Goal1 23 | - /Publish Point1 24 | Name: Tool Properties 25 | Splitter Ratio: 0.5886790156364441 26 | - Class: rviz/Views 27 | Expanded: 28 | - /Current View1 29 | Name: Views 30 | Splitter Ratio: 0.5 31 | - Class: rviz/Time 32 | Experimental: false 33 | Name: Time 34 | SyncMode: 0 35 | SyncSource: "" 36 | Preferences: 37 | PromptSaveOnExit: true 38 | Toolbars: 39 | toolButtonStyle: 2 40 | Visualization Manager: 41 | Class: "" 42 | Displays: 43 | - Class: rviz/TF 44 | Enabled: true 45 | Frame Timeout: 15 46 | Frames: 47 | All Enabled: false 48 | base_footprint: 49 | Value: false 50 | base_link: 51 | Value: false 52 | left_wheel_link: 53 | Value: false 54 | right_wheel_link: 55 | Value: false 56 | robot_caster_back_link: 57 | Value: false 58 | Marker Alpha: 1 59 | Marker Scale: 1 60 | Name: TF 61 | Show Arrows: true 62 | Show Axes: true 63 | Show Names: false 64 | Tree: 65 | base_footprint: 66 | base_link: 67 | left_wheel_link: 68 | {} 69 | right_wheel_link: 70 | {} 71 | robot_caster_back_link: 72 | {} 73 | Update Interval: 0 74 | Value: true 75 | - Alpha: 1 76 | Class: rviz/RobotModel 77 | Collision Enabled: false 78 | Enabled: true 79 | Links: 80 | All Links Enabled: true 81 | Expand Joint Details: false 82 | Expand Link Details: false 83 | Expand Tree: false 84 | Link Tree Style: "" 85 | base_footprint: 86 | Alpha: 1 87 | Show Axes: false 88 | Show Trail: false 89 | base_link: 90 | Alpha: 1 91 | Show Axes: false 92 | Show Trail: false 93 | Value: true 94 | left_wheel_link: 95 | Alpha: 1 96 | Show Axes: false 97 | Show Trail: false 98 | Value: true 99 | right_wheel_link: 100 | Alpha: 1 101 | Show Axes: false 102 | Show Trail: false 103 | Value: true 104 | robot_caster_back_link: 105 | Alpha: 1 106 | Show Axes: false 107 | Show Trail: false 108 | Value: true 109 | Name: RobotModel 110 | Robot Description: robot_description 111 | TF Prefix: "" 112 | Update Interval: 0 113 | Value: true 114 | Visual Enabled: true 115 | - Alpha: 1 116 | Class: rviz/Axes 117 | Enabled: true 118 | Length: 1 119 | Name: Axes 120 | Radius: 0.009999999776482582 121 | Reference Frame: 122 | Value: true 123 | - Alpha: 1 124 | Axes Length: 1 125 | Axes Radius: 0.10000000149011612 126 | Class: rviz/Pose 127 | Color: 255; 25; 0 128 | Enabled: true 129 | Head Length: 0.30000001192092896 130 | Head Radius: 0.10000000149011612 131 | Name: Pose 132 | Queue Size: 10 133 | Shaft Length: 1 134 | Shaft Radius: 0.05000000074505806 135 | Shape: Arrow 136 | Topic: /move_base_simple/goal 137 | Unreliable: false 138 | Value: true 139 | - Alpha: 0.5 140 | Cell Size: 1 141 | Class: rviz/Grid 142 | Color: 160; 160; 164 143 | Enabled: true 144 | Line Style: 145 | Line Width: 0.029999999329447746 146 | Value: Lines 147 | Name: Grid 148 | Normal Cell Count: 0 149 | Offset: 150 | X: 0 151 | Y: 0 152 | Z: 0 153 | Plane: XY 154 | Plane Cell Count: 10 155 | Reference Frame: 156 | Value: true 157 | Enabled: true 158 | Global Options: 159 | Background Color: 48; 48; 48 160 | Default Light: true 161 | Fixed Frame: base_footprint 162 | Frame Rate: 30 163 | Name: root 164 | Tools: 165 | - Class: rviz/Interact 166 | Hide Inactive Objects: true 167 | - Class: rviz/MoveCamera 168 | - Class: rviz/Select 169 | - Class: rviz/FocusCamera 170 | - Class: rviz/Measure 171 | - Class: rviz/SetInitialPose 172 | Theta std deviation: 0.2617993950843811 173 | Topic: /initialpose 174 | X std deviation: 0.5 175 | Y std deviation: 0.5 176 | - Class: rviz/SetGoal 177 | Topic: /move_base_simple/goal 178 | - Class: rviz/PublishPoint 179 | Single click: true 180 | Topic: /clicked_point 181 | Value: true 182 | Views: 183 | Current: 184 | Class: rviz/ThirdPersonFollower 185 | Distance: 1.1994764804840088 186 | Enable Stereo Rendering: 187 | Stereo Eye Separation: 0.05999999865889549 188 | Stereo Focal Distance: 1 189 | Swap Stereo Eyes: false 190 | Value: false 191 | Field of View: 0.7853981852531433 192 | Focal Point: 193 | X: -0.04994729161262512 194 | Y: 0.4954104423522949 195 | Z: -1.0132799843631801e-06 196 | Focal Shape Fixed Size: true 197 | Focal Shape Size: 0.05000000074505806 198 | Invert Z Axis: false 199 | Name: Current View 200 | Near Clip Distance: 0.009999999776482582 201 | Pitch: 0.8197966814041138 202 | Target Frame: 203 | Yaw: 0.8304104208946228 204 | Saved: ~ 205 | Window Geometry: 206 | Displays: 207 | collapsed: false 208 | Height: 948 209 | Hide Left Dock: false 210 | Hide Right Dock: true 211 | QMainWindow State: 000000ff00000000fd00000004000000000000017a00000316fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000316000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000037afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000280000037a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d0065010000000000000780000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000006000000031600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 212 | Selection: 213 | collapsed: false 214 | Time: 215 | collapsed: false 216 | Tool Properties: 217 | collapsed: false 218 | Views: 219 | collapsed: true 220 | Width: 1920 221 | X: 1920 222 | Y: 27 223 | -------------------------------------------------------------------------------- /chapter_6/mobile_robot_description/config/robot.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 | - /TF1 10 | - /TF1/Frames1 11 | - /RobotModel1 12 | - /RobotModel1/Links1 13 | - /Axes1 14 | - /Pose1 15 | Splitter Ratio: 0.5 16 | Tree Height: 747 17 | - Class: rviz/Selection 18 | Name: Selection 19 | - Class: rviz/Tool Properties 20 | Expanded: 21 | - /2D Pose Estimate1 22 | - /2D Nav Goal1 23 | - /Publish Point1 24 | Name: Tool Properties 25 | Splitter Ratio: 0.588679 26 | - Class: rviz/Views 27 | Expanded: 28 | - /Current View1 29 | Name: Views 30 | Splitter Ratio: 0.5 31 | - Class: rviz/Time 32 | Experimental: false 33 | Name: Time 34 | SyncMode: 0 35 | SyncSource: "" 36 | Visualization Manager: 37 | Class: "" 38 | Displays: 39 | - Alpha: 0.5 40 | Cell Size: 1 41 | Class: rviz/Grid 42 | Color: 160; 160; 164 43 | Enabled: true 44 | Line Style: 45 | Line Width: 0.03 46 | Value: Lines 47 | Name: Grid 48 | Normal Cell Count: 0 49 | Offset: 50 | X: 0 51 | Y: 0 52 | Z: 0 53 | Plane: XY 54 | Plane Cell Count: 10 55 | Reference Frame: odom 56 | Value: true 57 | - Class: rviz/TF 58 | Enabled: true 59 | Frame Timeout: 15 60 | Frames: 61 | All Enabled: false 62 | base_footprint: 63 | Value: false 64 | base_link: 65 | Value: false 66 | left_wheel_link: 67 | Value: false 68 | odom: 69 | Value: true 70 | right_wheel_link: 71 | Value: false 72 | robot_caster_back_link: 73 | Value: false 74 | Marker Scale: 1 75 | Name: TF 76 | Show Arrows: true 77 | Show Axes: true 78 | Show Names: false 79 | Tree: 80 | odom: 81 | base_footprint: 82 | base_link: 83 | left_wheel_link: 84 | {} 85 | right_wheel_link: 86 | {} 87 | robot_caster_back_link: 88 | {} 89 | Update Interval: 0 90 | Value: true 91 | - Alpha: 1 92 | Class: rviz/RobotModel 93 | Collision Enabled: false 94 | Enabled: true 95 | Links: 96 | All Links Enabled: true 97 | Expand Joint Details: false 98 | Expand Link Details: false 99 | Expand Tree: false 100 | Link Tree Style: "" 101 | base_footprint: 102 | Alpha: 1 103 | Show Axes: false 104 | Show Trail: false 105 | base_link: 106 | Alpha: 1 107 | Show Axes: false 108 | Show Trail: false 109 | Value: true 110 | left_wheel_link: 111 | Alpha: 1 112 | Show Axes: false 113 | Show Trail: false 114 | Value: true 115 | right_wheel_link: 116 | Alpha: 1 117 | Show Axes: false 118 | Show Trail: false 119 | Value: true 120 | robot_caster_back_link: 121 | Alpha: 1 122 | Show Axes: false 123 | Show Trail: false 124 | Value: true 125 | Name: RobotModel 126 | Robot Description: robot_description 127 | TF Prefix: "" 128 | Update Interval: 0 129 | Value: true 130 | Visual Enabled: true 131 | - Class: rviz/Axes 132 | Enabled: true 133 | Length: 1 134 | Name: Axes 135 | Radius: 0.01 136 | Reference Frame: 137 | Value: true 138 | - Alpha: 1 139 | Axes Length: 1 140 | Axes Radius: 0.1 141 | Class: rviz/Pose 142 | Color: 255; 25; 0 143 | Enabled: true 144 | Head Length: 0.3 145 | Head Radius: 0.1 146 | Name: Pose 147 | Shaft Length: 1 148 | Shaft Radius: 0.05 149 | Shape: Arrow 150 | Topic: /move_base_simple/goal 151 | Unreliable: false 152 | Value: true 153 | Enabled: true 154 | Global Options: 155 | Background Color: 48; 48; 48 156 | Fixed Frame: odom 157 | Frame Rate: 30 158 | Name: root 159 | Tools: 160 | - Class: rviz/Interact 161 | Hide Inactive Objects: true 162 | - Class: rviz/MoveCamera 163 | - Class: rviz/Select 164 | - Class: rviz/FocusCamera 165 | - Class: rviz/Measure 166 | - Class: rviz/SetInitialPose 167 | Topic: /initialpose 168 | - Class: rviz/SetGoal 169 | Topic: /move_base_simple/goal 170 | - Class: rviz/PublishPoint 171 | Single click: true 172 | Topic: /clicked_point 173 | Value: true 174 | Views: 175 | Current: 176 | Class: rviz/ThirdPersonFollower 177 | Distance: 8.81253 178 | Enable Stereo Rendering: 179 | Stereo Eye Separation: 0.06 180 | Stereo Focal Distance: 1 181 | Swap Stereo Eyes: false 182 | Value: false 183 | Focal Point: 184 | X: 0.210075 185 | Y: 1.75923 186 | Z: -1.01328e-06 187 | Name: Current View 188 | Near Clip Distance: 0.01 189 | Pitch: 1.5698 190 | Target Frame: 191 | Value: ThirdPersonFollower (rviz) 192 | Yaw: 1.54041 193 | Saved: ~ 194 | Window Geometry: 195 | Displays: 196 | collapsed: false 197 | Height: 1028 198 | Hide Left Dock: false 199 | Hide Right Dock: true 200 | QMainWindow State: 000000ff00000000fd00000004000000000000017a0000037afc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000280000037a000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000037afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000280000037a000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003a80000003efc0100000002fb0000000800540069006d00650100000000000003a8000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000002280000037a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 201 | Selection: 202 | collapsed: false 203 | Time: 204 | collapsed: false 205 | Tool Properties: 206 | collapsed: false 207 | Views: 208 | collapsed: true 209 | Width: 936 210 | X: 963 211 | Y: 14 212 | -------------------------------------------------------------------------------- /chapter_6/mobile_robot_description/launch/view_model.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /chapter_6/mobile_robot_description/launch/view_robot.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /chapter_6/mobile_robot_description/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | mobile_robot_description 4 | 0.0.1 5 | The mobile_robot_description package 6 | 7 | Lentin 8 | 9 | BSD 10 | 11 | 12 | catkin 13 | urdf 14 | xacro 15 | urdf 16 | xacro 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /chapter_6/mobile_robot_description/urdf/robot_model.xacro: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | -------------------------------------------------------------------------------- /chapter_6/mobile_robot_pkg/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(mobile_robot_pkg) 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 COMPONENTS 8 | move_base_msgs 9 | nav_msgs 10 | robot_state_publisher 11 | roscpp 12 | rospy 13 | std_msgs 14 | tf 15 | ) 16 | 17 | 18 | catkin_package() 19 | 20 | 21 | include_directories( 22 | ${catkin_INCLUDE_DIRS} 23 | ) 24 | 25 | 26 | -------------------------------------------------------------------------------- /chapter_6/mobile_robot_pkg/launch/keyboard_teleop.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 0.125 8 | 39 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /chapter_6/mobile_robot_pkg/launch/robot_standalone.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /chapter_6/mobile_robot_pkg/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | mobile_robot_pkg 4 | 0.0.1 5 | The mobile_robot_pkg package 6 | 7 | lentin 8 | 9 | BSD 10 | 11 | 12 | catkin 13 | move_base_msgs 14 | nav_msgs 15 | robot_state_publisher 16 | roscpp 17 | rospy 18 | std_msgs 19 | tf 20 | 21 | move_base_msgs 22 | nav_msgs 23 | robot_state_publisher 24 | roscpp 25 | rospy 26 | std_msgs 27 | tf 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /chapter_6/mobile_robot_pkg/scripts/dead_reckoning.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | """ 3 | dead_reckoning - Subscribe navigation goal from Rviz and move robot to the pose 4 | 5 | Copyright (C) 2021 Lentin Joseph. 6 | 7 | This program is free software: you can redistribute it and/or modify 8 | it under the terms of the GNU General Public License as published by 9 | the Free Software Foundation, either version 3 of the License, or 10 | (at your option) any later version. 11 | 12 | This program is distributed in the hope that it will be useful, 13 | but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | GNU General Public License for more details. 16 | 17 | You should have received a copy of the GNU General Public License 18 | along with this program. If not, see . 19 | """ 20 | import rospy 21 | 22 | from nav_msgs.msg import Odometry 23 | from geometry_msgs.msg import PoseStamped 24 | from geometry_msgs.msg import Twist 25 | from std_msgs.msg import Float32 26 | import tf 27 | 28 | from math import radians, degrees 29 | import math 30 | 31 | import signal 32 | import sys 33 | 34 | class DeadReckoning: 35 | 36 | def __init__(self): 37 | 38 | self.goal_x = 0.0 39 | self.goal_y = 0.0 40 | self.goal_yaw = 0.0 41 | 42 | self.robot_pose_x = 0.0 43 | self.robot_pose_y = 0.0 44 | self.robot_yaw = 0.0 45 | 46 | self.stop_vel = 0 47 | self.delta_angle = 45 48 | 49 | self.turn_vel = 15 50 | self.linear_vel = 1 51 | 52 | self.delta_distance = 0.4 53 | self.obstacle_distance = 50 54 | 55 | self.obstacle_distance_upper_limit = 40 56 | self.obstacle_distance_lower_limit = 5 57 | 58 | #Publish command velocity 59 | self.pub_twist = rospy.Publisher('/cmd_vel', Twist, queue_size=1) 60 | #Subscriber of Move base goal command 61 | rospy.Subscriber('/move_base_simple/goal', PoseStamped, self.poseCallback) 62 | #Odom callback 63 | rospy.Subscriber('/odom', Odometry, self.odomCallback) 64 | #Obstacle callback 65 | rospy.Subscriber('/obstacle_distance', Float32, self.obstacleCallback) 66 | 67 | signal.signal(signal.SIGINT, self.shutdown) 68 | pass 69 | 70 | def odomCallback(self,msg): 71 | 72 | position = msg.pose.pose.position 73 | orientation = msg.pose.pose.orientation 74 | 75 | self.robot_pose_x = position.x 76 | self.robot_pose_y = position.y 77 | 78 | roll, pitch, yaw = tf.transformations.euler_from_quaternion( 79 | [orientation.x, orientation.y, orientation.z, orientation.w]) 80 | 81 | yaw_degree = degrees(yaw) 82 | 83 | if(yaw_degree < 0): 84 | yaw_degree = 360 + yaw_degree 85 | 86 | self.robot_yaw = yaw_degree 87 | 88 | rospy.loginfo(msg) 89 | 90 | 91 | def poseCallback(self,msg): 92 | 93 | position = msg.pose.position 94 | quat = msg.pose.orientation 95 | 96 | rospy.loginfo("Point Position: [ %f, %f, %f ]"%(position.x, position.y, position.z)) 97 | rospy.loginfo("Quat Orientation: [ %f, %f, %f, %f]"%(quat.x, quat.y, quat.z, quat.w)) 98 | 99 | # Pose of robot 100 | self.goal_x = position.x 101 | self.goal_y = position.y 102 | 103 | # Also print Roll, Pitch, Yaw 104 | roll, pitch, yaw = tf.transformations.euler_from_quaternion( 105 | [quat.x, quat.y, quat.z, quat.w]) 106 | 107 | self.goal_yaw = degrees(yaw) 108 | 109 | if(self.goal_yaw < 0): 110 | self.goal_yaw = 360 + self.goal_yaw 111 | 112 | 113 | self.do_dead_reckoning(self.goal_x,self.goal_y,self.goal_yaw) 114 | 115 | 116 | def do_dead_reckoning(self,goal_x, goal_y, yaw_degree): 117 | 118 | rospy.loginfo("Goal Position: [ %f, %f, %f ]" % (goal_x, goal_y, yaw_degree)) 119 | rospy.loginfo("Robot Position: [ %f, %f, %f ]" % 120 | (self.robot_pose_x, self.robot_pose_y, self.robot_yaw)) 121 | 122 | difference_angle = yaw_degree - self.robot_yaw 123 | rospy.loginfo("Difference angle %d",difference_angle) 124 | 125 | if(difference_angle > 180): 126 | difference_angle = 360 + difference_angle 127 | self.turn_vel = -self.turn_vel 128 | 129 | rospy.loginfo("Robot rotating along in the goal axis") 130 | 131 | ####################################################################################### 132 | #Robot is rotating to align to the goal position 133 | while(abs(difference_angle) > self.delta_angle): 134 | 135 | try: 136 | difference_angle = self.goal_yaw - self.robot_yaw 137 | rospy.loginfo("Difference angle [%f]" % ( 138 | difference_angle)) 139 | difference_angle = abs(difference_angle) 140 | self.send_turn_cmd(1) 141 | rospy.sleep(0.01) 142 | except: 143 | rospy.logwarn("Exception at rotation") 144 | self.send_turn_cmd(self.stop_vel) 145 | rospy.sleep(2) 146 | sys.exit(0) 147 | 148 | self.send_turn_cmd(self.stop_vel) 149 | rospy.sleep(2) 150 | 151 | ############################################################################################ 152 | #Robot moving to goal point 153 | rospy.loginfo("Robot moving to the goal point") 154 | 155 | distance = math.hypot(goal_x - self.robot_pose_x, self.goal_y - self.robot_pose_y) 156 | rospy.loginfo("Robot moving to the goal point [%f]" % (distance)) 157 | 158 | 159 | while(distance > self.delta_distance): 160 | 161 | try: 162 | 163 | distance = math.hypot(goal_x - self.robot_pose_x, goal_y - self.robot_pose_y) 164 | rospy.loginfo("Distance [%f]" % (distance)) 165 | self.send_linear_cmd(self.current_linear_velocity) 166 | rospy.sleep(0.01) 167 | 168 | if(distance > 1.5): 169 | rospy.logwarn("Robot went outside the goal") 170 | break 171 | 172 | if(self.obstacle_distance <= self.obstacle_distance_upper_limit and self.obstacle_distance >= self.obstacle_distance_lower_limit): 173 | rospy.logwarn( 174 | "Obstacle detected, stopping robot and moving away from it") 175 | self.send_linear_cmd(self.stop_vel) 176 | rospy.sleep(1) 177 | self.send_linear_cmd(-self.current_linear_velocity) 178 | rospy.sleep(0.4) 179 | self.send_linear_cmd(self.stop_vel) 180 | break 181 | 182 | except: 183 | 184 | rospy.logwarn("Exception at translation") 185 | self.send_linear_cmd(self.stop_vel) 186 | rospy.sleep(2) 187 | sys.exit(0) 188 | 189 | self.send_linear_cmd(self.stop_vel) 190 | rospy.sleep(2) 191 | 192 | 193 | def send_linear_cmd(self,speed): 194 | 195 | twist = Twist() 196 | twist.linear.x = speed 197 | twist.linear.y = 0 198 | twist.linear.z = 0 199 | 200 | twist.angular.x = 0 201 | twist.angular.y = 0 202 | twist.angular.z = 0 203 | 204 | self.pub_twist.publish(twist) 205 | 206 | 207 | def send_turn_cmd(self,speed): 208 | 209 | twist = Twist() 210 | 211 | twist.linear.x = 0 212 | twist.linear.y = 0 213 | twist.linear.z = 0 214 | 215 | twist.angular.x = 0 216 | twist.angular.y = 0 217 | twist.angular.z = speed 218 | 219 | self.pub_twist.publish(twist) 220 | 221 | def obstacleCallback(self,msg): 222 | 223 | self.obstacle_distance = msg.data 224 | rospy.loginfo(msg) 225 | 226 | def shutdown(self,signum,frame): 227 | rospy.loginfo("Shutting down node") 228 | sys.exit(0) 229 | pass 230 | 231 | 232 | 233 | if __name__ == "__main__": 234 | rospy.init_node("Dead_reckoning_node") 235 | rospy.loginfo("Starting Dead reckoning node") 236 | 237 | obj = DeadReckoning() 238 | 239 | 240 | rospy.spin() 241 | -------------------------------------------------------------------------------- /chapter_6/mobile_robot_pkg/scripts/diff_tf.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | """ 4 | diff_tf.py - follows the output of a wheel encoder and 5 | creates tf and odometry messages. 6 | some code borrowed from the arbotix diff_controller script 7 | A good reference: http://rossum.sourceforge.net/papers/DiffSteer/ 8 | 9 | Copyright (C) 2012 Jon Stephan. 10 | 11 | This program is free software: you can redistribute it and/or modify 12 | it under the terms of the GNU General Public License as published by 13 | the Free Software Foundation, either version 3 of the License, or 14 | (at your option) any later version. 15 | 16 | This program is distributed in the hope that it will be useful, 17 | but WITHOUT ANY WARRANTY; without even the implied warranty of 18 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 19 | GNU General Public License for more details. 20 | 21 | You should have received a copy of the GNU General Public License 22 | along with this program. If not, see . 23 | 24 | ---------------------------------- 25 | Portions of this code borrowed from the arbotix_python diff_controller. 26 | 27 | diff_controller.py - controller for a differential drive 28 | Copyright (c) 2010-2011 Vanadium Labs LLC. All right reserved. 29 | 30 | Redistribution and use in source and binary forms, with or without 31 | modification, are permitted provided that the following conditions are met: 32 | * Redistributions of source code must retain the above copyright 33 | notice, this list of conditions and the following disclaimer. 34 | * Redistributions in binary form must reproduce the above copyright 35 | notice, this list of conditions and the following disclaimer in the 36 | documentation and/or other materials provided with the distribution. 37 | * Neither the name of Vanadium Labs LLC nor the names of its 38 | contributors may be used to endorse or promote products derived 39 | from this software without specific prior written permission. 40 | 41 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 42 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 43 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 44 | DISCLAIMED. IN NO EVENT SHALL VANADIUM LABS BE LIABLE FOR ANY DIRECT, INDIRECT, 45 | INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 46 | LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, 47 | OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 48 | LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 49 | OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF 50 | ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 51 | 52 | """ 53 | 54 | import rospy 55 | from math import sin, cos, pi 56 | 57 | from geometry_msgs.msg import Quaternion 58 | from geometry_msgs.msg import Twist 59 | 60 | from geometry_msgs.msg import Vector3 61 | 62 | from nav_msgs.msg import Odometry 63 | import tf 64 | from tf.broadcaster import TransformBroadcaster 65 | from std_msgs.msg import Int16, Int32, Int64 66 | 67 | ############################################################################# 68 | class DiffTf: 69 | ############################################################################# 70 | 71 | ############################################################################# 72 | def __init__(self): 73 | ############################################################################# 74 | rospy.init_node("diff_tf") 75 | self.nodename = rospy.get_name() 76 | rospy.loginfo("-I- %s started" % self.nodename) 77 | 78 | #### parameters ####### 79 | 80 | #Wheel radius : 0.0325 81 | # wheel circum = 2* 3.14 * 0.0325 = 0.2041 82 | # One rotation encoder ticks : 20 ticks (resolution of encoder disk) 83 | # For 1 meter: 20 * ( 1 / 0.2041) = 98 ticks 84 | 85 | self.rate = rospy.get_param('~rate',10.0) # the rate at which to publish the transform 86 | self.ticks_meter = float(rospy.get_param('ticks_meter', 98)) # The number of wheel encoder ticks per meter of travel 87 | self.base_width = float(rospy.get_param('~base_width', 0.125)) # The wheel base width in meters 88 | 89 | self.base_frame_id = rospy.get_param('~base_frame_id','base_footprint') # the name of the base frame of the robot 90 | self.odom_frame_id = rospy.get_param('~odom_frame_id', 'odom') # the name of the odometry reference frame 91 | 92 | self.encoder_min = rospy.get_param('encoder_min', -2147483648) 93 | self.encoder_max = rospy.get_param('encoder_max', 2147483648) 94 | self.encoder_low_wrap = rospy.get_param('wheel_low_wrap', (self.encoder_max - self.encoder_min) * 0.3 + self.encoder_min ) 95 | self.encoder_high_wrap = rospy.get_param('wheel_high_wrap', (self.encoder_max - self.encoder_min) * 0.7 + self.encoder_min ) 96 | 97 | self.t_delta = rospy.Duration(1.0/self.rate) 98 | self.t_next = rospy.Time.now() + self.t_delta 99 | 100 | # internal data 101 | self.enc_left = None # wheel encoder readings 102 | self.enc_right = None 103 | self.left = 0 # actual values coming back from robot 104 | self.right = 0 105 | self.lmult = 0 106 | self.rmult = 0 107 | self.prev_lencoder = 0 108 | self.prev_rencoder = 0 109 | self.x = 0 # position in xy plane 110 | self.y = 0 111 | self.th = 0 112 | self.dx = 0 # speeds in x/rotation 113 | self.dr = 0 114 | self.yaw = 0.01 115 | self.pitch = 0.01 116 | self.roll = 0.01 117 | 118 | self.then = rospy.Time.now() 119 | 120 | self.quaternion_1 = Quaternion() 121 | 122 | # subscriptions 123 | rospy.Subscriber("left_ticks", Int32, self.lwheelCallback) 124 | rospy.Subscriber("right_ticks", Int32, self.rwheelCallback) 125 | 126 | self.odomPub = rospy.Publisher("odom", Odometry,queue_size=10) 127 | self.odomBroadcaster = TransformBroadcaster() 128 | 129 | ############################################################################# 130 | def spin(self): 131 | ############################################################################# 132 | r = rospy.Rate(self.rate) 133 | while not rospy.is_shutdown(): 134 | self.update() 135 | r.sleep() 136 | 137 | 138 | ############################################################################# 139 | def update(self): 140 | ############################################################################# 141 | now = rospy.Time.now() 142 | if now > self.t_next: 143 | elapsed = now - self.then 144 | self.then = now 145 | elapsed = elapsed.to_sec() 146 | 147 | 148 | # calculate odometry 149 | if self.enc_left == None: 150 | d_left = 0 151 | d_right = 0 152 | else: 153 | d_left = (self.left - self.enc_left) / self.ticks_meter 154 | d_right = (self.right - self.enc_right) / self.ticks_meter 155 | self.enc_left = self.left 156 | self.enc_right = self.right 157 | 158 | # distance traveled is the average of the two wheels 159 | d = ( d_left + d_right ) / 2 160 | # this approximation works (in radians) for small angles 161 | th = ( d_right - d_left ) / self.base_width 162 | # calculate velocities 163 | self.dx = d / elapsed 164 | self.dr = th / elapsed 165 | 166 | 167 | 168 | if (d != 0): 169 | # calculate distance traveled in x and y 170 | x = cos( th ) * d 171 | y = -sin( th ) * d 172 | # calculate the final position of the robot 173 | self.x = self.x + ( cos( self.th ) * x - sin( self.th ) * y ) 174 | self.y = self.y + ( sin( self.th ) * x + cos( self.th ) * y ) 175 | if( th != 0): 176 | self.th = self.th + th 177 | 178 | # publish the odom information 179 | quaternion = Quaternion() 180 | 181 | 182 | quaternion.x = 0.0 183 | quaternion.y = 0.0 184 | 185 | quaternion.z = sin( self.th / 2 ) 186 | quaternion.w = cos( self.th / 2 ) 187 | 188 | self.odomBroadcaster.sendTransform( 189 | (self.x, self.y, 0), 190 | (quaternion.x, quaternion.y, quaternion.z, quaternion.w), 191 | rospy.Time.now(), 192 | self.base_frame_id, 193 | self.odom_frame_id 194 | ) 195 | 196 | odom = Odometry() 197 | odom.header.stamp = now 198 | odom.header.frame_id = self.odom_frame_id 199 | odom.pose.pose.position.x = self.x 200 | odom.pose.pose.position.y = self.y 201 | odom.pose.pose.position.z = 0 202 | odom.pose.pose.orientation = quaternion 203 | odom.child_frame_id = self.base_frame_id 204 | odom.twist.twist.linear.x = self.dx 205 | odom.twist.twist.linear.y = 0 206 | odom.twist.twist.angular.z = self.dr 207 | self.odomPub.publish(odom) 208 | 209 | 210 | ############################################################################# 211 | def lwheelCallback(self, msg): 212 | ############################################################################# 213 | enc = msg.data 214 | if (enc < self.encoder_low_wrap and self.prev_lencoder > self.encoder_high_wrap): 215 | self.lmult = self.lmult + 1 216 | 217 | if (enc > self.encoder_high_wrap and self.prev_lencoder < self.encoder_low_wrap): 218 | self.lmult = self.lmult - 1 219 | 220 | self.left = 1.0 * (enc + self.lmult * (self.encoder_max - self.encoder_min)) 221 | 222 | 223 | self.prev_lencoder = enc 224 | 225 | ############################################################################# 226 | def rwheelCallback(self, msg): 227 | ############################################################################# 228 | enc = msg.data 229 | if(enc < self.encoder_low_wrap and self.prev_rencoder > self.encoder_high_wrap): 230 | self.rmult = self.rmult + 1 231 | 232 | if(enc > self.encoder_high_wrap and self.prev_rencoder < self.encoder_low_wrap): 233 | self.rmult = self.rmult - 1 234 | 235 | self.right = 1.0 * (enc + self.rmult * (self.encoder_max - self.encoder_min)) 236 | 237 | 238 | self.prev_rencoder = enc 239 | 240 | ############################################################################# 241 | ############################################################################# 242 | if __name__ == '__main__': 243 | """ main """ 244 | diffTf = DiffTf() 245 | diffTf.spin() 246 | 247 | 248 | 249 | -------------------------------------------------------------------------------- /chapter_6/mobile_robot_pkg/scripts/pid_velocity.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | """ 3 | pid_velocity - takes messages on wheel_vtarget 4 | target velocities for the wheels and monitors wheel for feedback 5 | 6 | Copyright (C) 2012 Jon Stephan. 7 | 8 | This program is free software: you can redistribute it and/or modify 9 | it under the terms of the GNU General Public License as published by 10 | the Free Software Foundation, either version 3 of the License, or 11 | (at your option) any later version. 12 | 13 | This program is distributed in the hope that it will be useful, 14 | but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | GNU General Public License for more details. 17 | 18 | You should have received a copy of the GNU General Public License 19 | along with this program. If not, see . 20 | """ 21 | 22 | import rospy 23 | import roslib 24 | 25 | from std_msgs.msg import Int16,Int64 26 | from std_msgs.msg import Float32 27 | from numpy import array 28 | 29 | 30 | ###################################################### 31 | ###################################################### 32 | class PidVelocity(): 33 | ###################################################### 34 | ###################################################### 35 | 36 | 37 | ##################################################### 38 | def __init__(self): 39 | ##################################################### 40 | rospy.init_node("pid_velocity") 41 | self.nodename = rospy.get_name() 42 | rospy.loginfo("%s started" % self.nodename) 43 | 44 | ### initialize variables 45 | self.target = 0 46 | self.motor = 0 47 | self.vel = 0 48 | self.integral = 0 49 | self.error = 0 50 | self.derivative = 0 51 | self.previous_error = 0 52 | self.wheel_prev = 0 53 | self.wheel_latest = 0 54 | self.then = rospy.Time.now() 55 | self.wheel_mult = 0 56 | self.prev_encoder = 0 57 | 58 | ### get parameters #### 59 | self.Kp = rospy.get_param('~Kp',10) 60 | self.Ki = rospy.get_param('~Ki',10) 61 | self.Kd = rospy.get_param('~Kd',0.001) 62 | self.out_min = rospy.get_param('~out_min',-255) 63 | self.out_max = rospy.get_param('~out_max',255) 64 | self.rate = rospy.get_param('~rate',30) 65 | self.rolling_pts = rospy.get_param('~rolling_pts',2) 66 | self.timeout_ticks = rospy.get_param('~timeout_ticks',4) 67 | self.ticks_per_meter = rospy.get_param('ticks_meter', 14860) 68 | self.vel_threshold = rospy.get_param('~vel_threshold', 0.001) 69 | self.encoder_min = rospy.get_param('encoder_min', -2147483648) 70 | self.encoder_max = rospy.get_param('encoder_max', 2147483648) 71 | self.encoder_low_wrap = rospy.get_param('wheel_low_wrap', (self.encoder_max - self.encoder_min) * 0.3 + self.encoder_min ) 72 | self.encoder_high_wrap = rospy.get_param('wheel_high_wrap', (self.encoder_max - self.encoder_min) * 0.7 + self.encoder_min ) 73 | self.prev_vel = [0.0] * self.rolling_pts 74 | self.wheel_latest = 0.0 75 | self.prev_pid_time = rospy.Time.now() 76 | rospy.logdebug("%s got Kp:%0.3f Ki:%0.3f Kd:%0.3f tpm:%0.3f" % (self.nodename, self.Kp, self.Ki, self.Kd, self.ticks_per_meter)) 77 | 78 | #### subscribers/publishers 79 | rospy.Subscriber("wheel", Int64, self.wheelCallback) 80 | rospy.Subscriber("wheel_vtarget", Float32, self.targetCallback) 81 | self.pub_motor = rospy.Publisher('motor_cmd',Float32,queue_size=10) 82 | self.pub_vel = rospy.Publisher('wheel_vel', Float32,queue_size=10) 83 | 84 | 85 | ##################################################### 86 | def spin(self): 87 | ##################################################### 88 | self.r = rospy.Rate(self.rate) 89 | self.then = rospy.Time.now() 90 | self.ticks_since_target = self.timeout_ticks 91 | self.wheel_prev = self.wheel_latest 92 | self.then = rospy.Time.now() 93 | while not rospy.is_shutdown(): 94 | self.spinOnce() 95 | self.r.sleep() 96 | 97 | ##################################################### 98 | def spinOnce(self): 99 | ##################################################### 100 | self.previous_error = 0.0 101 | self.prev_vel = [0.0] * self.rolling_pts 102 | self.integral = 0.0 103 | self.error = 0.0 104 | self.derivative = 0.0 105 | self.vel = 0.0 106 | 107 | # only do the loop if we've recently recieved a target velocity message 108 | while not rospy.is_shutdown() and self.ticks_since_target < self.timeout_ticks: 109 | self.calcVelocity() 110 | self.doPid() 111 | self.pub_motor.publish(self.motor) 112 | self.r.sleep() 113 | self.ticks_since_target += 1 114 | if self.ticks_since_target == self.timeout_ticks: 115 | self.pub_motor.publish(0) 116 | 117 | ##################################################### 118 | def calcVelocity(self): 119 | ##################################################### 120 | self.dt_duration = rospy.Time.now() - self.then 121 | self.dt = self.dt_duration.to_sec() 122 | rospy.logdebug("-D- %s caclVelocity dt=%0.3f wheel_latest=%0.3f wheel_prev=%0.3f" % (self.nodename, self.dt, self.wheel_latest, self.wheel_prev)) 123 | 124 | if (self.wheel_latest == self.wheel_prev): 125 | # we haven't received an updated wheel lately 126 | cur_vel = (1 / self.ticks_per_meter) / self.dt # if we got a tick right now, this would be the velocity 127 | if abs(cur_vel) < self.vel_threshold: 128 | # if the velocity is < threshold, consider our velocity 0 129 | rospy.logdebug("-D- %s below threshold cur_vel=%0.3f vel=0" % (self.nodename, cur_vel)) 130 | self.appendVel(0) 131 | self.calcRollingVel() 132 | else: 133 | rospy.logdebug("-D- %s above threshold cur_vel=%0.3f" % (self.nodename, cur_vel)) 134 | if abs(cur_vel) < self.vel: 135 | rospy.logdebug("-D- %s cur_vel < self.vel" % self.nodename) 136 | # we know we're slower than what we're currently publishing as a velocity 137 | self.appendVel(cur_vel) 138 | self.calcRollingVel() 139 | 140 | else: 141 | # we received a new wheel value 142 | cur_vel = (self.wheel_latest - self.wheel_prev) / self.dt 143 | self.appendVel(cur_vel) 144 | self.calcRollingVel() 145 | rospy.logdebug("-D- %s **** wheel updated vel=%0.3f **** " % (self.nodename, self.vel)) 146 | self.wheel_prev = self.wheel_latest 147 | self.then = rospy.Time.now() 148 | 149 | self.pub_vel.publish(self.vel) 150 | 151 | ##################################################### 152 | def appendVel(self, val): 153 | ##################################################### 154 | self.prev_vel.append(val) 155 | del self.prev_vel[0] 156 | 157 | ##################################################### 158 | def calcRollingVel(self): 159 | ##################################################### 160 | p = array(self.prev_vel) 161 | self.vel = p.mean() 162 | 163 | ##################################################### 164 | def doPid(self): 165 | ##################################################### 166 | pid_dt_duration = rospy.Time.now() - self.prev_pid_time 167 | pid_dt = pid_dt_duration.to_sec() 168 | self.prev_pid_time = rospy.Time.now() 169 | 170 | self.error = self.target - self.vel 171 | self.integral = self.integral + (self.error * pid_dt) 172 | # rospy.loginfo("i = i + (e * dt): %0.3f = %0.3f + (%0.3f * %0.3f)" % (self.integral, self.integral, self.error, pid_dt)) 173 | self.derivative = (self.error - self.previous_error) / pid_dt 174 | self.previous_error = self.error 175 | 176 | self.motor = (self.Kp * self.error) + (self.Ki * self.integral) + (self.Kd * self.derivative) 177 | 178 | if self.motor > self.out_max: 179 | self.motor = self.out_max 180 | self.integral = self.integral - (self.error * pid_dt) 181 | if self.motor < self.out_min: 182 | self.motor = self.out_min 183 | self.integral = self.integral - (self.error * pid_dt) 184 | 185 | if (self.target == 0): 186 | self.motor = 0 187 | 188 | rospy.logdebug("vel:%0.2f tar:%0.2f err:%0.2f int:%0.2f der:%0.2f ## motor:%d " % 189 | (self.vel, self.target, self.error, self.integral, self.derivative, self.motor)) 190 | 191 | 192 | 193 | 194 | ##################################################### 195 | def wheelCallback(self, msg): 196 | ###################################################### 197 | enc = long(msg.data) 198 | # rospy.logwarn(enc) 199 | if (enc < self.encoder_low_wrap and self.prev_encoder > self.encoder_high_wrap) : 200 | self.wheel_mult = self.wheel_mult + 1 201 | 202 | if (enc > self.encoder_high_wrap and self.prev_encoder < self.encoder_low_wrap) : 203 | self.wheel_mult = self.wheel_mult - 1 204 | 205 | 206 | self.wheel_latest = 1.0 * (enc + self.wheel_mult * (self.encoder_max - self.encoder_min)) / self.ticks_per_meter 207 | self.prev_encoder = enc 208 | 209 | 210 | # rospy.logdebug("-D- %s wheelCallback msg.data= %0.3f wheel_latest = %0.3f mult=%0.3f" % (self.nodename, enc, self.wheel_latest, self.wheel_mult)) 211 | 212 | ###################################################### 213 | def targetCallback(self, msg): 214 | ###################################################### 215 | self.target = msg.data 216 | self.ticks_since_target = 0 217 | # rospy.logdebug("-D- %s targetCallback " % (self.nodename)) 218 | 219 | 220 | if __name__ == '__main__': 221 | """ main """ 222 | pidVelocity = PidVelocity() 223 | pidVelocity.spin() 224 | 225 | 226 | -------------------------------------------------------------------------------- /chapter_6/mobile_robot_pkg/scripts/robot_teleop_key: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Copyright (c) 2011, 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 Willow Garage, Inc. 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 | import rospy 31 | 32 | from geometry_msgs.msg import Twist 33 | 34 | import sys, select, termios, tty 35 | 36 | msg = """ 37 | Control Your Turtlebot! 38 | --------------------------- 39 | Moving around: 40 | u i o 41 | j k l 42 | m , . 43 | 44 | q/z : increase/decrease max speeds by 10% 45 | w/x : increase/decrease only linear speed by 10% 46 | e/c : increase/decrease only angular speed by 10% 47 | space key, k : force stop 48 | anything else : stop smoothly 49 | 50 | CTRL-C to quit 51 | """ 52 | 53 | moveBindings = { 54 | 'i':(1,0), 55 | 'o':(1,-1), 56 | 'j':(0,1), 57 | 'l':(0,-1), 58 | 'u':(1,1), 59 | ',':(-1,0), 60 | '.':(-1,1), 61 | 'm':(-1,-1), 62 | } 63 | 64 | speedBindings={ 65 | 'q':(1.1,1.1), 66 | 'z':(.9,.9), 67 | 'w':(1.1,1), 68 | 'x':(.9,1), 69 | 'e':(1,1.1), 70 | 'c':(1,.9), 71 | } 72 | 73 | def getKey(): 74 | tty.setraw(sys.stdin.fileno()) 75 | rlist, _, _ = select.select([sys.stdin], [], [], 0.1) 76 | if rlist: 77 | key = sys.stdin.read(1) 78 | else: 79 | key = '' 80 | 81 | termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings) 82 | return key 83 | 84 | #speed = .2 85 | speed = 30 86 | turn = 30 87 | 88 | def vels(speed,turn): 89 | return "currently:\tspeed %s\tturn %s " % (speed,turn) 90 | 91 | if __name__=="__main__": 92 | settings = termios.tcgetattr(sys.stdin) 93 | 94 | rospy.init_node('robot_teleop') 95 | pub = rospy.Publisher('~cmd_vel', Twist, queue_size=5) 96 | 97 | x = 0 98 | th = 0 99 | status = 0 100 | count = 0 101 | acc = 0.1 102 | target_speed = 0 103 | target_turn = 0 104 | control_speed = 0 105 | control_turn = 0 106 | try: 107 | print (msg) 108 | print (vels(speed,turn)) 109 | while(1): 110 | key = getKey() 111 | if key in moveBindings.keys(): 112 | x = moveBindings[key][0] 113 | th = moveBindings[key][1] 114 | count = 0 115 | elif key in speedBindings.keys(): 116 | 117 | #Modified for mobile robot 118 | #speed = speed * speedBindings[key][0] 119 | #turn = turn * speedBindings[key][1] 120 | 121 | speed = speed 122 | turn = turn 123 | 124 | 125 | count = 0 126 | 127 | print (vels(speed,turn)) 128 | if (status == 14): 129 | print (msg) 130 | status = (status + 1) % 15 131 | elif key == ' ' or key == 'k' : 132 | x = 0 133 | th = 0 134 | control_speed = 0 135 | control_turn = 0 136 | else: 137 | count = count + 1 138 | if count > 4: 139 | x = 0 140 | th = 0 141 | if (key == '\x03'): 142 | break 143 | 144 | target_speed = speed * x 145 | target_turn = turn * th 146 | 147 | if target_speed > control_speed: 148 | control_speed = min( target_speed, control_speed + 0.1 ) 149 | elif target_speed < control_speed: 150 | control_speed = max( target_speed, control_speed - 0.1 ) 151 | else: 152 | control_speed = target_speed 153 | 154 | if target_turn > control_turn: 155 | control_turn = min( target_turn, control_turn + 0.5 ) 156 | elif target_turn < control_turn: 157 | control_turn = max( target_turn, control_turn - 0.5 ) 158 | else: 159 | control_turn = target_turn 160 | 161 | twist = Twist() 162 | twist.linear.x = control_speed; twist.linear.y = 0; twist.linear.z = 0 163 | twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = control_turn 164 | pub.publish(twist) 165 | 166 | 167 | except: 168 | rospy.logwarn("Exception in teleoperation") 169 | 170 | finally: 171 | twist = Twist() 172 | twist.linear.x = 0; twist.linear.y = 0; twist.linear.z = 0 173 | twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0 174 | pub.publish(twist) 175 | 176 | termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings) 177 | 178 | -------------------------------------------------------------------------------- /chapter_6/mobile_robot_pkg/scripts/twist_to_motors.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | """ 4 | twist_to_motors - converts a twist message to motor commands. Needed for navigation stack 5 | 6 | 7 | Copyright (C) 2012 Jon Stephan. 8 | 9 | This program is free software: you can redistribute it and/or modify 10 | it under the terms of the GNU General Public License as published by 11 | the Free Software Foundation, either version 3 of the License, or 12 | (at your option) any later version. 13 | 14 | This program is distributed in the hope that it will be useful, 15 | but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | GNU General Public License for more details. 18 | 19 | You should have received a copy of the GNU General Public License 20 | along with this program. If not, see . 21 | """ 22 | 23 | import rospy 24 | from std_msgs.msg import Float32, Int32, Bool 25 | from geometry_msgs.msg import Twist 26 | 27 | #Mapping -1 - 1 to -255 to 255 28 | from numpy import interp 29 | ############################################################# 30 | ############################################################# 31 | class TwistToMotors(): 32 | ############################################################# 33 | ############################################################# 34 | 35 | ############################################################# 36 | def __init__(self): 37 | ############################################################# 38 | rospy.init_node("twist_to_motors") 39 | nodename = rospy.get_name() 40 | rospy.loginfo("%s started" % nodename) 41 | 42 | rospy.on_shutdown(self.shutdown_cb) 43 | 44 | 45 | self.sign = lambda a: (a>0) - (a<0) 46 | 47 | self.left_speed = Int32() 48 | self.right_speed = Int32() 49 | 50 | self.w = rospy.get_param("~base_width", 0.125) 51 | self.fixed_speed = rospy.get_param("~fixed_speed", 180) 52 | 53 | self.pub_lmotor = rospy.Publisher('set_left_speed', Int32,queue_size=1) 54 | self.pub_rmotor = rospy.Publisher('set_right_speed', Int32,queue_size=1) 55 | 56 | self.reset = rospy.Publisher('reset', Bool,queue_size=1) 57 | 58 | rospy.Subscriber('/cmd_vel', Twist, self.twistCallback) 59 | 60 | 61 | self.rate = rospy.get_param("~rate", 50) 62 | self.timeout_ticks = rospy.get_param("~timeout_ticks", 2) 63 | self.left = 0 64 | self.right = 0 65 | 66 | def shutdown_cb(self): 67 | rospy.logwarn("Resetting board") 68 | self.pub_lmotor.publish(0) 69 | self.pub_rmotor.publish(0) 70 | 71 | self.reset.publish(0) 72 | 73 | 74 | pass 75 | 76 | ############################################################# 77 | def spin(self): 78 | ############################################################# 79 | 80 | r = rospy.Rate(self.rate) 81 | idle = rospy.Rate(10) 82 | then = rospy.Time.now() 83 | self.ticks_since_target = self.timeout_ticks 84 | 85 | ###### main loop ###### 86 | while not rospy.is_shutdown(): 87 | 88 | while not rospy.is_shutdown() and self.ticks_since_target < self.timeout_ticks: 89 | self.spinOnce() 90 | r.sleep() 91 | idle.sleep() 92 | 93 | ############################################################# 94 | def spinOnce(self): 95 | ############################################################# 96 | 97 | # dx = (l + r) / 2 98 | # dr = (r - l) / w 99 | 100 | self.right = 1.0 * self.dx + self.dr * self.w / 2 101 | self.left = 1.0 * self.dx - self.dr * self.w / 2 102 | 103 | self.left_mapped = self.sign(self.left)*self.fixed_speed 104 | self.right_mapped = self.sign(self.right)*self.fixed_speed 105 | 106 | self.left_speed.data = int(self.left_mapped) 107 | self.right_speed.data =int(self.right_mapped) 108 | 109 | rospy.loginfo(self.left_speed) 110 | rospy.loginfo(self.right_speed) 111 | 112 | self.pub_lmotor.publish(self.left_speed) 113 | self.pub_rmotor.publish(self.right_speed) 114 | 115 | self.ticks_since_target += 1 116 | 117 | ############################################################# 118 | def twistCallback(self,msg): 119 | ############################################################# 120 | # rospy.loginfo("-D- twistCallback: %s" % str(msg)) 121 | self.ticks_since_target = 0 122 | self.dx = msg.linear.x 123 | self.dr = msg.angular.z 124 | self.dy = msg.linear.y 125 | 126 | ############################################################# 127 | ############################################################# 128 | if __name__ == '__main__': 129 | """ main """ 130 | twistToMotors = TwistToMotors() 131 | twistToMotors.spin() 132 | -------------------------------------------------------------------------------- /cover_edition_2.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Apress/Robot-Operating-System-Abs-Begs/b3d77f5a7590484ad1642cc2e2d4917a45a2a2b9/cover_edition_2.jpg --------------------------------------------------------------------------------