├── README.md
├── chapter10
    ├── gmapping_only.launch
    ├── scan_to_ogm.cpp
    ├── tf_echo2.cpp
    └── tf_pub.cpp
├── chapter11
    ├── encoder_odom_publisher.cpp
    ├── manual_pose_and_goal_pub.cpp
    ├── rviz_click_to_2d.cpp
    └── tf_pub.cpp
├── chapter12
    ├── simple_diff_drive.cpp
    └── simple_drive_controller.cpp
├── chapter13
    ├── costmap_basic.yaml
    ├── path_planner.cpp
    └── practical_common.h
├── chapter14
    └── tick_publisher.cpp
├── chapter15
    └── ultrasonic_publisher.cpp
├── chapter16
    ├── lsm303_l3gd20h_imu_pub.cpp
    └── lsm303_l3gd20h_imu_w_mag.cpp
├── chapter19
    ├── cv_bridge_test.cpp
    ├── cv_test_node.cpp
    ├── hsv_colors.cpp
    └── image_transport_test.cpp
├── chapter2
    ├── hello_blink.cpp
    ├── hello_button.cpp
    └── hello_callback.cpp
├── chapter20
    └── odom_plus.cpp
├── chapter3
    ├── hello_roomba.cpp
    └── roomba_sleep.cpp
├── chapter4
    └── hello_motor.cpp
├── chapter5
    ├── hello_i2c_lsm303.cpp
    └── hello_serial.cpp
└── chapter9
    ├── go_to_x.cpp
    ├── go_to_xy.cpp
    └── simple_goal_pub.cpp


/README.md:
--------------------------------------------------------------------------------
1 | # practical-robotics-hello-programs
2 | This repository contains code examples that accompany the book Practical Robotics in C++ by Lloyd Brombach.
3 | Visit https://github.com/lbrombach/practical_robot for the entire ROS package for the robot project build from chapter 21.  
4 | 


--------------------------------------------------------------------------------
/chapter10/gmapping_only.launch:
--------------------------------------------------------------------------------
 1 | <launch>
 2 | 
 3 | <arg name="scan_topic" default="scan" />
 4 | 
 5 | 
 6 | <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">
 7 | 
 8 |     <param name="odom_frame" value="odom"/>
 9 |     <param name="base_frame" value="base_link"/>
10 |     <param name="map_frame" value="map"/>
11 | 
12 |     <!-- Process 1 out of every this many scans (set it to a higher number to skip more scans)  -->
13 |     <param name="throttle_scans" value="1"/>
14 | 
15 |     <param name="map_update_interval" value="5.0"/> <!-- default: 5.0 -->
16 | 
17 |     <!-- The maximum usable range of the laser. A beam is cropped to this value.  -->
18 |     <param name="maxUrange" value="7"/>
19 | 
20 |     <!-- The maximum range of the sensor. If regions with no obstacles within the range of the sensor should appear as free space in the map, set maxUrange < maximum range of the real sensor <= maxRange -->
21 |     <param name="maxRange" value="8"/>
22 | 
23 |     //tf broadcast period in seconds. default = .05. 0 to disable
24 |     <param name="transform_publish_period" value=".05"/>
25 | 
26 |     <param name="sigma" value="0.05"/>
27 |     <param name="kernelSize" value="1"/>
28 |     <param name="lstep" value="0.05"/>
29 |     <param name="astep" value="0.05"/>
30 |     <param name="iterations" value="5"/>
31 |     <param name="lsigma" value="0.075"/>
32 |     <param name="ogain" value="3.0"/>
33 |     <param name="minimumScore" value="15.0"/>
34 |     <!-- Number of beams to skip in each scan. -->
35 |     <param name="lskip" value="0"/>
36 | 
37 |     <param name="srr" value="0.01"/>
38 |     <param name="srt" value="0.02"/>
39 |     <param name="str" value="0.01"/>
40 |     <param name="stt" value="0.02"/>
41 | 
42 |     <!-- Process a scan each time the robot translates this far  -->
43 |     <param name="linearUpdate" value="0.1"/>
44 | 
45 |     <!-- Process a scan each time the robot rotates this far  -->
46 |     <param name="angularUpdate" value="0.1"/>
47 | 
48 |     <param name="temporalUpdate" value="-1.0"/>
49 |     <param name="resampleThreshold" value="0.5"/>
50 | 
51 |     <!-- Number of particles in the filter. default 30        -->
52 |     <param name="particles" value="15"/>
53 | 
54 | <!-- Initial map size  -->
55 |     <param name="xmin" value="0"/>
56 |     <param name="ymin" value="0"/>
57 |     <param name="xmax" value="10.0"/>
58 |     <param name="ymax" value="10.0"/>
59 | 
60 |     <!-- Processing parameters (resolution of the map)  -->
61 |     <param name="delta" value="0.1"/>
62 | 
63 |     <param name="llsamplerange" value="0.01"/>
64 |     <param name="llsamplestep" value="0.01"/>
65 |     <param name="lasamplerange" value="0.005"/>
66 |     <param name="lasamplestep" value="0.005"/>
67 | 
68 |     <remap from="scan" to="$(arg scan_topic)"/>
69 |   </node>
70 | 
71 | </launch>
72 | 


--------------------------------------------------------------------------------
/chapter10/scan_to_ogm.cpp:
--------------------------------------------------------------------------------
  1 | /*
  2 | *scan_to_ogm_beta.cpp
  3 | *
  4 | *This is a simple ROS node that accepts synchronized laser scan and pose messages to build an
  5 | *occupancy grid map and publish that map message. This is an example program to accompany chapter 10 
  6 | *of the book Practical Robotics in C++ by Lloyd Brombach and published by Packt Publishing.
  7 | *
  8 | *Author: Lloyd Brombach (lbrombach2@gmail.com)
  9 | *10/24/2019
 10 | */
 11 | 
 12 | #include "ros/ros.h"
 13 | #include <message_filters/subscriber.h>
 14 | #include <message_filters/time_synchronizer.h>
 15 | #include <message_filters/sync_policies/approximate_time.h>
 16 | #include "nav_msgs/OccupancyGrid.h"
 17 | #include "geometry_msgs/PoseStamped.h"
 18 | #include <sensor_msgs/LaserScan.h>
 19 | #include <iostream>
 20 | #include <cmath>
 21 | 
 22 | using namespace std;
 23 | using namespace sensor_msgs;
 24 | using namespace geometry_msgs;
 25 | using namespace message_filters;
 26 | 
 27 | //manual offset for laser position. Better to handle with transforms
 28 | double transformX = .1;
 29 | 
 30 | //map metadata
 31 | const int width = 100;
 32 | const int height = 100;
 33 | const double resolution = .1;
 34 | 
 35 | const int MIN_INTENSITY = 45;
 36 | 
 37 | ros::Publisher pubMap;
 38 | nav_msgs::OccupancyGrid::Ptr mapPtr(new nav_msgs::OccupancyGrid());
 39 | 
 40 | 
 41 | //helper functions to make sure we iterate from low to high in marking
 42 | double getStart(double collision, double currentPose, double border = 0)
 43 | {
 44 |     double start = (currentPose < collision) ? currentPose : collision;
 45 |     start = (start < 0) ? 0 : start;
 46 |     return start;
 47 | }
 48 | double getEnd(double collision, double currentPose, double border)
 49 | {
 50 |     double end = (currentPose < collision) ? collision : currentPose;
 51 |     end = (end > border) ? border : end;
 52 |     return end;
 53 | }
 54 | 
 55 | //decreases occupancy value of cells that fall between laser and detected obstacle
 56 | void mark_free(double xCollision, double yCollision, double scanAngle, const PoseStampedConstPtr &pose)
 57 | {
 58 |     mapPtr->data[mapPtr->info.width * (int)pose->pose.position.y*10 + int(pose->pose.position.x)*10] = 0;
 59 |     double m = -1;
 60 |     double b = -1;
 61 |     int start = 0;
 62 |     int last = 0;
 63 |     int ogmIndex = 0;
 64 |     int y = 0;
 65 |     // cannot divide by zero, vertical line so mark every cell free until hit
 66 |     if (pose->pose.position.x == xCollision)
 67 |     {
 68 |         for (int y = (int)(getStart(yCollision, pose->pose.position.y, 0) * resolution); y < (int)(getEnd(yCollision, pose->pose.position.y, height - 1) * resolution); y++)
 69 |         {
 70 |             // decrement is by four. Make sure we don't go below zero
 71 |                 mapPtr->data[mapPtr->info.width * y + (int)(pose->pose.position.x * resolution)] -= 15;
 72 |                 mapPtr->data[mapPtr->info.width * y + (int)(pose->pose.position.x * resolution)] =
 73 |                        (mapPtr->data[mapPtr->info.width * y + (int)(pose->pose.position.x * resolution)] <0) ?
 74 |                        0 : mapPtr->data[mapPtr->info.width * y + (int)(pose->pose.position.x * resolution)];
 75 |         }
 76 |     }
 77 |     else
 78 |     {
 79 |         /*
 80 |         /checks y coordinate at every .5 resolution * x val. Decrements cells on that lines closer to "free" until
 81 |         /we hit border or obstacle cell. Here we start by calulating y=m*x+b slope and b values
 82 |         */
 83 |         m = (pose->pose.position.y - yCollision) / (pose->pose.position.x - xCollision);
 84 |         b = (pose->pose.position.y / resolution * 10) - (m * pose->pose.position.x / resolution * 10);
 85 | 
 86 |         //order for iterating. Everything multiplied by 10 so we can iterate at higher resolution than 1x per cell
 87 |         start = getStart(xCollision, pose->pose.position.x, 0) / resolution * 10;
 88 |         last = getEnd(xCollision, pose->pose.position.x, width / resolution) / resolution * 10;
 89 |         //iterate every .5 cell widths. Decrease occupancy value by 4 per scan
 90 |         for (int x = start; x < last; x += 5)
 91 |         {
 92 |             y = m * x + b;
 93 |             ogmIndex = (mapPtr->info.width * (y / 10)) + (x / 10);
 94 |             if (y > 0 && y / 10 < mapPtr->info.width)
 95 |             {
 96 |                 mapPtr->data[ogmIndex] -= 15;
 97 |                 mapPtr->data[ogmIndex] = (mapPtr->data[ogmIndex] < 0) ? 0 : mapPtr->data[ogmIndex];
 98 |             }
 99 |         }
100 |     }
101 | }
102 | 
103 | 
104 | //callback function that happens whenwe recieve pose with close enough time stamp to scan
105 | //ranges are in meters. cells are .1 meters so we mulitply hits by 10 to identify cell x and y
106 | void scan_handler(const PoseStampedConstPtr &pose, const LaserScanConstPtr &scan)
107 | {
108 |     int xHit = -1;
109 |     int yHit = -1;
110 |     for (int i = 0; i < scan->ranges.size(); i++)
111 |     {
112 |         //look for objects detected over every scan index when intesity is high enough
113 |         if (scan->intensities[i] > MIN_INTENSITY )
114 |         {
115 |             double scanAngle = pose->pose.orientation.z + scan->angle_min + (i * scan->angle_increment);
116 |             double xDistance = cos(scanAngle) * scan->ranges[i];
117 |             double yDistance = sin(scanAngle) * scan->ranges[i];
118 |             double xCollision = xDistance + pose->pose.position.x;
119 |             double yCollision = yDistance + pose->pose.position.y;
120 |             //mark free whatever is between laser and object detected
121 |             //x and y map coordinates of detection
122 |             xHit = int(xCollision * 10);
123 |             yHit = int(yCollision * 10);
124 |             //increase occupancy value by 20 whatever cell the object is detected in to a max value of 100
125 |             if (xHit >= 0 && xHit < width && yHit >= 0 && yHit < height && mapPtr->data[mapPtr->info.width * yHit + xHit] <= 100&& scan->ranges[i] > scan->range_min && scan->ranges[i] < scan->range_max )
126 |             {
127 |                 mapPtr->data[mapPtr->info.width * yHit + xHit] += 25;
128 |              if(mapPtr->data[mapPtr->info.width * yHit + (xHit+1)]<mapPtr->data.size())
129 |                 mapPtr->data[mapPtr->info.width * yHit + (xHit+1)] += 25;
130 | 
131 |                 mapPtr->data[mapPtr->info.width * yHit + xHit] = (mapPtr->data[mapPtr->info.width * yHit + xHit] > 100) ? 100 : mapPtr->data[mapPtr->info.width * yHit + xHit];
132 |             }
133 |             if(scan->ranges[i] > scan->range_min && scan->ranges[i] < scan->range_max)
134 |             {
135 |             mark_free(xCollision, yCollision, scanAngle, pose);
136 |             }
137 | 
138 |         }
139 |     }
140 |     mapPtr->header.stamp = ros::Time::now();
141 |     pubMap.publish(mapPtr);
142 | }
143 | 
144 | int main(int argc, char **argv)
145 | {
146 | 
147 |     mapPtr->info.width = width;
148 |     mapPtr->info.height = height;
149 |     mapPtr->info.resolution = resolution;
150 | 
151 |     mapPtr->info.origin.position.x = 0;
152 |     mapPtr->info.origin.position.y = 0;
153 |     //init all cells to unknown
154 |     mapPtr->data.resize(mapPtr->info.width * mapPtr->info.height);
155 |     for (int i = 0; i < mapPtr->data.size(); i++)
156 |     {
157 |         mapPtr->data[i] = -1;
158 |     }
159 | 
160 | 
161 |     ros::init(argc, argv, "simple_OGM_Pub");
162 |     ros::NodeHandle node;
163 |     //use filters and approximate sync to run callack when timestamps are close enough
164 |     message_filters::Subscriber<PoseStamped> pose_sub(node, "pose", 1);
165 |     message_filters::Subscriber<LaserScan> scan_sub(node, "scan", 1);
166 | 
167 |     typedef sync_policies::ApproximateTime<PoseStamped, LaserScan> MySyncPolicy;
168 |     // ApproximateTime takes a queue size as its constructor argument, hence MySyncPolicy(10)
169 |     Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), pose_sub, scan_sub);
170 |     sync.registerCallback(boost::bind(&scan_handler, _1, _2));
171 | 
172 |     //advertise publisher
173 |     pubMap = node.advertise<nav_msgs::OccupancyGrid>("map", 0);
174 | 
175 |     while (ros::ok())
176 |     {
177 |         ros::spin();
178 |     }
179 | 
180 |     return 0;
181 | }
182 | 


--------------------------------------------------------------------------------
/chapter10/tf_echo2.cpp:
--------------------------------------------------------------------------------
 1 | /*
 2 | *tf_echo2.cpp is a simple ROS node that displays odom to base_link 
 3 | *transform data based on a lookupTransform() call.
 4 | *Intent is a simple utility that helps teach converting from quaternion to euler angle.
 5 | *This program is an accompaniment to the book Practical Robotics in C++
 6 | *written by Lloyd Brombach and published by Packt Publishing
 7 | *
 8 | *Author: Lloyd Brombach (lbrombach2@gmail.com)
 9 | *11/7/2019
10 | */
11 | 
12 | #include "ros/ros.h"
13 | #include <nav_msgs/Odometry.h>
14 | #include <tf/transform_listener.h>
15 | #include "std_msgs/Float32.h"
16 | 
17 | using namespace std;
18 | 
19 | float z = 0;
20 | 
21 | int main(int argc, char **argv)
22 | {
23 |     //handshake with ros master
24 |     ros::init(argc, argv, "tf_echo2");
25 |     ros::NodeHandle node;
26 | 
27 |     //set to 1 hz
28 |     ros::Rate loop_rate(1);
29 |     while (ros::ok())
30 |     {
31 |         ros::spinOnce();
32 |         //create listener. Must be static.
33 |         static tf::TransformListener listener;
34 | 
35 |         //create transform object
36 |         tf::StampedTransform odom_base_tf;
37 | 
38 |         //test if transform available
39 |         if (listener.canTransform("odom", "base_link", ros::Time(0), NULL))
40 |         {
41 |             //get transform data
42 |             listener.lookupTransform("odom", "base_link", ros::Time(0), odom_base_tf);
43 | 
44 |             //convert quaternion data to Euler angle
45 |             tf::Quaternion q(0, 0, odom_base_tf.getRotation().z(), odom_base_tf.getRotation().w());
46 |             tf::Matrix3x3 m(q);
47 |             double roll, pitch, yaw;
48 |             m.getRPY(roll, pitch, yaw);
49 |             z = yaw;
50 | 
51 |             //display transform data
52 |             cout << odom_base_tf.getOrigin().x() << ", " << odom_base_tf.getOrigin().y() << "   " << z << endl;
53 |         }
54 |         else
55 |         {
56 |             cout << "UNABLE TO LOOKUP ODOM -> BASE_LINK TRANSFORM" << endl;
57 |         }
58 |         loop_rate.sleep();
59 |     }
60 | 
61 |     return 0;
62 | }
63 | 


--------------------------------------------------------------------------------
/chapter10/tf_pub.cpp:
--------------------------------------------------------------------------------
 1 | /*
 2 | *tf_pub.cpp is a simple ROS node that subscribes to an odometry message
 3 | *and publishes an odom to base_link transform based on that data.
 4 | *Unlike some of node and messages we use for examples in Practical Robotics in C++,
 5 | *tf_pub uses quaternions and illustrates how to conver euler to quaternion
 6 | *This program is an accompaniment to the book Practical Robotics in C++
 7 | *written by Lloyd Brombach and published by Packt Publishing
 8 | *
 9 | *Author: Lloyd Brombach (lbrombach2@gmail.com)
10 | *11/7/2019
11 | */
12 | 
13 | #include "ros/ros.h"
14 | #include <nav_msgs/Odometry.h>
15 | #include <tf/transform_broadcaster.h>
16 | #include "std_msgs/Float32.h"
17 | 
18 | 
19 | using namespace std;
20 | 
21 | //callback function that broadcasts odom message data as a transform
22 | //this odom message should contain an euler angle in the orientation z data member
23 | void handle_odom(const nav_msgs::Odometry &odom)
24 | {
25 |     //create broadcater object
26 |     static tf::TransformBroadcaster br;
27 |     //create transform object
28 |     tf::Transform odom_base_tf;
29 | 
30 |     //set transform x, y location with data from odom message
31 |     odom_base_tf.setOrigin( tf::Vector3(odom.pose.pose.position.x, odom.pose.pose.position.y, 0.0) );
32 |     //create quaternion object from euler data received in odom
33 |     tf::Quaternion tf_q(odom.pose.pose.orientation.x, odom.pose.pose.orientation.y,
34 |                         odom.pose.pose.orientation.z, odom.pose.pose.orientation.w);
35 | 
36 |     //add quaternion data to transform object
37 |     odom_base_tf.setRotation(tf_q);
38 | 
39 |     //broadcast transform
40 |     br.sendTransform(tf::StampedTransform(odom_base_tf, odom.header.stamp, "odom", "base_link"));
41 |     
42 | }
43 | 
44 | 
45 | int main(int argc, char **argv)
46 | {
47 |     ros::init(argc, argv, "tf_pub");
48 |     ros::NodeHandle node;
49 | 
50 |     ros::Subscriber subOdom = node.subscribe("encoder/odom_quat", 10, handle_odom);
51 | 
52 |     ros::Rate loop_rate(30);
53 |     while (ros::ok())
54 |     {
55 |      ros::spinOnce();
56 |      loop_rate.sleep();
57 |     }
58 | 
59 |     return 0;
60 | }
61 | 


--------------------------------------------------------------------------------
/chapter11/encoder_odom_publisher.cpp:
--------------------------------------------------------------------------------
  1 | /*
  2 | *This is a ROS node that subscribes to encoder count messages and publishes odometry
  3 | *on in a simple form where orientation.z is an euler angle, then publishes again
  4 | *on a topic that uses the quaternion version of the message. This is written
  5 | *to be readable for all levels and accompanies the book Practical Robotics in C++.
  6 | *
  7 | *Author: Lloyd Brombach (lbrombach2@gmail.com)
  8 | *11/7/2019
  9 | */
 10 | 
 11 | #include "ros/ros.h"
 12 | #include "std_msgs/Int16.h"
 13 | #include <nav_msgs/Odometry.h>
 14 | #include <geometry_msgs/PoseStamped.h>
 15 | #include <tf2/LinearMath/Quaternion.h>
 16 | #include <tf2_ros/transform_broadcaster.h>
 17 | #include <cmath>
 18 | 
 19 | ros::Publisher odom_pub;
 20 | ros::Publisher pub_quat;
 21 | nav_msgs::Odometry newOdom;
 22 | nav_msgs::Odometry oldOdom;
 23 | 
 24 | const double initialX = 0.0;
 25 | const double initialY = 0.0;
 26 | const double initialTheta = 0.00000000001;
 27 | const double PI = 3.141592;
 28 | 
 29 | const double ticksPerwheelRev = 508.8; //not in use. just a reference for now
 30 | const double wheelRadius = .03575; // 55.18;
 31 | const double WHEEL_BASE = .224; //223.8375mm actually
 32 | const double TICKS_PER_M = 1125*2; //1.1645; //1.365 is on hard floor. carpet avg is 1.1926. overall avg = 1.1645
 33 | 
 34 | double leftDistance = 0;
 35 | double rightDistance = 0;
 36 | bool initialPoseRecieved = false;
 37 | 
 38 | 
 39 | using namespace std;
 40 | 
 41 | //accept initial_2d pose message form clicks or manual publisher
 42 | //at at least one is required to start this node publishing
 43 | void setInitialPose(const geometry_msgs::PoseStamped &rvizClick)
 44 | {
 45 |      oldOdom.pose.pose.position.x = rvizClick.pose.position.x;
 46 |      oldOdom.pose.pose.position.y = rvizClick.pose.position.y;
 47 |      oldOdom.pose.pose.orientation.z = rvizClick.pose.orientation.z;
 48 |      initialPoseRecieved = true;
 49 | }
 50 | 
 51 | //calculate distance left wheel has traveled since last cycle
 52 | void Calc_Left(const std_msgs::Int16& lCount)
 53 | {
 54 | static int lastCountL = 0;
 55 |     if(lCount.data!=0&&lastCountL!=0)
 56 |     {
 57 |         int leftTicks = (lCount.data - lastCountL);
 58 | 
 59 |         if (leftTicks > 10000)
 60 |         {
 61 |             leftTicks=0-(65535 - leftTicks);
 62 |         }
 63 |         else if (leftTicks < -10000)
 64 |         {
 65 |             leftTicks = 65535-leftTicks;
 66 |         }
 67 |         leftDistance = leftTicks/TICKS_PER_M;
 68 |     }
 69 |         lastCountL = lCount.data;
 70 | }
 71 | 
 72 | //calculate distance right wheel has traveled since last cycle
 73 | void Calc_Right(const std_msgs::Int16& rCount)
 74 | {
 75 | static int lastCountR = 0;
 76 |     if(rCount.data!=0&&lastCountR!=0)
 77 |     {
 78 |         int rightTicks = rCount.data - lastCountR;
 79 |         if (rightTicks > 10000)
 80 |         {
 81 |             rightTicks=(0-(65535 - rightTicks))/TICKS_PER_M;
 82 |         }
 83 |         else if (rightTicks < -10000)
 84 |         {
 85 |         rightTicks = 65535 - rightTicks;
 86 |         }
 87 |         rightDistance = rightTicks/TICKS_PER_M;
 88 |     }
 89 |         lastCountR=rCount.data;
 90 | }
 91 | 
 92 | //publishes the simpler version of the odom message in full quaternion form
 93 | //and includes covariance data. This covariance should be "personalized"
 94 | //for every specific robot.
 95 | void publish_quat()
 96 | {
 97 |     tf2::Quaternion q;
 98 |     q.setRPY(0, 0, newOdom.pose.pose.orientation.z);
 99 | 
100 |     nav_msgs::Odometry quatOdom;
101 |     quatOdom.header.stamp = newOdom.header.stamp;
102 |     quatOdom.header.frame_id = "odom";
103 |     quatOdom.child_frame_id = "base_link";
104 |     quatOdom.pose.pose.position.x = newOdom.pose.pose.position.x;
105 |     quatOdom.pose.pose.position.y = newOdom.pose.pose.position.y;
106 |     quatOdom.pose.pose.position.z = newOdom.pose.pose.position.z;
107 |     quatOdom.pose.pose.orientation.x = q.x();
108 |     quatOdom.pose.pose.orientation.y = q.y();
109 |     quatOdom.pose.pose.orientation.z = q.z();
110 |     quatOdom.pose.pose.orientation.w = q.w();
111 |     quatOdom.twist.twist.linear.x = newOdom.twist.twist.linear.x;
112 |     quatOdom.twist.twist.linear.y = newOdom.twist.twist.linear.y;
113 |     quatOdom.twist.twist.linear.z = newOdom.twist.twist.linear.z;
114 |     quatOdom.twist.twist.angular.x = newOdom.twist.twist.angular.x;
115 |     quatOdom.twist.twist.angular.y = newOdom.twist.twist.angular.y;
116 |     quatOdom.twist.twist.angular.z = newOdom.twist.twist.angular.z;
117 | 
118 |     for(int i = 0; i<36; i++)
119 |     {
120 |        if(i == 0 || i == 7 || i == 14)
121 |        {
122 |            quatOdom.pose.covariance[i] = .01;
123 |        }
124 |        else if (i == 21 || i == 28 || i== 35)
125 |        {
126 |            quatOdom.pose.covariance[i] += .165;
127 |        }
128 |        else
129 |        {
130 |            quatOdom.pose.covariance[i] = 0;
131 |        }
132 |     }
133 | 
134 |     pub_quat.publish(quatOdom);
135 | 
136 | }
137 | 
138 | void update_odom()
139 | {
140 |     //average distance
141 |     double cycleDistance = (rightDistance+leftDistance)/2;
142 |     //how many radians robot has turned since last cycle
143 |     double cycleAngle = asin((rightDistance-leftDistance)/WHEEL_BASE);
144 | 
145 |     //average angle during last cycle
146 |     double avgAngle = cycleAngle/2 + oldOdom.pose.pose.orientation.z;
147 |     if (avgAngle > PI)
148 |         {
149 |         avgAngle -= 2*PI;
150 |         }
151 |     else if (avgAngle < -PI)
152 |     {
153 |         avgAngle += 2*PI;
154 |     }
155 | 
156 |     //calculate new x, y, and theta
157 |     newOdom.pose.pose.position.x = oldOdom.pose.pose.position.x + cos(avgAngle)*cycleDistance;
158 |     newOdom.pose.pose.position.y = oldOdom.pose.pose.position.y + sin(avgAngle)*cycleDistance;
159 |     newOdom.pose.pose.orientation.z = cycleAngle + oldOdom.pose.pose.orientation.z;
160 | 
161 |     //prevent lockup from a single erroneous cycle
162 |     if(isnan(newOdom.pose.pose.position.x) || isnan(newOdom.pose.pose.position.y)
163 |         || isnan(newOdom.pose.pose.position.z) )
164 |     {
165 |         newOdom.pose.pose.position.x = oldOdom.pose.pose.position.x;
166 |         newOdom.pose.pose.position.y = oldOdom.pose.pose.position.y;
167 |         newOdom.pose.pose.orientation.z = oldOdom.pose.pose.orientation.z;
168 |     }
169 | 
170 |     //keep theta in range proper range
171 |     if (newOdom.pose.pose.orientation.z > PI)
172 |     {
173 |         newOdom.pose.pose.orientation.z -= 2*PI;
174 |     }
175 |     else if (newOdom.pose.pose.orientation.z < -PI)
176 |     {
177 |         newOdom.pose.pose.orientation.z += 2*PI;
178 |     }
179 | 
180 |     //calculate velocity
181 |     newOdom.header.stamp = ros::Time::now();
182 |     newOdom.twist.twist.linear.x = cycleDistance/(newOdom.header.stamp.toSec() - oldOdom.header.stamp.toSec());
183 |     newOdom.twist.twist.angular.z = cycleAngle/(newOdom.header.stamp.toSec() - oldOdom.header.stamp.toSec());
184 | 
185 |     //save odom x, y, and theta for use in next cycle
186 |     oldOdom.pose.pose.position.x = newOdom.pose.pose.position.x;
187 |     oldOdom.pose.pose.position.y = newOdom.pose.pose.position.y;
188 |     oldOdom.pose.pose.orientation.z = newOdom.pose.pose.orientation.z;
189 |     oldOdom.header.stamp = newOdom.header.stamp;
190 | 
191 |     //publish odom message
192 |     odom_pub.publish(newOdom);
193 | }
194 | 
195 | int main(int argc, char **argv)
196 | {
197 |     //set fixed data fields
198 |     newOdom.header.frame_id = "odom";
199 |     newOdom.pose.pose.position.z = 0;
200 |     newOdom.pose.pose.orientation.x = 0;
201 |     newOdom.pose.pose.orientation.y = 0;
202 |     newOdom.twist.twist.linear.x = 0;
203 |     newOdom.twist.twist.linear.y = 0;
204 |     newOdom.twist.twist.linear.z = 0;
205 |     newOdom.twist.twist.angular.x = 0;
206 |     newOdom.twist.twist.angular.y = 0;
207 |     newOdom.twist.twist.angular.z = 0;
208 |     oldOdom.pose.pose.position.x = initialX;
209 |     oldOdom.pose.pose.position.y = initialY;
210 |     oldOdom.pose.pose.orientation.z = initialTheta;
211 | 
212 |     //handshake with ros master and create node object
213 |     ros::init(argc, argv, "encoder_odom_publisher");
214 |     ros::NodeHandle node;
215 | 
216 |     //Subscribe to topics
217 |     ros::Subscriber subForRightCounts = node.subscribe("rightWheel", 100, Calc_Right,ros::TransportHints().tcpNoDelay());
218 |     ros::Subscriber subForLeftCounts = node.subscribe("leftWheel",100, Calc_Left,ros::TransportHints().tcpNoDelay());
219 |     ros::Subscriber subInitialPose = node.subscribe("initial_2d", 1, setInitialPose);
220 | 
221 |     //advertise publisher of simpler odom msg where orientation.z is an euler angle
222 |     odom_pub = node.advertise<nav_msgs::Odometry>("encoder/odom", 100);
223 | 
224 |     //advertise publisher of full odom msg where orientation is quaternion
225 |     pub_quat = node.advertise<nav_msgs::Odometry>("encoder/odom_quat", 100);
226 | 
227 | 
228 |     ros::Rate loop_rate(30);
229 |     while(ros::ok())
230 |     {
231 |         ros::spinOnce();
232 |         if(initialPoseRecieved)
233 |         {
234 |             update_odom();
235 |             publish_quat();
236 |         }
237 |         loop_rate.sleep();
238 |     }
239 | 
240 |     return 0;
241 | }
242 | 


--------------------------------------------------------------------------------
/chapter11/manual_pose_and_goal_pub.cpp:
--------------------------------------------------------------------------------
  1 | /*
  2 | *manual_pose_and_goal.cpp
  3 | *
  4 | *This ros node publishes an initial_2d pose msg or goal_2d pose message as input by the user.
  5 | *these msgs are published ignoring the quaternion nature and simply using z for yaw
  6 | *the initialpose and move_base_simple/goal are also published as if user had clicked in rviz
  7 | *
  8 | *Author: Lloyd Brombach (lbrombach2@gmail.com)
  9 | *11/4/2019
 10 | */
 11 | #include "ros/ros.h"
 12 | #include "geometry_msgs/PoseStamped.h"
 13 | #include "geometry_msgs/PoseWithCovarianceStamped.h"
 14 | #include <tf2/LinearMath/Quaternion.h>
 15 | #include <iostream>
 16 | 
 17 | using namespace std;
 18 | ros::Publisher pub;
 19 | ros::Publisher pub1;
 20 | ros::Publisher pub2;
 21 | ros::Publisher pub3;
 22 | 
 23 | 
 24 | void pub_msg(float x, float y, float yaw, int choice)
 25 | {
 26 |     geometry_msgs::PoseWithCovarianceStamped pose;
 27 |     geometry_msgs::PoseStamped goal;
 28 |     geometry_msgs::PoseStamped rpy;
 29 |     rpy.pose.position.x = x;
 30 |     rpy.pose.position.y = y;
 31 |     rpy.pose.position.z = 0;
 32 |     rpy.pose.orientation.x = 0;
 33 |     rpy.pose.orientation.y = 0;
 34 |     rpy.pose.orientation.z = yaw;
 35 |     rpy.pose.orientation.w = 0;
 36 | 
 37 |     if(choice == 1) //publish a pose
 38 |     {
 39 |         pose.header.frame_id = "map";
 40 |         pose.header.stamp = ros::Time::now();
 41 |         pose.pose.pose = rpy.pose;
 42 |         pub2.publish(rpy);
 43 |     }
 44 |     else //publish a goal
 45 |     {
 46 |         cout<<"publishering GOAL"<<endl;
 47 | 
 48 |         goal.header.frame_id = "map";
 49 |         goal.header.stamp = ros::Time::now();
 50 |         goal.pose = rpy.pose;
 51 |         pub.publish(goal);
 52 |     }
 53 | 
 54 |      tf2::Quaternion q;
 55 |      q.setRPY(0, 0, yaw);
 56 |         rpy.pose.orientation.x = q.x();
 57 |         rpy.pose.orientation.y = q.y();
 58 |         rpy.pose.orientation.z = q.z();
 59 |         rpy.pose.orientation.w = q.w();
 60 | 
 61 |         if(choice == 1) //publish a pose
 62 |     {
 63 |         pose.pose.pose = rpy.pose;
 64 |         pub3.publish(pose);
 65 |     }
 66 |     else //publish a goal
 67 |     {
 68 |         goal.pose = rpy.pose;
 69 |         pub1.publish(goal);
 70 |     }
 71 | 
 72 | 
 73 | }
 74 | 
 75 | int main(int argc, char **argv)
 76 | {
 77 |     ros::init(argc, argv, "manual_pub");
 78 |     ros::NodeHandle node;
 79 |     pub = node.advertise<geometry_msgs::PoseStamped>("goal_2d", 10);
 80 |     pub1 = node.advertise<geometry_msgs::PoseStamped>("move_base_simple/goal", 10);
 81 |     pub2 = node.advertise<geometry_msgs::PoseStamped>("initial_2d", 10);
 82 |     pub3 = node.advertise<geometry_msgs::PoseWithCovarianceStamped>("initialpose", 10);
 83 | 
 84 |     while (ros::ok())
 85 |     {
 86 |         float x =-1;
 87 |         float y =-1;
 88 |         float yaw = -11;
 89 |         int choice  = -1;
 90 | 
 91 |         while(choice < 1 || choice > 2
 92 |             || x < 0 || y < 0 || yaw < -3.141592 || yaw > 3.141592)
 93 |         {
 94 |             cout<<"\nEnter positive float value for x : "<<endl;
 95 |             cin>>x;
 96 |             cout<<"\nEnter positive float value for y : "<<endl;
 97 |             cin>>y;
 98 |             cout<<"\nEnter float value in for yaw in radians from -PI to +PI: "<<endl;
 99 |             cin>>yaw;
100 | 
101 | 
102 |             cout<<"\nEnter 1 if this is a pose, 2 if it is a goal"<<endl;
103 |             cin>>choice;
104 |             if(cin.fail())
105 |             {
106 |                 cin.clear();
107 |                 cin.ignore(50, '/n');
108 |                 choice = -1;
109 |             }
110 |         }
111 | 
112 |         pub_msg(x, y, yaw, choice);
113 | 
114 |     }
115 |     return 0;
116 | }
117 | 


--------------------------------------------------------------------------------
/chapter11/rviz_click_to_2d.cpp:
--------------------------------------------------------------------------------
 1 | /*
 2 | *rviz_click_to_2d.cpp
 3 | *
 4 | *This ros node subscribes to the clicked pose topics published by rviz
 5 | *initialpose and move_base_simple/goal, and republishes the data as simple
 6 | *2D poses using PoseStamped messages, after converting from quaternion to euler angles
 7 | *Intended to accompany example robot/nodes along with the book Practical Robotics in C++
 8 | *
 9 | *Author: Lloyd Brombach (lbrombach2@gmail.com)
10 | *11/4/2019
11 | */
12 | #include "ros/ros.h"
13 | #include "geometry_msgs/PoseStamped.h"
14 | #include "geometry_msgs/PoseWithCovarianceStamped.h"
15 | #include <tf/transform_broadcaster.h>
16 | #include <iostream>
17 | using namespace std;
18 | ros::Publisher pub;
19 | ros::Publisher pub2;
20 | 
21 | void handle_goal(const geometry_msgs::PoseStamped &goal)
22 | {
23 |     geometry_msgs::PoseStamped rpyGoal;
24 |     rpyGoal.header.frame_id = "map";
25 |     rpyGoal.header.stamp = goal.header.stamp;
26 |     rpyGoal.pose.position.x = goal.pose.position.x;
27 |     rpyGoal.pose.position.y = goal.pose.position.y;
28 |     rpyGoal.pose.position.z = 0;
29 |     tf::Quaternion q(0, 0, goal.pose.orientation.z, goal.pose.orientation.w);
30 |     tf::Matrix3x3 m(q);
31 |     double roll, pitch, yaw;
32 |     m.getRPY(roll, pitch, yaw);
33 |     rpyGoal.pose.orientation.x = 0;
34 |     rpyGoal.pose.orientation.y = 0;
35 |     rpyGoal.pose.orientation.z = yaw;
36 |     rpyGoal.pose.orientation.w = 0;
37 |     pub.publish(rpyGoal);
38 | }
39 | void handle_initial_pose(const geometry_msgs::PoseWithCovarianceStamped &pose)
40 | {
41 |     geometry_msgs::PoseStamped rpyPose;
42 |     rpyPose.header.frame_id = "map";
43 |     rpyPose.header.stamp = pose.header.stamp;
44 |     rpyPose.pose.position.x = pose.pose.pose.position.x;
45 |     rpyPose.pose.position.y = pose.pose.pose.position.y;
46 |     rpyPose.pose.position.z = 0;
47 |     tf::Quaternion q(0, 0, pose.pose.pose.orientation.z, pose.pose.pose.orientation.w);
48 |     tf::Matrix3x3 m(q);
49 |     double roll, pitch, yaw;
50 |     m.getRPY(roll, pitch, yaw);
51 |     rpyPose.pose.orientation.x = 0;
52 |     rpyPose.pose.orientation.y = 0;
53 |     rpyPose.pose.orientation.z = yaw;
54 |     rpyPose.pose.orientation.w = 0;
55 |     pub2.publish(rpyPose);
56 | }
57 | int main(int argc, char **argv)
58 | {
59 |     ros::init(argc, argv, "rviz_click_to_2d");
60 |     ros::NodeHandle node;
61 |     pub = node.advertise<geometry_msgs::PoseStamped>("goal_2d", 0);
62 |     pub2 = node.advertise<geometry_msgs::PoseStamped>("initial_2d", 0);
63 |     ros::Subscriber sub = node.subscribe("move_base_simple/goal", 0, handle_goal);
64 | 
65 |     ros::Subscriber sub2 = node.subscribe("initialpose", 0, handle_initial_pose);
66 |     ros::Rate loop_rate(10);
67 |     while (ros::ok())
68 |     {
69 |         ros::spinOnce();
70 |         loop_rate.sleep();
71 |     }
72 |     return 0;
73 | }
74 | 


--------------------------------------------------------------------------------
/chapter11/tf_pub.cpp:
--------------------------------------------------------------------------------
 1 | /*
 2 | *tf_pub.cpp is a simple ROS node that subscribes to an odometry message
 3 | *and publishes an odom to base_link transform based on that data.
 4 | *Unlike some of node and messages we use for examples in Practical Robotics in C++,
 5 | *tf_pub uses quaternions and illustrates how to conver euler to quaternion
 6 | *This program is an accompaniment to the book Practical Robotics in C++
 7 | *written by Lloyd Brombach and published by Packt Publishing
 8 | *
 9 | *Author: Lloyd Brombach (lbrombach2@gmail.com)
10 | *11/7/2019
11 | */
12 | 
13 | #include "ros/ros.h"
14 | #include <nav_msgs/Odometry.h>
15 | #include <tf/transform_broadcaster.h>
16 | #include "std_msgs/Float32.h"
17 | 
18 | 
19 | using namespace std;
20 | 
21 | //callback function that broadcasts odom message data as a transform
22 | //this odom message should contain an euler angle in the orientation z data member
23 | void handle_odom(const nav_msgs::Odometry &odom)
24 | {
25 |     //create broadcater object
26 |     static tf::TransformBroadcaster br;
27 |     //create transform object
28 |     tf::Transform odom_base_tf;
29 | 
30 |     //set transform x, y location with data from odom message
31 |     odom_base_tf.setOrigin( tf::Vector3(odom.pose.pose.position.x, odom.pose.pose.position.y, 0.0) );
32 |     //create quaternion object from euler data received in odom
33 |     tf::Quaternion tf_q(odom.pose.pose.orientation.x, odom.pose.pose.orientation.y,
34 |                         odom.pose.pose.orientation.z, odom.pose.pose.orientation.w);
35 | 
36 |     //add quaternion data to transform object
37 |     odom_base_tf.setRotation(tf_q);
38 | 
39 |     //broadcast transform
40 |     br.sendTransform(tf::StampedTransform(odom_base_tf, odom.header.stamp, "odom", "base_link"));
41 |     
42 | }
43 | 
44 | 
45 | int main(int argc, char **argv)
46 | {
47 |     ros::init(argc, argv, "tf_pub");
48 |     ros::NodeHandle node;
49 | 
50 |     ros::Subscriber subOdom = node.subscribe("encoder/odom_quat", 10, handle_odom);
51 | 
52 |     ros::Rate loop_rate(30);
53 |     while (ros::ok())
54 |     {
55 |      ros::spinOnce();
56 |      loop_rate.sleep();
57 |     }
58 | 
59 |     return 0;
60 | }
61 | 


--------------------------------------------------------------------------------
/chapter12/simple_diff_drive.cpp:
--------------------------------------------------------------------------------
  1 | /*
  2 | *simple_diff_drive.cpp
  3 | *
  4 | *This is a simple differential drive  ROS node that accepts cmd_vel messages
  5 | *and outputs PWM and direction pin signals to an L298 dual motor driver.
  6 | *This is intended to be simple to read and implement, but lacks the robustness of a PID
  7 | *Robot prioritizes angular velocities (turns) and stops forward motion while turning.
  8 | *
  9 | *Rather than a PID to maintain straight driving, a simpler check of the average drift
 10 | *over several cycles is applied to a multiplier to modify the PWM to each wheel
 11 | *This DRIFT_MULTIPLIER might need to be tweaked for individual wheel sets
 12 | *
 13 | *Pin numbers are as example project is wired in the book Practical Robotics in C++.
 14 | *
 15 | *The constants near the top should be "personalized" for your robot, but should be very close if using
 16 | *Roomba wheel modules as I did in the book project.
 17 | *
 18 | *Author: Lloyd Brombach (lbrombach2@gmail.com)
 19 | *11/7/2019
 20 | */
 21 | 
 22 | 
 23 | #include "ros/ros.h"
 24 | #include "geometry_msgs/Twist.h"
 25 | #include "std_msgs/Int16.h"
 26 | #include <iostream>
 27 | #include <pigpiod_if2.h>
 28 | #include <cstdlib>
 29 | 
 30 | 
 31 | const int PWM_INCREMENT = 1; //the rate pwm out can change per cycle
 32 | const double ticksPerwheelRev = 254*2; //508.8; //not in use yet..just a reference for now
 33 | const double wheelRadius = .03575; // 55.18;
 34 | const double wheelBase = .224; //223.8375mm actually
 35 | const double TICKS_PER_M = 1125*2; //1.1645; //1.365 is on hard floor. carpet avg is 1.1926. overall avg = 1.1645 1125.766 t/m
 36 | const int KP = 238;
 37 | const int DRIFT_MULTIPLIER = 125;
 38 | const int TURN_PWM = 54;
 39 | const int MIN_PWM = 52;
 40 | const int MAX_PWM = 100;
 41 | 
 42 | //left motor
 43 | const int PWM_L = 21;
 44 | const int MOTOR_L_FWD = 26;
 45 | const int MOTOR_L_REV = 13;
 46 | //right motor
 47 | const int PWM_R = 12;
 48 | const int MOTOR_R_FWD = 20;
 49 | const int MOTOR_R_REV = 19;
 50 | 
 51 | double leftVelocity = 0;
 52 | double rightVelocity = 0;
 53 | double leftPwmReq = 0;
 54 | double rightPwmReq = 0;
 55 | double lastCmdMsgRcvd = 0; //time of last command recieved
 56 | 
 57 | 
 58 | int pi =-1;
 59 | 
 60 | using namespace std;
 61 | 
 62 | 
 63 | void Calc_Left_Vel(const std_msgs::Int16& lCount)
 64 | {
 65 | static double lastTime = 0;
 66 | static int lastCount = 0;
 67 | int cycleDistance = (65535 + lCount.data - lastCount) % 65535;
 68 | if (cycleDistance > 10000)
 69 |     {
 70 |         cycleDistance=0-(65535 - cycleDistance);
 71 |     }
 72 | leftVelocity = cycleDistance/TICKS_PER_M/(ros::Time::now().toSec()-lastTime);
 73 | lastCount = lCount.data;
 74 | lastTime = ros::Time::now().toSec();
 75 | }
 76 | void Calc_Right_Vel(const std_msgs::Int16& rCount)
 77 | {
 78 | static double lastTime = 0;
 79 | static int lastCount = 0;
 80 | int cycleDistance = (65535 + rCount.data - lastCount) % 65535;
 81 | if (cycleDistance > 10000)
 82 |     {
 83 |         cycleDistance=0-(65535 - cycleDistance);
 84 |     }
 85 | rightVelocity = cycleDistance/TICKS_PER_M/(ros::Time::now().toSec()-lastTime);
 86 | lastCount=rCount.data;
 87 | lastTime = ros::Time::now().toSec();
 88 | }
 89 | 
 90 | 
 91 | void Set_Speeds(const geometry_msgs::Twist& cmdVel)
 92 | {
 93 |     lastCmdMsgRcvd = ros::Time::now().toSec();
 94 |     int b = (cmdVel.linear.x > .025 && cmdVel.linear.x < .052) ? 45 : 40;
 95 |     leftPwmReq  =KP*cmdVel.linear.x+b;
 96 |     rightPwmReq =KP*cmdVel.linear.x+b;
 97 |     cout<<"iniitial pwm request (left and right same) = "<< leftPwmReq<<endl;
 98 |     if(cmdVel.angular.z != 0)
 99 |     {
100 |         if(cmdVel.angular.z > .0 )//standard gentle turn
101 |         {
102 |          leftPwmReq = -TURN_PWM;
103 |          rightPwmReq = TURN_PWM;
104 |         }
105 |         else
106 |         {
107 |          leftPwmReq =   TURN_PWM;
108 |          rightPwmReq = -TURN_PWM;
109 |         }
110 |         if( abs(cmdVel.angular.z>.12))//turn a little faster if angle is greater that .12
111 |         {
112 |           leftPwmReq  *= 1.1;
113 |           rightPwmReq *= 1.1;
114 |         }
115 | 
116 |     }
117 |     else if(abs(cmdVel.linear.x) > .0478 ) // should equal about pwmval of 50
118 |     {
119 |      static double prevDiff = 0;
120 |      static double prevPrevDiff = 0;
121 |      double angularVelDifference = leftVelocity - rightVelocity; //how much faster one wheel is actually turning
122 |      double avgAngularDiff = (prevDiff+prevPrevDiff+angularVelDifference)/3; //average several cycles
123 |      prevPrevDiff=prevDiff;
124 |      prevDiff = angularVelDifference;
125 | 
126 |     //apply corrective offset to each wheel to try and go straight
127 |      leftPwmReq -= (int)(avgAngularDiff*DRIFT_MULTIPLIER);
128 |      rightPwmReq += (int)(avgAngularDiff*DRIFT_MULTIPLIER);
129 |     }
130 | 
131 |     //don't PWM values that don't do anything
132 |     if(abs(leftPwmReq)< MIN_PWM)
133 |     {
134 |      leftPwmReq=0;
135 |     }
136 | 
137 |     if(abs(rightPwmReq)<MIN_PWM)
138 |     {
139 |      rightPwmReq=0;
140 |     }
141 |     ////left for debugging and tweaking
142 |     //cout<<"CMD_VEL = "<<cmdVel.linear.x<<endl;
143 |     //cout<<"VEL, AND PWM REQ LEFT AND RIGHT "<<leftVelocity<<"    "<<leftPwmReq<<" ..... ";
144 |     //cout<<rightVelocity<<"    "<<rightPwmReq<<endl;
145 | 
146 | }
147 | 
148 | 
149 | void set_pin_values()
150 | {
151 |     static int leftPwmOut = 0;
152 |     static int rightPwmOut = 0;
153 | 
154 |     //if PwmReq*PwmOut is negative, that means the wheel is switching
155 |     //directions and we should bring to a stop before switching directions
156 |     static bool stopped = false;
157 |     if( (leftPwmReq*leftVelocity < 0 && leftPwmOut != 0)
158 |         || (rightPwmReq*rightVelocity < 0 && rightPwmOut != 0))
159 |     {
160 |         leftPwmReq = 0;
161 |         rightPwmReq = 0;
162 |     }
163 | 
164 | 
165 |     //set motor driver direction pins
166 |     if(leftPwmReq>0) //left fwd
167 |     {
168 |      gpio_write(pi, MOTOR_L_REV, 1);
169 |      gpio_write(pi, MOTOR_L_FWD, 0);
170 |     }
171 |     else if(leftPwmReq<0) //left rev
172 |     {
173 |      gpio_write(pi, MOTOR_L_FWD, 1);
174 |      gpio_write(pi, MOTOR_L_REV, 0);
175 |     }
176 |     else if (leftPwmReq == 0 && leftPwmOut == 0 ) //left stop
177 |     {
178 |      gpio_write(pi, MOTOR_L_FWD, 1);
179 |      gpio_write(pi, MOTOR_L_REV, 1);
180 |     }
181 | 
182 |     if(rightPwmReq>0 )//right fwd
183 |     {
184 |      gpio_write(pi, MOTOR_R_REV, 1);
185 |      gpio_write(pi, MOTOR_R_FWD, 0);
186 |     }
187 |     else if(rightPwmReq<0) //right rev
188 |     {
189 |      gpio_write(pi, MOTOR_R_FWD, 1);
190 |      gpio_write(pi, MOTOR_R_REV, 0);
191 |     }
192 |     else if (rightPwmReq == 0 && rightPwmOut == 0)
193 |     {
194 |      gpio_write(pi, MOTOR_R_FWD, 1);
195 |      gpio_write(pi, MOTOR_R_REV, 1);
196 |     }
197 | 
198 | 
199 |     //bump up pwm if robot is having trouble starting from stopped
200 |     if(  leftPwmReq != 0 && leftVelocity == 0)
201 |     {
202 |         leftPwmReq *= 1.4;
203 |     }
204 |     if( rightPwmReq != 0 && rightVelocity == 0)
205 |     {
206 |         rightPwmReq *= 1.4;
207 |     }
208 | 
209 |     //this section increments PWM changes instead of jarring/dangeroud sudden big changes
210 |     if (abs(leftPwmReq) > leftPwmOut)
211 |     {
212 |      leftPwmOut += PWM_INCREMENT;
213 |     }
214 |     else if (abs(leftPwmReq) < leftPwmOut)
215 |     {
216 |      leftPwmOut -= PWM_INCREMENT;
217 |     }
218 |     if (abs(rightPwmReq) > rightPwmOut)
219 |     {
220 |      rightPwmOut += PWM_INCREMENT;
221 |     }
222 |     else if(abs(rightPwmReq) < rightPwmOut)
223 |     {
224 |      rightPwmOut -= PWM_INCREMENT;
225 |     }
226 | 
227 |     //cap output at max defined in constants
228 |     leftPwmOut = (leftPwmOut>MAX_PWM) ? MAX_PWM : leftPwmOut;
229 |     rightPwmOut = (rightPwmOut>MAX_PWM) ? MAX_PWM : rightPwmOut;
230 | 
231 |     //limit output to a low of zero
232 |     leftPwmOut = (leftPwmOut< 0 ) ? 0 : leftPwmOut;
233 |     rightPwmOut = (rightPwmOut< 0) ? 0 : rightPwmOut;
234 | 
235 |     //write the pwm values tot he pins
236 |     set_PWM_dutycycle(pi, PWM_L, leftPwmOut);
237 |     set_PWM_dutycycle(pi, PWM_R, rightPwmOut);
238 | 
239 |     cout<<"PWM OUT LEFT AND RIGHT            "<<leftPwmOut<<"           "<<rightPwmOut<<endl<<endl;
240 | }
241 | 
242 | 
243 | int PigpioSetup()
244 | {
245 |     char *addrStr = NULL;
246 |     char *portStr = NULL;
247 |     pi = pigpio_start(addrStr, portStr);
248 |     //next 10 lines sets up our pins. Remember that high is "off"
249 |     //and we must drive in1 or in2 low to start the output to motor
250 |     set_mode(pi,PWM_L, PI_OUTPUT);
251 |     set_mode(pi,MOTOR_L_FWD, PI_OUTPUT);
252 |     set_mode(pi,MOTOR_L_REV, PI_OUTPUT);
253 |     set_mode(pi,PWM_R, PI_OUTPUT);
254 |     set_mode(pi,MOTOR_R_FWD, PI_OUTPUT);
255 |     set_mode(pi,MOTOR_R_REV, PI_OUTPUT);
256 | 
257 |     gpio_write(pi, MOTOR_L_FWD, 1); //initializes motor off
258 |     gpio_write(pi, MOTOR_L_REV, 1); //initializes motor off
259 |     gpio_write(pi, MOTOR_R_FWD, 1); //initializes motor off
260 |     gpio_write(pi, MOTOR_R_REV, 1); //initializes motor off
261 | 
262 |     return pi;
263 | }
264 | 
265 | int main(int argc, char **argv)
266 | {
267 |     //initialize pipiod interface
268 |     int pi = PigpioSetup();
269 |     if(PigpioSetup()>=0)
270 |     {
271 |      cout<<"daemon interface started ok at "<<pi<<endl;
272 |     }
273 |     else
274 |     {
275 |      cout<<"Failed to connect to PIGPIO Daemon - is it running?"<<endl;
276 |      return -1;
277 |     }
278 |     ////////////////
279 | 
280 |     ros::init(argc, argv, "simple_diff_drive");
281 |     ros::NodeHandle node;
282 | 
283 |     //Subscribe to topics
284 |     ros::Subscriber subForRightCounts = node.subscribe("rightWheel", 1000, Calc_Right_Vel,ros::TransportHints().tcpNoDelay());
285 |     ros::Subscriber subForLeftCounts = node.subscribe("leftWheel",1000, Calc_Left_Vel,ros::TransportHints().tcpNoDelay());
286 |     ros::Subscriber subForVelocity = node.subscribe("cmd_vel", 1, Set_Speeds,ros::TransportHints().tcpNoDelay());
287 | 
288 |     ros::Rate loop_rate(30);
289 |     while(ros::ok())
290 |     {
291 |      ros::spinOnce();
292 | 
293 |      //stop motors if no cmd_vel msgs recieved
294 |      if(ros::Time::now().toSec() - lastCmdMsgRcvd > 1)
295 |      {
296 |       cout<<"NOT RECIEVING CMD_VEL - STOPPING MOTORS  --  time sincel last = "<<ros::Time::now().toSec() - lastCmdMsgRcvd<<endl;
297 |       leftPwmReq = 0;
298 |       rightPwmReq = 0;
299 |      }
300 | 
301 |      set_pin_values();
302 |      loop_rate.sleep();
303 |     }
304 | 
305 |     gpio_write(pi, MOTOR_L_FWD, 1); //initializes motor off
306 |     gpio_write(pi, MOTOR_L_REV, 1); //initializes motor off
307 |     gpio_write(pi, MOTOR_R_FWD, 1); //initializes motor off
308 |     gpio_write(pi, MOTOR_R_REV, 1); //initializes motor off
309 | 
310 |     return 0;
311 | }
312 | 


--------------------------------------------------------------------------------
/chapter12/simple_drive_controller.cpp:
--------------------------------------------------------------------------------
  1 | /*
  2 | *simple_drive_controller.cpp is an example ROS node accompanying the book
  3 | *Practical Robotics in C++.
  4 | *
  5 | *This node subscribes to the encoder/odom topic as well as waypoint_2d. When a new
  6 | *waypoint is recieved, this node calculates a straight course to the waypoint without obstacle avoidance
  7 | *and publishes cmd_vel msgs to first turn to the waypoint, then go forward. If the angle drifts
  8 | *outside of "close enough" while enroute, the robot will stop and correct is heading before continuing.
  9 | *Written to be a readable, but functional, example for all levels following along with the book.
 10 | *
 11 | *
 12 | *Author: Lloyd Brombach (lbrombach2@gmail.com)
 13 | *11/7/2019
 14 | */
 15 | 
 16 | 
 17 | #include "ros/ros.h"
 18 | #include "geometry_msgs/Twist.h"
 19 | #include "geometry_msgs/PoseStamped.h"
 20 | #include <nav_msgs/Odometry.h>
 21 | #include <cstdlib>
 22 | #include <math.h>
 23 | #include <iostream>
 24 | 
 25 | using namespace std;
 26 | 
 27 | ros::Publisher pubVelocity;
 28 | nav_msgs::Odometry odom;
 29 | geometry_msgs::Twist cmdVel;
 30 | geometry_msgs::PoseStamped desired;
 31 | const double PI = 3.141592;
 32 | const double Ka = .35;
 33 | const double Klv = .66;
 34 | const double initialX = 5.0;
 35 | const double initialY = 5.0;
 36 | const double angularTolerance = .1;
 37 | const double distanceTolerance = .05;
 38 | const double MAX_LINEAR_VEL = 1;
 39 | bool waypointActive = false;
 40 | 
 41 | 
 42 | void update_pose(const nav_msgs::Odometry &currentOdom)
 43 | {
 44 |     odom.pose.pose.position.x = currentOdom.pose.pose.position.x;
 45 |     odom.pose.pose.position.y = currentOdom.pose.pose.position.y;
 46 |     odom.pose.pose.orientation.z = currentOdom.pose.pose.orientation.z;
 47 | }
 48 | 
 49 | void update_goal(const geometry_msgs::PoseStamped &desiredPose)
 50 | {
 51 | cout<<"got new goal!"<<endl;
 52 |     desired.pose.position.x = desiredPose.pose.position.x;
 53 |     desired.pose.position.y = desiredPose.pose.position.y;
 54 |     desired.pose.orientation.z = desiredPose.pose.orientation.z;
 55 |     waypointActive = true;
 56 |     cout<<"waypoint active set true"<<endl;
 57 | }
 58 | 
 59 | double getDistanceError()
 60 | {
 61 |     double deltaX = desired.pose.position.x - odom.pose.pose.position.x;
 62 |     double deltaY = desired.pose.position.y - odom.pose.pose.position.y;
 63 |     cout<<"Distance error = "<<sqrt(pow(deltaX, 2) + pow(deltaY, 2))<<endl;
 64 |     return sqrt(pow(deltaX, 2) + pow(deltaY, 2));
 65 | }
 66 | 
 67 | double getAngularError()
 68 | {
 69 |     double deltaX = desired.pose.position.x - odom.pose.pose.position.x;
 70 |     double deltaY = desired.pose.position.y - odom.pose.pose.position.y;
 71 |     double thetaBearing = atan2(deltaY, deltaX);
 72 |     double angularError = thetaBearing - odom.pose.pose.orientation.z;
 73 |     angularError = (angularError > PI)  ? angularError - (2*PI) : angularError;
 74 |     angularError = (angularError < -PI) ? angularError + (2*PI) : angularError;
 75 | cout<<"angular error = " <<angularError<<endl;
 76 |     return angularError;
 77 | }
 78 | 
 79 | 
 80 | void set_velocity()
 81 | {
 82 |     cmdVel.linear.x = 0;
 83 |     cmdVel.linear.y = 0;
 84 |     cmdVel.linear.z = 0;
 85 |     cmdVel.angular.x = 0;
 86 |     cmdVel.angular.y = 0;
 87 |     cmdVel.angular.z = 0;
 88 | 
 89 |     static bool angle_met = true;
 90 |     static bool location_met = true;
 91 |     double final_desired_heading_error = desired.pose.orientation.z - odom.pose.pose.orientation.z;
 92 | 
 93 |     if(abs(getDistanceError()) >= .05)
 94 |         {
 95 |         location_met = false;
 96 |         }
 97 |     else if (abs(getDistanceError()) < .03)
 98 |         {
 99 |         location_met = true;
100 |         }
101 | 
102 |     double angularError = (location_met == false) ? getAngularError() : final_desired_heading_error;
103 |     if (abs(angularError) > .15)
104 |         {
105 |          angle_met = false;
106 |         }
107 |     else if (abs(angularError) < .1)
108 |         {
109 |          angle_met = true;
110 |         }
111 | 
112 |     if (waypointActive == true && angle_met == false)
113 |         {
114 |          cmdVel.angular.z = Ka * angularError;
115 |          cmdVel.linear.x = 0;
116 |         }
117 |     else if (waypointActive == true && abs(getDistanceError()) >= .05 && location_met == false)
118 |         {
119 |          cmdVel.linear.x = Klv * getDistanceError();
120 |          cmdVel.angular.z = 0;
121 |         }
122 |     else
123 |     {
124 |         cout << "********I'm HERE, now set final desired heading! **********"<<endl;
125 |         location_met = true;
126 |     }
127 | 
128 |     if (location_met && abs(final_desired_heading_error) < .05)
129 |         {
130 |          cout<<"Target Achieved"<<endl;
131 |          waypointActive = false;
132 |         }
133 | 
134 |     pubVelocity.publish(cmdVel);
135 | 
136 | }
137 | 
138 | 
139 | int main(int argc, char **argv)
140 | {
141 |     desired.pose.position.x = -1;
142 |     ros::init(argc, argv, "simple_drive_controller");
143 |     ros::NodeHandle node;
144 | 
145 |     //Subscribe to topics
146 |     ros::Subscriber subCurrentPose = node.subscribe("encoder/odom", 10, update_pose, ros::TransportHints().tcpNoDelay());
147 |     ros::Subscriber subDesiredPose = node.subscribe("waypoint_2d", 1, update_goal, ros::TransportHints().tcpNoDelay());
148 |     pubVelocity = node.advertise<geometry_msgs::Twist>("cmd_vel", 1);
149 | 
150 |     ros::Rate loop_rate(10);
151 |     while (ros::ok())
152 |     {
153 |         ros::spinOnce();
154 |         if(desired.pose.position.x != -1)
155 |         {
156 |         set_velocity();
157 |         }
158 |         cout << "goal = " << desired.pose.position.x << ", " << desired.pose.position.y << endl
159 |              << "current x,y = " << odom.pose.pose.position.x << ", " << odom.pose.pose.position.y << endl
160 |              << "  Distance error = " << getDistanceError() << endl;
161 |         cout << "cmd_vel = " << cmdVel.linear.x <<" ,  "<<cmdVel.angular.z<< endl;
162 |         loop_rate.sleep();
163 |     }
164 | 
165 |     return 0;
166 | }
167 | 


--------------------------------------------------------------------------------
/chapter13/costmap_basic.yaml:
--------------------------------------------------------------------------------
 1 | plugins:
 2 |     - {name: static_map,       type: "costmap_2d::StaticLayer"}
 3 |     - {name: inflation_layer,   type: "costmap_2d::InflationLayer"}
 4 | 
 5 | 
 6 | #robot with radius = .16
 7 | footprint: [[-0.16, 0], [-0.113137, -0.113137], [0, -0.16], [0.113137, -0.113137], [0.16, 0], [0.113137, 0.113137], [0, 0.16], [-0.113137, 0.113137]]
 8 | 
 9 | 
10 | 
11 | global_frame: /odom
12 | robot_base_frame: base_link
13 | resolution: .1
14 | update_frequency: .5
15 | publish_frequency: .5
16 | transform_tolerance: 0.5
17 | rolling_window: false
18 | always_send_full_costmap: true
19 | 
20 | 
21 | static_layer:
22 |     map_topic: /map
23 |     unknown_cost_value: -1
24 |     lethal_cost_threshold: 100
25 |     first_map_only: false
26 |     subscribe_to_updates: false
27 | 
28 | 
29 | 
30 | 


--------------------------------------------------------------------------------
/chapter13/path_planner.cpp:
--------------------------------------------------------------------------------
  1 | /*
  2 | *path_planner.cpp
  3 | *
  4 | *This is a simple path planner node that subscribes to an occupancy grid
  5 | *_map and publishes waypoints one at a time. The next waypoint is published
  6 | *when the current pose reaches the previous waypoint. A* is the algorithm used.
  7 | *
  8 | *Author: Lloyd Brombach (lbrombach2@gmail.com)
  9 | *11/7/2019
 10 | */
 11 | #include "ros/ros.h"
 12 | #include "practical_nav/practical_common.h"
 13 | #include "nav_msgs/OccupancyGrid.h"
 14 | #include "nav_msgs/Path.h"
 15 | #include <tf/transform_listener.h>
 16 | #include <vector>
 17 | #include <math.h>
 18 | #include <iostream>
 19 | 
 20 | using namespace std;
 21 | 
 22 | //create our subscriber and publisher.
 23 | ros::Subscriber subMap, subPose, subGoal;
 24 | ros::Publisher pub;
 25 | ros::Publisher pathPub;
 26 | 
 27 | //this is where we'll keep the working _map data
 28 | nav_msgs::OccupancyGrid::Ptr _map(new nav_msgs::OccupancyGrid());
 29 | 
 30 | //we'll use these points internally for our lists
 31 | struct cell
 32 | {
 33 |     cell() : index(-1), x(-1), y(-1), theta(-1), F(INT32_MAX), G(INT32_MAX), H(INT32_MAX),
 34 |              prevX(-1), prevY(-1) {}
 35 |     cell(const cell &incoming);
 36 | 
 37 |     int index; //the index in the nav_msgs::OccupancyGrid
 38 |     int x;     //x, y as grid cells are pose in meters/mapResolution (10)
 39 |     int y;
 40 |     double theta; //the final waypoint is the goal and requires heading theta
 41 |     int F;        //this cells total cost, will be calculated as G + H
 42 |     int G;        //cost (distancetraveled) from start(current position)
 43 |     int H;        //manhatten distance distance to target
 44 |     int prevX;    //map grid coordinates of previous cell
 45 |     int prevY;
 46 | };
 47 | //copy constructor
 48 | cell::cell(const cell &incoming)
 49 | {
 50 |     index = incoming.index;
 51 |     x = incoming.x;
 52 |     y = incoming.y;
 53 |     theta = incoming.theta;
 54 |     F = incoming.F;
 55 |     G = incoming.G;
 56 |     H = incoming.H;
 57 |     prevX = incoming.prevX;
 58 |     prevY = incoming.prevY;
 59 | }
 60 | 
 61 | cell start;
 62 | cell goal;
 63 | bool goalActive = false;
 64 | 
 65 | //copy the supplied costmap to a new _map we can access freely
 66 | void map_handler(const nav_msgs::OccupancyGridPtr &costmap)
 67 | {
 68 |     static bool init_complete = false;
 69 |     //only do this stuff the first time a map is recieved.
 70 |     if (init_complete == false)
 71 |     {
 72 |         _map->header.frame_id = costmap->header.frame_id;
 73 |         _map->info.resolution = costmap->info.resolution;
 74 |         _map->info.width = costmap->info.width;
 75 |         _map->info.height = costmap->info.height;
 76 |         _map->info.origin.position.x = costmap->info.origin.position.x;
 77 |         _map->info.origin.position.y = costmap->info.origin.position.y;
 78 |         _map->info.origin.orientation.x = costmap->info.origin.orientation.x;
 79 |         _map->info.origin.orientation.y = costmap->info.origin.orientation.y;
 80 |         _map->info.origin.orientation.z = costmap->info.origin.orientation.z;
 81 |         _map->info.origin.orientation.w = costmap->info.origin.orientation.w;
 82 |         _map->data.resize(costmap->data.size());
 83 | 
 84 |         cout << "Map recieved. Initializing _map size "
 85 |              << _map->info.width << " x " << _map->info.height<<" = "<<costmap->data.size() <<"  at resolution "
 86 |              << _map->info.resolution<<"\nOrigin: "
 87 |              << _map->info.origin.position.x<<", "<< _map->info.origin.position.x<<endl;
 88 | 
 89 | 
 90 |         init_complete = true;
 91 |     }
 92 | 
 93 |     //origins in cells instead of meters.
 94 |     int originX = 1 - (_map->info.origin.position.x / _map->info.resolution);
 95 |     int originY = 1 - (_map->info.origin.position.y / _map->info.resolution);
 96 | 
 97 | 
 98 |     //this part we can do every time to ensure we see updates.
 99 |     //copy the contents of the costmap into the occupancy grid path_planner uses internally
100 |     //starting at 0, 0. Data at x<0 or y<0 in the gmap is lost, so set start position accordingly
101 |         for(int row = originY; row < _map->info.height ; row++)
102 |         {
103 |             for(int col = originX; col < _map->info.width; col++)
104 |             {
105 |                 _map->data[getIndex(col-originY, row-originX, costmap)]
106 |                             = costmap->data[getIndex(col, row, costmap)];
107 |             }
108 |         }
109 | 
110 | }
111 | 
112 | //set our start cell as the current grid cell
113 | bool update_start_cell()
114 | {
115 |     static tf::TransformListener listener;
116 |     tf::StampedTransform odom_base_tf;
117 | 
118 |     if(listener.canTransform("odom","base_link", ros::Time(0), NULL))
119 |     {
120 |         listener.lookupTransform("odom", "base_link", ros::Time(0), odom_base_tf);
121 | 
122 |         //dont forget that grid cell is pose in meters / map resolution
123 |         start.x = odom_base_tf.getOrigin().x()/ map_resolution(_map);
124 |         start.y = odom_base_tf.getOrigin().y()/ map_resolution(_map);
125 | 
126 |         tf::Quaternion q(0, 0, odom_base_tf.getRotation().z(), odom_base_tf.getRotation().w());
127 |         tf::Matrix3x3 m(q);
128 |         double roll, pitch, yaw;
129 |         m.getRPY(roll, pitch, yaw);
130 |         start.theta = yaw;
131 |         start.index = getIndex(start.x, start.y, _map);
132 |         return true;
133 |     }
134 |     else
135 |     {
136 |         cout<<"UNABLE TO LOOKUP ODOM -> BASE_LINK TRANSFORM, no path planned"<<endl;
137 |         return false;
138 |     }
139 | }
140 | 
141 | //set the goal recieved and set goalActive = true to start pathfinding
142 | void set_goal(const geometry_msgs::PoseStamped &desiredPose)
143 | {
144 |     //dont forget that grid cell is pose in meters / map resolution
145 |     goal.x = (int)(desiredPose.pose.position.x / map_resolution(_map));
146 |     goal.y = (int)(desiredPose.pose.position.y / map_resolution(_map));
147 |     goal.theta = desiredPose.pose.orientation.z;
148 |     goal.index = getIndex(goal.x, goal.y, _map);
149 |     goal.H = 0; //must set to zero to identify when found
150 |     goalActive = true;
151 |     cout << "goal active set true" << endl;
152 | }
153 | 
154 | //check if cell with index recieved is in the supplied open or closed list
155 | bool contains(vector<cell> &list, int toCheck)
156 | {
157 |     for (int i = 0; i < list.size(); i++)
158 |     {
159 |         if (list[i].index == toCheck)
160 |         {
161 |             return true;
162 |         }
163 |     }
164 |     //if not found in above loop it is not found
165 |     return false;
166 | }
167 | 
168 | //helper to calculate G - the cost to traverse this particular cell
169 | double getG(int x, int y, int currentX, int currentY, double currentG)
170 | {
171 |     //cost is infinite if cell is obstacle
172 |     if (is_obstacle(x, y, _map))
173 |     {
174 |         return INT32_MAX;
175 |     }
176 |     //if cell is not diagonal, the cost to move is 10
177 |     else if (x == currentX || y == currentY)
178 |     {
179 |         return currentG + 10;
180 |     }
181 |     //cost is 14.142 if cell is diagonal
182 |     else
183 |     {
184 |         return currentG + 14.142;
185 |     }
186 | }
187 | 
188 | //helper to calculate H heuristic - the manhatten distance from this cell to goal
189 | double getH(int x, int y)
190 | {
191 |     return (abs(goal.x - x) + abs(goal.y - y)) * 10;
192 | }
193 | 
194 | //helper to calculate F, but avoid integer rollover
195 | double getF(int g, int h)
196 | {
197 |     if (g == INT32_MAX)
198 |     {
199 |         return g;
200 |     }
201 |     else
202 |     {
203 |         return g + h;
204 |     }
205 | }
206 | 
207 | //deletes all waypoints between start and the furthest waypoint with a straight, unobstructed path
208 | void optimize(vector<cell> &path)
209 | {
210 |     //is there an obstacle between start and cell we're checking
211 |     bool obstacle_on_line = true;
212 | 
213 |     // path[0] = index of goal cell in path[]
214 |     int furthestFreeCell = 0;
215 | 
216 |     //starting at last goal (path[0]) and checking each waypoint until we find clear straight line to a cell
217 |     while (obstacle_on_line == true && path[furthestFreeCell++].index != path.back().index)
218 |     {
219 |         cout<<"furthest free cell = "<<path[furthestFreeCell].index<<" and val = "<<(int)_map->data[path[furthestFreeCell].index]<<endl;
220 |         //we're going to iterate between points. set our start and endpoints for iterating
221 |         int startX, endX, startY, endY;
222 |         if (start.x <= path[furthestFreeCell].x)
223 |         {
224 |             startX = start.x;
225 |             endX = path[furthestFreeCell].x;
226 |         }
227 |         else
228 |         {
229 |             startX = path[furthestFreeCell].x;
230 |             endX = start.x;
231 |         }
232 | 
233 |         if (start.y <= path[furthestFreeCell].y)
234 |         {
235 |             startY = start.y;
236 |             endY = path[furthestFreeCell].y;
237 |         }
238 |         else
239 |         {
240 |             startY = path[furthestFreeCell].y;
241 |             endY = start.y;
242 |         }
243 | 
244 |         //if any one thing in for loop detects an obstacle, this will be set back to true
245 |         //if no obstacles detected, we have found an unobstructed straight line to a waypoint
246 |         obstacle_on_line = false;
247 |         //check every y for every x in range
248 |         for (int x = startX; x != endX && obstacle_on_line == false; x++)
249 |         {
250 |             //make sure we don't try to calculate slope of vertical line where y is always the same
251 |             if (startY == endY)
252 |             {
253 |                 obstacle_on_line = is_obstacle(x, startY, _map);
254 |             }
255 |             else
256 |             {
257 |                 obstacle_on_line = is_obstacle(x, (int)get_y_intercept(start.x, start.y, path[furthestFreeCell].x, path[furthestFreeCell].y, x), _map);
258 |             }
259 |         }
260 |         //check every x for every y in range
261 |         for (int y = startY; y != endY && obstacle_on_line == false; y++)
262 |         {
263 |             //handle horizontal lines
264 |             if (startX == endX)
265 |             {
266 |                 obstacle_on_line = is_obstacle(startX, y, _map);
267 |             }
268 |             else
269 |             {
270 |                 obstacle_on_line = is_obstacle((int)get_x_intercept(start.x, start.y, path[furthestFreeCell].x, path[furthestFreeCell].y, y), y, _map);
271 |             }
272 |         }
273 |     }
274 |     //if in a gray area, optimize might think the closest straight path is the current location
275 |     //make sure at least on cell remains or planner will simply publish the start (current) and the robot won't move
276 |     if(furthestFreeCell == path.size()-1)
277 |     {
278 |         furthestFreeCell--;
279 |     }
280 |     cout << "FOUND FURTHEST STRAIGHT LINE FROM START TO waypoint at x, y = " << path[furthestFreeCell].x << ", " << path[furthestFreeCell].y << endl;
281 | 
282 |     //pop cells off waypoint list until we get to furthestFreeCell
283 |     while (path[furthestFreeCell].index != path.back().index)
284 |     {
285 |         path.pop_back();
286 |     }
287 |     //put start back for visualization waypoint list
288 |     path.push_back(cell(start));
289 | }
290 | //publish the next waypoint is 2d form, ignoring quaternion nature of PoseSatamped data type
291 | void publish_waypoint(cell nextWaypoint)
292 | {
293 |     geometry_msgs::PoseStamped waypoint;
294 |     waypoint.header.frame_id = "map";
295 |     waypoint.header.stamp = ros::Time::now();
296 |     //convert cell x, y coords to position in meters
297 |     waypoint.pose.position.x = (double)(nextWaypoint.x) / 10 + .05;
298 |     waypoint.pose.position.y = (double)(nextWaypoint.y) / 10 + .05;
299 |     waypoint.pose.position.z = 0;
300 |     waypoint.pose.orientation.x = 0;
301 |     waypoint.pose.orientation.y = 0;
302 |     waypoint.pose.orientation.z = nextWaypoint.theta;
303 |     waypoint.pose.orientation.w = 0;
304 | 
305 |     cout << "Publishing waypoint and theta " << waypoint.pose.position.x << ", " << waypoint.pose.position.y << "   " << waypoint.pose.orientation.z << endl;
306 |     pub.publish(waypoint);
307 | }
308 | 
309 | //optional - publish path for rviz
310 | void publishPathforRviz(vector<cell> &path)
311 | {
312 |     nav_msgs::Path waypoints;
313 |     waypoints.header.frame_id = "map";
314 |     waypoints.header.stamp = ros::Time::now();
315 |     waypoints.poses.resize(path.size());
316 | 
317 |     for (int i = 0; i < path.size(); i++)
318 |     {
319 |         //path finds integers of grid cells, we need to publish waypoints
320 |         //as doubles, and add .05 meters so we aim for middle of the cells
321 |         waypoints.poses[i].header.frame_id = "map";
322 |         waypoints.poses[i].header.stamp = ros::Time::now();
323 |         waypoints.poses[i].pose.position.x = (double)(path[i].x) / 10 + .05;
324 |         waypoints.poses[i].pose.position.y = (double)(path[i].y) / 10 + .05;
325 |         waypoints.poses[i].pose.position.z = 0;
326 |         waypoints.poses[i].pose.orientation.x = 0;
327 |         waypoints.poses[i].pose.orientation.y = 0;
328 |         waypoints.poses[i].pose.orientation.z = 0;
329 |         waypoints.poses[i].pose.orientation.w = 1;
330 |     }
331 |     pathPub.publish(waypoints);
332 | }
333 | 
334 | //trace the path through the closed list and return the next waypoint to be published
335 | int trace(vector<cell> &closed)
336 | {
337 |     vector<cell> path;
338 |     //closed.back() will be our goal, and will be element [0] in path
339 |     path.push_back(cell(closed.back()));
340 |     bool pathComplete = false;
341 |     while (pathComplete == false)
342 |     {
343 |         bool found = false;
344 |         //check the closed list for the parent cell of the last cell in path[]
345 |         for (int i = 0; found == false && i < closed.size(); i++)
346 |         {
347 |             //when we find the parent cell, push it to path and we will lool for its parent cell next
348 |             if (closed[i].x == path.back().prevX && closed[i].y == path.back().prevY)
349 |             {
350 |                 path.push_back(cell(closed[i]));
351 |                 found = true;
352 |             }
353 |         }
354 |         //when last element in path[] is the same as our start, we have the complete path
355 |         if (path.back().index == start.index)
356 |         {
357 |             pathComplete = true;
358 |         }
359 |     }
360 | 
361 |     //remove waypoints between start and whicever cell we cen get to going in a straght line
362 |     if(path.size() > 2)
363 |     {
364 |         optimize(path);
365 |     }
366 | 
367 |     //optionally call this function to publish path to visualize on rviz
368 |     publishPathforRviz(path);
369 | 
370 |     //the waypoint at back() is currently our start point. By removing it, the new back() will
371 |     //be our first waypoint - the one we need to publish to the drive controller.
372 |     if (path.back().index != goal.index)
373 |     {
374 |         path.pop_back();
375 |     }
376 | 
377 |     //if goal, publish goal heading, else publish the heading we took to get here anyway
378 |     if (path.back().index != path.front().index)
379 |     {
380 |         double deltaX = path.back().x - start.x;
381 |         double deltaY = path.back().y - start.y;
382 |         path.back().theta = atan2(deltaY, deltaX);
383 |     }
384 |     publish_waypoint(path.back());
385 |     return path.back().index;
386 | }
387 | 
388 | int find_path()
389 | {
390 |     vector<cell> open;
391 |     vector<cell> closed;
392 |     cell current(start);
393 |     //special case start G must be initialized to 0
394 |     current.G = 0;
395 |     current.H = getH(start.x, start.y);
396 |     current.F = current.G + current.H;
397 |     current.index = (getIndex(current.x, current.y, _map));
398 |     open.push_back(cell(current));
399 | 
400 |     //main loop cycles until .H = 0, which means we have found the goal
401 |     while (current.H > 0)
402 |     {
403 |         //iterate over the cells all around the current cell
404 |         for (int x = current.x - 1; x <= current.x + 1; x++)
405 |         {
406 |             for (int y = current.y - 1; y <= current.y + 1; y++)
407 |             {
408 |                 //don't check elements out of array bounds
409 |                 if (is_in_bounds(x, y, _map))
410 |                 {
411 |                     //when we run out of open elements, the must not be a path
412 |                     if (open.size() == 0)
413 |                     {
414 |                         cout << "NO PATH FOUND" << endl;
415 |                         goalActive = false;
416 |                         return -1;
417 |                     }
418 |                     //if in the open list, check for lower cost path to here
419 |                     if (contains(open, getIndex(x, y, _map)) == true)
420 |                     {
421 |                         //iterate the list until we find the relevant cell
422 |                         int i = 0;
423 |                         while (open[i].index != getIndex(x, y, _map))
424 |                         {
425 |                             i++;
426 |                         }
427 | 
428 |                         int tempG = getG(x, y, current.x, current.y, current.G);
429 |                         int tempH = getH(x, y);
430 |                         //if this calculation results in lower F cost, replace cells parents with current cell
431 |                         if (tempG + tempH < open[i].F && tempG != INT32_MAX)
432 |                         {
433 |                             open[i].F = tempG + tempH;
434 |                             open[i].G = tempG;
435 |                             open[i].prevX = current.x;
436 |                             open[i].prevY = current.y;
437 |                         }
438 |                     }
439 |                     //if this one not not in open or closed list, add it
440 |                     else if (contains(closed, getIndex(x, y, _map)) == false)
441 |                     {
442 |                         //create the cell object with current cell data
443 |                         cell newCell;
444 |                         newCell.x = x;
445 |                         newCell.y = y;
446 |                         newCell.index = getIndex(x, y, _map);
447 |                         newCell.prevX = current.x;
448 |                         newCell.prevY = current.y;
449 |                         //calc G dis froms start
450 |                         newCell.G = getG(x, y, current.x, current.y, current.G);
451 |                         //calc H to goal
452 |                         newCell.H = getH(x, y);
453 |                         //calc F = g+h
454 |                         if (getF(newCell.G, newCell.H) < INT32_MAX)
455 |                         {
456 |                             newCell.F = getF(newCell.G, newCell.H);
457 |                         }
458 |                         //add to closed list if obstacle, else add to open list
459 |                         if (newCell.F == INT32_MAX)
460 |                         {
461 |                             closed.push_back(cell(newCell));
462 |                         }
463 |                         else
464 |                         {
465 |                             open.push_back(newCell);
466 |                         }
467 |                     }
468 |                 }
469 |             }
470 |         }
471 | 
472 |         //when we've checked all neighbors, add current cell to closed list and remove from open
473 |         closed.push_back(cell(current));
474 |         bool found = false;
475 |         for (int i = 0; found == false; i++)
476 |         {
477 |             if (open[i].index == current.index)
478 |             {
479 |                 open.erase(open.begin() + i);
480 |                 found = true;
481 |             }
482 |         }
483 | 
484 |         //find the cell in open list with lowest f cost
485 |         int lowestF = 0;
486 |         for (int i = 0; i < open.size(); i++)
487 |         {
488 |             if (open[i].F < open[lowestF].F)
489 |             {
490 |                 lowestF = i;
491 |             }
492 |         }
493 | 
494 |         //now make the current = cell we found with lowest f cost
495 |         current.index = open[lowestF].index;
496 |         current.x = open[lowestF].x;
497 |         current.y = open[lowestF].y;
498 |         current.theta = open[lowestF].theta;
499 |         current.F = open[lowestF].F;
500 |         current.G = open[lowestF].G;
501 |         current.H = open[lowestF].H;
502 |         current.prevX = open[lowestF].prevX;
503 |         current.prevY = open[lowestF].prevY;
504 |     }
505 |     //at this point, we have found the goal. set goal's parents to last cell found
506 |     //then add to goal to closed list.
507 |     goal.prevX = closed.back().x;
508 |     goal.prevY = closed.back().y;
509 |     closed.push_back(cell(goal));
510 | 
511 |     //trace back through closed list
512 |     int nextWaypoint = trace(closed);
513 | 
514 |     ///////////////////TEMP____remove THIS setting false for moving robot//////////////////////////
515 |     // goalActive = false;
516 |     return nextWaypoint;
517 | }
518 | 
519 | int main(int argc, char **argv)
520 | {
521 |     ros::init(argc, argv, "path_planner");
522 |     ros::NodeHandle node;
523 | 
524 |     //start tf::listener early so it has time to fill buffer
525 |     update_start_cell();
526 | 
527 |     //subscribe to _map and goal location
528 |     subMap = node.subscribe("costmap", 0, map_handler);
529 |     subGoal = node.subscribe("goal_2d", 0, set_goal);
530 | 
531 |     //advertise publisher
532 |     pub = node.advertise<geometry_msgs::PoseStamped>("waypoint_2d", 0);
533 |     pathPub = node.advertise<nav_msgs::Path>("path", 0);
534 | 
535 |     ros::Rate loop_rate(1);
536 |     while (ros::ok())
537 |     {
538 |         if (goalActive == true)
539 |         {
540 |             if(update_start_cell() == false)
541 |             {
542 |                 start.index = 0;
543 |                 goal.index = 0;
544 |             }
545 |                  cout<< "start and goal = "
546 |                  << start.x << ", " << start.y << "......" << goal.x << ", " << goal.y << endl;
547 | 
548 |             //we have . Stop until we receive a new goal
549 |             if (start.index == goal.index)
550 |             {
551 |                 cout << "Arrived, goalActive set false" << endl;
552 |                 publish_waypoint(goal);
553 |                 goalActive = false;
554 |             }
555 |             // keep updating path every loop_rate()
556 |             else
557 |             {
558 |                 int nextWaypoint = find_path();
559 |                 if (nextWaypoint == -1)
560 |                 {
561 |                     cout << "NO PATH FOUND" << endl;
562 |                     goalActive = false;
563 |                 }
564 |             }
565 |         }
566 |         ros::spinOnce();
567 |         loop_rate.sleep();
568 |     }
569 | 
570 |     return 0;
571 | }
572 | 


--------------------------------------------------------------------------------
/chapter13/practical_common.h:
--------------------------------------------------------------------------------
  1 | #ifndef PRACTICAL_COMMON_H_
  2 | #define PRACTICAL_COMMON_H_
  3 | #include "nav_msgs/OccupancyGrid.h"
  4 | #include <math.h>
  5 | #include<iostream>
  6 | 
  7 | 
  8 | 
  9 | //cells from other sources set above this will be considered 100% occupied
 10 | const int OCCUPIED_THRESHOLD = 50;
 11 | 
 12 | // distance from center of robot to furthest exterior point, in meters
 13 | const double ROBOT_RADIUS = .2;
 14 | 
 15 | 
 16 | //3 helper functions to get x, y coordinates from index,
 17 | //and index from x, y coordinates, based on  index = ogm.info.width * y + x
 18 | int getX(int index, const nav_msgs::OccupancyGridPtr &map)
 19 | {
 20 |     return index % map->info.width;
 21 | }
 22 | int getY(int index, const nav_msgs::OccupancyGridPtr &map)
 23 | {
 24 |     return index / map->info.width;
 25 | }
 26 | int getIndex(int x, int y, const nav_msgs::OccupancyGridPtr &map)
 27 | {
 28 |     return map->info.width * y + x;
 29 | }
 30 | 
 31 | 
 32 | //helper function to stay in map boundaries and avoid segfaults
 33 | bool is_in_bounds(int x, int y, const nav_msgs::OccupancyGridPtr &map)
 34 | {
 35 |     return (x >= 0 && x < map->info.width && y >= 0 && y < map->info.height);
 36 | }
 37 | 
 38 | //helper to check if cell is marked unknown
 39 | bool is_unknown(int x, int y, const nav_msgs::OccupancyGridPtr &map)
 40 | {
 41 |     return ((int)map->data[getIndex(x, y, map)] == -1);
 42 | }
 43 | 
 44 | 
 45 | //helper to check if cell is to be considered an obstacle - includes cells marked unknown
 46 | bool is_obstacle(int x, int y, const nav_msgs::OccupancyGridPtr &map)
 47 | {
 48 |  //   std::cout<<x<<", "<<y<< " .... "<<(int)map->data[getIndex(x, y, map)]<<std::endl;
 49 |     return ((int)map->data[getIndex(x, y, map)] > OCCUPIED_THRESHOLD) || is_unknown(x, y, map);
 50 | }
 51 | 
 52 | 
 53 | //helper to return map resolution
 54 | double map_resolution(const nav_msgs::OccupancyGridPtr &map)
 55 | {
 56 |     return map->info.resolution;
 57 | }
 58 | 
 59 | //returns slope m from slope intercept formula y=m*x+b from two coordinate pairs
 60 | //don't forget all coordinates must be either pose in meters or grid cells numbers
 61 | // ( grid cell number = pose(in meters) / map_resolution  )
 62 | //DO NOT MIX POSE COORDINATES WITH GRID CELL COORDINATES - MAKE ALL THE SAME
 63 | double get_m(double x1, double y1, double x2, double y2)
 64 | {
 65 |     //****CAUTION< WILL THROW ERROR IF WE DIVIDE BY ZERO
 66 |     return (y1 - y2) / (x1 - x2);
 67 | }
 68 | 
 69 | // b as is the offset from slope intercept formula y=m*x+b
 70 | //for b = y-(m*x)
 71 | //DO NOT MIX POSE COORDINATES WITH GRID CELL COORDINATES - MAKE ALL THE SAME
 72 | double get_b(double x1, double y1, double x2, double y2)
 73 | {
 74 |   //  cannot divide by zer0
 75 |     if(x1 != x2)
 76 |     {
 77 |     return y1 - (get_m(x1, y1, x2, y2) * x1);
 78 |     }
 79 |     else return x1; //line is vertical, so b = x1
 80 | }
 81 | 
 82 | //returns where y falls on a line between two supplied points, for the given x
 83 | //returns Y from slope intercept forumula y=m*x+b, given x
 84 | //DO NOT MIX POSE COORDINATES WITH GRID CELL COORDINATES - MAKE ALL THE SAME
 85 | //****DOES NOT HANDLE VERTICAL LINES****
 86 | double get_y_intercept(double x1, double y1, double x2, double y2, double checkX)
 87 | {
 88 |     double m = get_m(x1, y1, x2, y2);
 89 |     double b = get_b(x1, y1, x2, y2);
 90 |     return m * checkX + b;
 91 | }
 92 | 
 93 | //returns where y falls on a line between two supplied points, for the given y
 94 | //returns x from slope intercept forumula y=m*x+b, given y. for x= (y-b)/m
 95 | //DO NOT MIX POSE COORDINATES WITH GRID CELL COORDINATES - MAKE ALL THE SAME
 96 | //****DOES NOT HANDLE VERTICAL LINES****
 97 | double get_x_intercept(double x1, double y1, double x2, double y2, double checkY)
 98 | {
 99 | 
100 |     double m = get_m(x1, y1, x2, y2);
101 |     double b = get_b(x1, y1, x2, y2);
102 |     return (checkY - b) / m;
103 | }
104 | 
105 | 
106 | #endif /* PRACTICAL_COMMON_H_ */
107 | 


--------------------------------------------------------------------------------
/chapter14/tick_publisher.cpp:
--------------------------------------------------------------------------------
  1 | /*
  2 | *This is a ROS node that monitors a pair of hall effect encoders and publishes
  3 | *the tick counts for a left wheel and right wheel in ROS. Whether each
  4 | *GPIO event is incremented or decremented is determined by check the direction
  5 | *signal going to the motor driver. This is written simply
  6 | *to be readable for all levels and accompanies the book Practical Robotics in C++.
  7 | *
  8 | *Author: Lloyd Brombach (lbrombach2@gmail.com)
  9 | *11/7/2019
 10 | */
 11 | 
 12 | #include "ros/ros.h"
 13 | #include "std_msgs/Int16.h"
 14 | #include <pigpiod_if2.h>
 15 | #include <iostream>
 16 | 
 17 | using namespace std;
 18 | 
 19 | //GPIO Pin assignments
 20 | const int leftEncoder = 22; //left encoder
 21 | const int rightEncoder = 23; //right encoder
 22 | const int leftReverse = 13; //monitor as input that goes low when left motor set to reverse
 23 | const int rightReverse = 19; //monitor as input that goes low when right motor set to reverse
 24 | 
 25 | //max and min allowable values
 26 | const int encoderMin = -32768;
 27 | const int encoderMax = 32768;
 28 | 
 29 | std_msgs::Int16 leftCount;
 30 | std_msgs::Int16 rightCount;
 31 | 
 32 | 
 33 | //this is the callback function that runs when a change of state happens on the monitored gpio pin
 34 | void left_event(int pi, unsigned int gpio, unsigned int edge, unsigned int tick)
 35 | {
 36 | if(gpio_read(pi, leftReverse)==0) //decrement if motor commanded to reverse
 37 |     {
 38 |      if(leftCount.data==encoderMin) //handle rollunder
 39 |      {
 40 |       leftCount.data = encoderMax;
 41 |      }
 42 |      else
 43 |      {
 44 |       leftCount.data--;
 45 |      }
 46 | 
 47 |     }
 48 | else  //increment if not commanded to reverse (must be going forward)
 49 |     {
 50 |      if(leftCount.data==encoderMax) //handle rollover
 51 |      {
 52 |       leftCount.data = encoderMin;
 53 |      }
 54 |      else
 55 |      {
 56 |       leftCount.data++;
 57 |      }
 58 | 
 59 |     }
 60 | 
 61 | }
 62 | //this is the callback function that runs when a change of state happens on the monitored gpio pin
 63 | void right_event(int pi, unsigned int gpio, unsigned int edge, unsigned int tick)
 64 | {
 65 | if(gpio_read(pi, rightReverse)==0)
 66 |     {
 67 |      if(rightCount.data==encoderMin)
 68 |      {
 69 |       rightCount.data = encoderMax;
 70 |      }
 71 |      else
 72 |      {
 73 |       rightCount.data--;
 74 |      }
 75 |     }
 76 | else
 77 |     {
 78 |      if(rightCount.data==encoderMax)
 79 |       {
 80 |        rightCount.data = encoderMin;
 81 |       }
 82 |       else
 83 |       {
 84 |        rightCount.data++;
 85 |       }
 86 |     }
 87 | 
 88 | }
 89 | 
 90 | int PigpioSetup()
 91 | {
 92 |     char *addrStr = NULL;
 93 |     char *portStr = NULL;
 94 |     int pi = pigpio_start(addrStr, portStr);
 95 | 
 96 |     //set the mode and pullup to read the encoder like a switch
 97 |     set_mode(pi, leftEncoder, PI_INPUT);
 98 |     set_mode(pi, rightEncoder, PI_INPUT);
 99 |     set_mode(pi, leftReverse, PI_INPUT);
100 |     set_mode(pi, rightReverse, PI_INPUT);
101 |     set_pull_up_down(pi, leftEncoder, PI_PUD_UP);
102 |     set_pull_up_down(pi, rightEncoder, PI_PUD_UP);
103 |     set_pull_up_down(pi, leftReverse, PI_PUD_UP);
104 |     set_pull_up_down(pi, rightReverse, PI_PUD_UP);
105 | 
106 |     return pi;
107 | }
108 | 
109 | int main(int argc, char **argv)
110 | {
111 |     //initialize pipiod interface
112 |     int pi = PigpioSetup();
113 |     if(pi>=0)
114 |     {
115 |         cout<<"daemon interface started ok at "<<pi<<endl;
116 |     }
117 |     else
118 |     {
119 |         cout<<"Failed to connect to PIGPIO Daemon - is it running?"<<endl;
120 |         return -1;
121 |     }
122 | 
123 | 
124 |     //initializes callbacks
125 |     int cbLeft=callback(pi, leftEncoder,EITHER_EDGE, left_event);
126 |     int cbRight=callback(pi, rightEncoder, EITHER_EDGE, right_event);
127 | 
128 |     //normal ROS node setup: Register node with master,  advertise publishers
129 |     ros::init(argc, argv, "tick_publisher");
130 |     ros::NodeHandle node;
131 |     ros::Publisher pubLeft = node.advertise<std_msgs::Int16>("leftWheel", 1000);
132 |     ros::Publisher pubRight = node.advertise<std_msgs::Int16>("rightWheel", 1000);
133 | 
134 | 
135 |     ros::Rate loop_rate(30);
136 |     while(ros::ok())
137 |     {
138 |         pubLeft.publish(leftCount);
139 |         pubRight.publish(rightCount);
140 |         ros::spinOnce();
141 | 
142 |         loop_rate.sleep();
143 |     }
144 |     //terminate callbacks and pigpiod connectoin to release daemon resources
145 |     callback_cancel(cbLeft);
146 |     callback_cancel(cbRight);
147 |     pigpio_stop(pi);
148 |     return 0;
149 | }
150 | 


--------------------------------------------------------------------------------
/chapter15/ultrasonic_publisher.cpp:
--------------------------------------------------------------------------------
  1 | /*
  2 | *This is a ROS node that reads an HC-SR04 ultrasonic range sensor
  3 | *then publishes a ROS sensor_msgs::Range message. This is written
  4 | *to be readable for all levels and accompanies the book Practical Robotics in C++.
  5 | *
  6 | *Author: Lloyd Brombach (lbrombach2@gmail.com)
  7 | *11/7/2019
  8 | */
  9 | 
 10 | #include "ros/ros.h"
 11 | #include "sensor_msgs/Range.h"
 12 | #include <pigpiod_if2.h>
 13 | #include <iostream>
 14 | 
 15 | //assign alias to the gpio pin numbers
 16 | const int trigger =  6;
 17 | const int echo = 16;
 18 | 
 19 | sensor_msgs::Range range;
 20 | 
 21 | using namespace std;
 22 | 
 23 | //Returns range in centimers
 24 | float get_range(int pi)
 25 | {
 26 | //initiate reading with 10 microsecond pulse to trigger
 27 | gpio_write(pi, trigger, 1);
 28 | time_sleep(.00001);
 29 | gpio_write(pi, trigger, 0);
 30 | 
 31 | //wait for echo pin to go from low to high
 32 | while(gpio_read(pi, echo)==0){};
 33 | 
 34 | //get current tick (microseconds since boot) from system.
 35 | int start = get_current_tick(pi);
 36 | 
 37 | //wait for echo pin to return to return to low
 38 | while(gpio_read(pi, echo)==1){};
 39 | 
 40 | //calculate duration of high pulse, which is equal to round trip time
 41 | int echoTime = get_current_tick(pi) - start;
 42 | 
 43 | //speed of sound is 343 m/s, but total echo time is round trip
 44 | //half that times echo time (in seconds) is the range
 45 | return 171.5 * echoTime / 1000000;
 46 | }
 47 | 
 48 | //connects with pigpio daemon, intializes gpio pin modes and states
 49 | int PigpioSetup()
 50 | {
 51 |   char *addrStr = NULL;
 52 |   char *portStr = NULL;
 53 |   int pi = pigpio_start(addrStr, portStr);
 54 | 
 55 |   //set the pin modes and set the trigger output to low
 56 |   set_mode(pi, echo, PI_INPUT);
 57 |   set_mode(pi, trigger, PI_OUTPUT);
 58 |   gpio_write(pi, trigger, 0);
 59 | 
 60 |   //let pins stabilize before trying to read
 61 |   time_sleep(.01);
 62 | 
 63 |   return pi;
 64 | }
 65 | 
 66 | int main(int argc, char **argv)
 67 | {
 68 |   //normal ROS node setup: Register node with master,  advertise publisher
 69 |   ros::init(argc, argv, "ultrasonic_publisher");
 70 |   ros::NodeHandle node;
 71 |   ros::Publisher pub = node.advertise<sensor_msgs::Range>("ultra_range", 0);
 72 | 
 73 |   //initialize pipiod interface
 74 |   int pi = PigpioSetup();
 75 |   if(pi>=0)
 76 |   {
 77 |     cout<<"daemon interface started ok at "<<pi<<endl;
 78 |   }
 79 |   else
 80 |   {
 81 |     cout<<"Failed to connect to PIGPIO Daemon - is it running?"<<endl;
 82 |     return -1;
 83 |   }
 84 | 
 85 |   //set our range message fixed data
 86 |   range.header.frame_id = "sonar";
 87 |   //The message has enums for radiation_type where 0 = ultrasonic and 1= infrared
 88 |   range.radiation_type = 0;
 89 |   range.field_of_view = .35;
 90 |   range.min_range = .02;
 91 |   range.max_range = 4.0;
 92 | 
 93 | 
 94 |   ros::Rate loop_rate(10);
 95 |   while(ros::ok())
 96 |   {
 97 |     range.header.stamp = ros::Time::now();
 98 |     range.range = get_range(pi);
 99 |     pub.publish(range);
100 |     loop_rate.sleep();
101 |   }
102 | 
103 |   pigpio_stop(pi);
104 |   return 0;
105 | }
106 | 


--------------------------------------------------------------------------------
/chapter16/lsm303_l3gd20h_imu_pub.cpp:
--------------------------------------------------------------------------------
  1 | /*
  2 | *This is a ROS node that that reads and publishes
  3 | *an LSM303DLHC accelerometer and L3GD20 gyroscope in ROS. This is written simply
  4 | *to be readable for all levels and accompanies lessons int the book Practical Robotics in C++.
  5 | *
  6 | *Author: Lloyd Brombach (lbrombach2@gmail.com)
  7 | *11/7/2019
  8 | */
  9 | 
 10 | #include "ros/ros.h"
 11 | #include "sensor_msgs/Imu.h"
 12 | #include <pigpiod_if2.h>
 13 | #include <iostream>
 14 | 
 15 | const int LSM303_accel=0x19; //accelerometer address
 16 | const int LSM303_mag  =0x1e; //magnetometer address
 17 | const int L3GD20_gyro = 0x6b; //gyroscope address
 18 | const int I2Cbus=1; //RPi typical I2C bus. Find yours with "ls /dev/*i2c* "
 19 | const float RAD_PER_DEG = 0.0174533;
 20 | 
 21 | int pi = -1;
 22 | int ACCEL_HANDLE=-1;
 23 | int GYRO_HANDLE=-1;
 24 | 
 25 | //create Imu message object
 26 | sensor_msgs::Imu myImu;
 27 | 
 28 | using namespace std;
 29 | 
 30 | 
 31 | 
 32 | void gyro_setup()
 33 | {
 34 |     //initiate comms with gyroscope and get handle
 35 |     GYRO_HANDLE=i2c_open(pi,I2Cbus, L3GD20_gyro,0);
 36 |     if (GYRO_HANDLE>=0)
 37 |     {
 38 |       cout<<"Gyro found. Handle = "<<GYRO_HANDLE<<endl;
 39 |     }
 40 |     else
 41 |     {
 42 |       cout<<"Unable to open I2C comms with Gyro"<<endl;
 43 |     }
 44 | 
 45 | 
 46 |     time_sleep(.05);
 47 | 
 48 |     i2c_write_byte_data(pi, GYRO_HANDLE, 0x20, 95); //set frequency
 49 |     time_sleep(.05);
 50 | }
 51 | 
 52 | void get_gyro()
 53 | {
 54 |     //readings in default scale of 245 degrees per sec
 55 |     //sensitivity at 245 deg/sec is .00875 degrees per digit
 56 |     int xLSB = (int)i2c_read_byte_data(pi, GYRO_HANDLE, 0x28);
 57 |     int xMSB = (int)i2c_read_byte_data(pi, GYRO_HANDLE, 0x29);
 58 |     myImu.angular_velocity.x=(float)((int16_t)(xLSB | xMSB<<8))*.00875*RAD_PER_DEG;
 59 | 
 60 |     int yLSB = (int)i2c_read_byte_data(pi, GYRO_HANDLE, 0x2A);
 61 |     int yMSB = (int)i2c_read_byte_data(pi, GYRO_HANDLE, 0x2B);
 62 |     myImu.angular_velocity.y=(float)((int16_t)(yLSB | yMSB<<8))*.00875*RAD_PER_DEG;
 63 | 
 64 |     int zLSB = (int)i2c_read_byte_data(pi, GYRO_HANDLE, 0x2C);
 65 |     int zMSB = (int)i2c_read_byte_data(pi, GYRO_HANDLE, 0x2D);
 66 |     myImu.angular_velocity.z=(float)((int16_t)(zLSB | zMSB<<8))*.00875*RAD_PER_DEG;
 67 | }
 68 | 
 69 | void accel_setup()
 70 | {
 71 |     //initiate comms with accelerometer and get handle
 72 |     ACCEL_HANDLE=i2c_open(pi,I2Cbus, LSM303_accel,0);
 73 |     if (ACCEL_HANDLE>=0)
 74 |     {
 75 |         cout<<"Accelerometer found. Handle = "<<ACCEL_HANDLE<<endl;
 76 |     }
 77 |     else
 78 |     {
 79 |         cout<<"Unable to open I2C comms with Accelerometer"<<endl;
 80 |     }
 81 | 
 82 |     i2c_write_byte_data(pi, ACCEL_HANDLE, 0x20, 0x47); //set frequency
 83 |     time_sleep(.02);
 84 |     i2c_write_byte_data(pi, ACCEL_HANDLE, 0x23, 0x09); //continuous update, LSB at lower addr, +- 2g, Hi-Res disable
 85 |     time_sleep(.02);
 86 | }
 87 | 
 88 | void get_accel()
 89 | {
 90 |     int xLSB = (int)i2c_read_byte_data(pi, ACCEL_HANDLE, 0x28);
 91 |     int xMSB = (int)i2c_read_byte_data(pi, ACCEL_HANDLE, 0x29);
 92 |     //12 bits resolution, MSB right-jusified, then convert to Tesla
 93 |     myImu.linear_acceleration.x=(float)((int16_t)(xLSB | xMSB<<8)>>4)/1000*9.81;
 94 | 
 95 |     int yLSB = (int)i2c_read_byte_data(pi, ACCEL_HANDLE, 0x2A);
 96 |     int yMSB = (int)i2c_read_byte_data(pi, ACCEL_HANDLE, 0x2B);
 97 |     myImu.linear_acceleration.y=(float)((int16_t)(yLSB | yMSB<<8)>>4)/1000*9.81;
 98 | 
 99 |     int zLSB = (int)i2c_read_byte_data(pi, ACCEL_HANDLE, 0x2C);
100 |     int zMSB = (int)i2c_read_byte_data(pi, ACCEL_HANDLE, 0x2D);
101 |     myImu.linear_acceleration.z=(float)((int16_t)(zLSB | zMSB<<8)>>4)/1000*9.81;
102 | 
103 |     myImu.header.stamp = ros::Time::now();
104 | }
105 | 
106 | 
107 | int PigpioSetup()
108 | {
109 |   char *addrStr = NULL;
110 |   char *portStr = NULL;
111 |   pi = pigpio_start(addrStr, portStr);
112 |   return pi;
113 | }
114 | 
115 | 
116 | int main(int argc, char **argv)
117 | {
118 |   ros::init(argc, argv, "imu_publisher");
119 |   ros::NodeHandle node;
120 |   ros::Publisher pub = node.advertise<sensor_msgs::Imu>("imu/data_raw", 0);
121 | 
122 |   //initialize pipiod interface
123 |   int pi = PigpioSetup();
124 |   if(pi>=0)
125 |   {
126 |     cout<<"daemon interface started ok at "<<pi<<endl;
127 |   }
128 |   else
129 |   {
130 |     cout<<"Failed to connect to PIGPIO Daemon - is it running?"<<endl;
131 |     return -1;
132 |   }
133 |   accel_setup();
134 |   gyro_setup();
135 |   //set our range message fixed data
136 |   myImu.header.frame_id = "imu";
137 | 
138 |   //set accel covariance to generic ballpark guess.
139 |     for(int i = 0; i<9; i++)
140 |     {
141 |         if(i==0 || i==4 || i==8)
142 |         {
143 |             myImu.linear_acceleration_covariance[i]=.0001;
144 |             myImu.angular_velocity_covariance[i] = .0001;
145 |         }
146 |         else
147 |         {
148 |             myImu.linear_acceleration_covariance[i]=0;
149 |             myImu.angular_velocity_covariance[i] = 0;
150 |         }
151 |         myImu.orientation_covariance[i] = 0;
152 |     }
153 |   //set absolute orientaion and angular velocity to "Do not use"
154 |   myImu.orientation_covariance[0] = -1;
155 | 
156 |   ros::Rate loop_rate(10);
157 |   while(ros::ok())
158 |   {
159 |     get_accel();
160 |     get_gyro();
161 |     pub.publish(myImu);
162 |     loop_rate.sleep();
163 |   }
164 | 
165 |   i2c_write_byte_data(pi, ACCEL_HANDLE, 0x20, 0x00);//puts accelerometer into sleep mode
166 |   i2c_close(pi, ACCEL_HANDLE);
167 |   i2c_write_byte_data(pi, GYRO_HANDLE, 0x20, 0);//puts gyro to sleep mode
168 |   i2c_close(pi, GYRO_HANDLE);
169 | 
170 |   pigpio_stop(pi);
171 |   return 0;
172 | }
173 | 


--------------------------------------------------------------------------------
/chapter16/lsm303_l3gd20h_imu_w_mag.cpp:
--------------------------------------------------------------------------------
  1 | /*
  2 | *This is a ROS node that that reads and publishes an LSM303 magnetometer in addition to
  3 | *an LSM303DLHC accelerometer and L3GD20 gyroscope in ROS. This is written simply
  4 | *to be readable for all levels and accompanies lessons in the book Practical Robotics in C++.
  5 | *
  6 | *Author: Lloyd Brombach (lbrombach2@gmail.com)
  7 | *11/7/2019
  8 | */
  9 | 
 10 | #include "ros/ros.h"
 11 | #include "sensor_msgs/Imu.h"
 12 | #include "sensor_msgs/MagneticField.h"
 13 | #include <pigpiod_if2.h>
 14 | #include <iostream>
 15 | 
 16 | const int LSM303_accel=0x19; //accelerometer address
 17 | const int LSM303_mag  =0x1e;
 18 | const int L3GD20_gyro = 0x6b;
 19 | const int I2Cbus=1; //RPi typical I2C bus. Find yours with "ls /dev/*i2c* "
 20 | const float RAD_PER_DEG = 0.0174533;
 21 | const float TESLA_PER_GAUSS = .0001;
 22 | 
 23 | int pi = -1;
 24 | int ACCEL_HANDLE=-1;
 25 | int GYRO_HANDLE=-1;
 26 | int MAG_HANDLE=-1;
 27 | 
 28 | sensor_msgs::Imu myImu;
 29 | sensor_msgs::MagneticField mag;
 30 | 
 31 | using namespace std;
 32 | 
 33 | 
 34 | void mag_setup()
 35 | {
 36 |     //initiate comms with magnetometer and get handle
 37 |     MAG_HANDLE=i2c_open(pi,I2Cbus, LSM303_mag   ,0);
 38 |     if (MAG_HANDLE>=0)
 39 |     {
 40 |       cout<<"MagnetometerLSM303_mag found. Handle = "<<MAG_HANDLE<<endl;
 41 |     }
 42 |     else
 43 |     {
 44 |       cout<<"Unable to open I2C comms with Magnetometer"<<endl;
 45 |     }
 46 |     time_sleep(.05);
 47 | 
 48 |     i2c_write_byte_data(pi, MAG_HANDLE, 0x00, 16); //set frequency
 49 |     time_sleep(.05);
 50 |     //set scale to +- 1.3 gauss h
 51 |     i2c_write_byte_data(pi, MAG_HANDLE, 0x01, 32);
 52 |     time_sleep(.05);
 53 |     //set mode - 0 for continuous, 1 for single, 3 for off
 54 |     i2c_write_byte_data(pi, MAG_HANDLE, 0x02, 0);
 55 |     time_sleep(.05);
 56 | 
 57 |     cout<<"mag gain setting at reg 0x01 = "<<(int)i2c_read_byte_data(pi, MAG_HANDLE, 0x01)<<endl;
 58 | }
 59 | 
 60 | void get_mag()
 61 | {
 62 |     //readings in default scale of +/- 1.3Gauss
 63 |     // x,y gain at that scale ==1100, z gain = 980
 64 |     int xLSB = (int)i2c_read_byte_data(pi, MAG_HANDLE, 0x04);
 65 |     int xMSB = (int)i2c_read_byte_data(pi, MAG_HANDLE, 0x03);
 66 |     mag.magnetic_field.x=(float)((int16_t)(xLSB | xMSB<<8))/1100*TESLA_PER_GAUSS;
 67 | 
 68 |     int yLSB = (int)i2c_read_byte_data(pi, MAG_HANDLE, 0x08);
 69 |     int yMSB = (int)i2c_read_byte_data(pi, MAG_HANDLE, 0x07);
 70 |     mag.magnetic_field.y=(float)((int16_t)(yLSB | yMSB<<8))/1100*TESLA_PER_GAUSS;
 71 | 
 72 |     int zLSB = (int)i2c_read_byte_data(pi, MAG_HANDLE, 0x06);
 73 |     int zMSB = (int)i2c_read_byte_data(pi, MAG_HANDLE, 0x05);
 74 |     mag.magnetic_field.z=(float)((int16_t)(zLSB | zMSB<<8))/980*TESLA_PER_GAUSS;
 75 | }
 76 | 
 77 | void gyro_setup()
 78 | {
 79 |     //initiate comms with gyroscope and get handle
 80 |     GYRO_HANDLE=i2c_open(pi,I2Cbus, L3GD20_gyro,0);
 81 |     if (GYRO_HANDLE>=0)
 82 |     {
 83 |       cout<<"Gyro found. Handle = "<<GYRO_HANDLE<<endl;
 84 |     }
 85 |     else
 86 |     {
 87 |       cout<<"Unable to open I2C comms with Gyro"<<endl;
 88 |     }
 89 | 
 90 | 
 91 |     time_sleep(.05);
 92 | 
 93 |     i2c_write_byte_data(pi, GYRO_HANDLE, 0x20, 95); //set frequency
 94 |     time_sleep(.05);
 95 |     cout<<+(int)i2c_read_byte_data(pi, GYRO_HANDLE, 0x20)<<endl;
 96 | }
 97 | 
 98 | void get_gyro()
 99 | {
100 |     //readings in default scale of 245 degrees per sec
101 |     //sensitivity at 245 deg/sec is .00875 degrees per digit
102 |     int xLSB = (int)i2c_read_byte_data(pi, GYRO_HANDLE, 0x28);
103 |     int xMSB = (int)i2c_read_byte_data(pi, GYRO_HANDLE, 0x29);
104 |     myImu.angular_velocity.x=(float)((int16_t)(xLSB | xMSB<<8))*.00875*RAD_PER_DEG;
105 | 
106 |     int yLSB = (int)i2c_read_byte_data(pi, GYRO_HANDLE, 0x2A);
107 |     int yMSB = (int)i2c_read_byte_data(pi, GYRO_HANDLE, 0x2B);
108 |     myImu.angular_velocity.y=(float)((int16_t)(yLSB | yMSB<<8))*.00875*RAD_PER_DEG;
109 | 
110 |     int zLSB = (int)i2c_read_byte_data(pi, GYRO_HANDLE, 0x2C);
111 |     int zMSB = (int)i2c_read_byte_data(pi, GYRO_HANDLE, 0x2D);
112 |     myImu.angular_velocity.z=(float)((int16_t)(zLSB | zMSB<<8))*.00875*RAD_PER_DEG;
113 | }
114 | 
115 | void accel_setup()
116 | {
117 |     //initiate comms with accelerometer and get handle
118 |     ACCEL_HANDLE=i2c_open(pi,I2Cbus, LSM303_accel,0);
119 |     if (ACCEL_HANDLE>=0)
120 |     {
121 |         cout<<"Accelerometer found. Handle = "<<ACCEL_HANDLE<<endl;
122 |     }
123 |     else
124 |     {
125 |         cout<<"Unable to open I2C comms with Accelerometer"<<endl;
126 |     }
127 | 
128 |     i2c_write_byte_data(pi, ACCEL_HANDLE, 0x20, 0x47); //set frequency
129 |     time_sleep(.02);
130 |     i2c_write_byte_data(pi, ACCEL_HANDLE, 0x23, 0x09); //continuous update, LSB at lower addr, +- 2g, Hi-Res disable
131 |     time_sleep(.02);
132 | }
133 | 
134 | void get_accel()
135 | {
136 |     int xLSB = (int)i2c_read_byte_data(pi, ACCEL_HANDLE, 0x28);
137 |     int xMSB = (int)i2c_read_byte_data(pi, ACCEL_HANDLE, 0x29);
138 |     //12 bits resolution, MSB right-jusified, then convert to Tesla
139 |     myImu.linear_acceleration.x=(float)((int16_t)(xLSB | xMSB<<8)>>4)/1000*9.81;
140 | 
141 |     int yLSB = (int)i2c_read_byte_data(pi, ACCEL_HANDLE, 0x2A);
142 |     int yMSB = (int)i2c_read_byte_data(pi, ACCEL_HANDLE, 0x2B);
143 |     myImu.linear_acceleration.y=(float)((int16_t)(yLSB | yMSB<<8)>>4)/1000*9.81;
144 | 
145 |     int zLSB = (int)i2c_read_byte_data(pi, ACCEL_HANDLE, 0x2C);
146 |     int zMSB = (int)i2c_read_byte_data(pi, ACCEL_HANDLE, 0x2D);
147 |     myImu.linear_acceleration.z=(float)((int16_t)(zLSB | zMSB<<8)>>4)/1000*9.81;
148 | 
149 |     myImu.header.stamp = ros::Time::now();
150 | }
151 | 
152 | 
153 | int PigpioSetup()
154 | {
155 |   char *addrStr = NULL;
156 |   char *portStr = NULL;
157 |   pi = pigpio_start(addrStr, portStr);
158 |   return pi;
159 | }
160 | 
161 | 
162 | int main(int argc, char **argv)
163 | {
164 |   ros::init(argc, argv, "imu_publisher");
165 |   ros::NodeHandle node;
166 |   ros::Publisher pub = node.advertise<sensor_msgs::Imu>("imu/data_raw", 0);
167 |   ros::Publisher pub2 = node.advertise<sensor_msgs::MagneticField>("imu/mag", 0);
168 | 
169 |   //initialize pipiod interface
170 |   int pi = PigpioSetup();
171 |   if(pi>=0)
172 |   {
173 |     cout<<"daemon interface started ok at "<<pi<<endl;
174 |   }
175 |   else
176 |   {
177 |     cout<<"Failed to connect to PIGPIO Daemon - is it running?"<<endl;
178 |     return -1;
179 |   }
180 |   accel_setup();
181 |   gyro_setup();
182 |   mag_setup();
183 |   //set our range message fixed data
184 |   myImu.header.frame_id = "imu";
185 |   mag.header.frame_id = "imu";
186 |   //set absolute orientaion and angular velocity to unknown
187 |   myImu.orientation_covariance[0] = -1;
188 |   //set accel covariance to unknown
189 |   for(int i = 0; i<9; i++)
190 |   {
191 |     myImu.linear_acceleration_covariance[i]=0;
192 |     myImu.angular_velocity_covariance[i]=0;
193 |     mag.magnetic_field_covariance[i]=0;
194 |   }
195 | 
196 | 
197 |   ros::Rate loop_rate(10);
198 |   while(ros::ok())
199 |   {
200 |     get_accel();
201 |     get_gyro();
202 |     get_mag();
203 |     pub.publish(myImu);
204 |     pub2.publish(mag);
205 |     loop_rate.sleep();
206 |   }
207 | 
208 |   i2c_write_byte_data(pi, ACCEL_HANDLE, 0x20, 0x00);//puts accelerometer into sleep mode
209 |   i2c_close(pi, ACCEL_HANDLE);
210 |   i2c_write_byte_data(pi, GYRO_HANDLE, 0x20, 0);//puts gyro to sleep mode
211 |   i2c_close(pi, GYRO_HANDLE);
212 |   i2c_write_byte_data(pi, MAG_HANDLE, 0x02, 3);//puts mag to sleep
213 |   i2c_close(pi, MAG_HANDLE);
214 | 
215 |   pigpio_stop(pi);
216 |   return 0;
217 | }
218 | 


--------------------------------------------------------------------------------
/chapter19/cv_bridge_test.cpp:
--------------------------------------------------------------------------------
 1 | #include <ros/ros.h>
 2 | #include <image_transport/image_transport.h>
 3 | #include <cv_bridge/cv_bridge.h>
 4 | #include <sensor_msgs/image_encodings.h>
 5 | #include <opencv2/core/core.hpp>
 6 | #include <opencv2/highgui.hpp>
 7 | #include <opencv2/imgproc.hpp>
 8 | #include <iostream>
 9 | 
10 | using namespace std;
11 | using namespace cv;
12 | 
13 | image_transport::Subscriber image_sub;
14 | image_transport::Publisher image_pub;
15 | 
16 | void handle_image(const sensor_msgs::ImageConstPtr &img)
17 | {
18 |     //convert ros image message to open-cv compatible BGR image
19 |     Mat image = cv_bridge::toCvShare(img, sensor_msgs::image_encodings::BGR8)->image;
20 | 
21 |     //draw a rectangle on image, originating at (150,150) or size 100x100,
22 |     //set color to (255,0,0) (blue), and line width to 3.
23 |     //  cv::rectangle(image, cv::Rect(150, 150, 100, 100), cv::Scalar(255, 0, 0), -1);
24 |     cv::Rect rectangle(150, 150, 100, 100);
25 |     cv::Scalar color(255, 0, 255);
26 |     cv::rectangle(image, rectangle, color, -1);
27 | 
28 |     //use cv_bridge to convert opencv image to ros image message
29 |     //create image message
30 |     sensor_msgs::Image::Ptr output_img;
31 |     //convert opencv image we drew on to ROS image message
32 |     output_img = cv_bridge::CvImage(img->header, "bgr8", image).toImageMsg();
33 |     //publish our ros image message
34 |     image_pub.publish(output_img);
35 | }
36 | 
37 | int main(int argc, char **argv)
38 | {
39 |     ros::init(argc, argv, "cv_bridge_test");
40 |     ros::NodeHandle nh;
41 |     image_transport::ImageTransport it_(nh);
42 | 
43 |     // Subscribe to input video feed and publish output video feed
44 |     image_sub = it_.subscribe("/usb_cam/image_raw", 1, handle_image);
45 |     image_pub = it_.advertise("image_output", 1);
46 | 
47 |     ros::Rate loop_rate(30);
48 |     while (ros::ok())
49 |     {
50 |         ros::spinOnce();
51 |         loop_rate.sleep();
52 |     }
53 | 
54 |     return 0;
55 | }


--------------------------------------------------------------------------------
/chapter19/cv_test_node.cpp:
--------------------------------------------------------------------------------
 1 | #include <ros/ros.h>
 2 | #include <opencv2/core/core.hpp>
 3 | #include <opencv2/highgui.hpp>
 4 | #include <opencv2/imgproc.hpp>
 5 | #include <iostream>
 6 | 
 7 | using namespace std;
 8 | 
 9 | int main(int argc, char **argv)
10 | {
11 |     ros::init(argc, argv, "open_cv_test");
12 |     ros::NodeHandle nh;
13 | 
14 |     //display Open CV version
15 |     cout << "OpenCV version : " << CV_VERSION << endl;
16 | 
17 |     //create blank 3-color image of size 400x400pixels.
18 |     cv::Mat testImage = cv::Mat::zeros(cv::Size(400, 400), CV_8UC3);
19 | 
20 |     //draw a rectangle on testImage, originating at (150,150) or size 100x100,
21 |     //set color to (255,0,0) (blue), and line width to 3.
22 |     cv::rectangle(testImage, cv::Rect(150,150,100,100), cv::Scalar(255, 0, 0), 3);
23 | 
24 |     //display testImage in a new window called test_window
25 |     cv::imshow("test_window", testImage);
26 | 
27 |     //wait for key press tocontinue
28 |     cv::waitKey(0);
29 |     
30 |     return 0;
31 | }


--------------------------------------------------------------------------------
/chapter19/hsv_colors.cpp:
--------------------------------------------------------------------------------
 1 | #include <opencv2/core/core.hpp>
 2 | #include <opencv2/highgui.hpp>
 3 | #include <opencv2/imgproc.hpp>
 4 | #include "opencv2/imgcodecs.hpp"
 5 | #include <opencv2/videoio.hpp>
 6 | #include <iostream>
 7 | #include <string>
 8 | 
 9 | using namespace cv;
10 | using namespace std;
11 | 
12 | int main()
13 | {
14 |     namedWindow("out");
15 |     Mat hsv = Mat(Size(900, 150), CV_8UC3, Scalar(0, 255, 255));
16 |     Mat bgr;
17 |     int hue = 0;
18 | 
19 |     for (int col = 0; col < 900; col++)
20 |     {
21 |         for (int row = 0; row < hsv.size().height; row++)
22 |         {
23 |             hsv.at<Vec3b>(row, col) = Vec3b(hue, 255, 255);
24 |         }
25 |         if (col % 5 == 0)
26 |         {
27 |             hue++;
28 |         }
29 | 
30 |         cvtColor(hsv, bgr, CV_HSV2BGR);
31 |         imshow("out", bgr);
32 |         waitKey(2);
33 |     }
34 |     for (int col = 50; col < bgr.size().width; col += 50)
35 |     {
36 |         line(bgr, Point(col, bgr.size().height-30), Point(col, bgr.size().height-50), Scalar(0,0,0), 2);
37 |         putText(bgr, to_string(col / 5), Point(col-10, bgr.size().height-15), FONT_HERSHEY_COMPLEX_SMALL, .75, Scalar(0, 0, 0), 2);
38 |     }
39 |     imshow("out", bgr);
40 |     waitKey(0);
41 |     return 0;
42 | }


--------------------------------------------------------------------------------
/chapter19/image_transport_test.cpp:
--------------------------------------------------------------------------------
 1 | #include <ros/ros.h>
 2 | #include <image_transport/image_transport.h>
 3 | #include <cv_bridge/cv_bridge.h>
 4 | #include <sensor_msgs/image_encodings.h>
 5 | 
 6 | //using namespace std;
 7 | 
 8 | image_transport::Subscriber image_sub;
 9 | image_transport::Publisher image_pub;
10 | 
11 | void handle_image(const sensor_msgs::ImageConstPtr &img)
12 | {
13 |     //declare a new image
14 |     sensor_msgs::Image output_image;
15 | 
16 |     //simply copy all the data fields from the recieved image
17 |     output_image.header = img->header;
18 |     output_image.width = img->width;
19 |     output_image.height = img->height;
20 |     output_image.encoding = img->encoding;
21 |     output_image.is_bigendian = img->is_bigendian;
22 |     output_image.step = img->step;
23 |     output_image.data = img->data;
24 | 
25 |     //publish our ros image message
26 |     image_pub.publish(output_image);
27 | }
28 | 
29 | int main(int argc, char **argv)
30 | {
31 |     ros::init(argc, argv, "cv_bridge_test");
32 |     ros::NodeHandle nh;
33 |     image_transport::ImageTransport it_(nh);
34 | 
35 |     // Subscribe to input video feed and publish output video feed
36 |     image_sub = it_.subscribe("/usb_cam/image_raw", 1, handle_image);
37 |     image_pub = it_.advertise("image_output", 1);
38 | 
39 |     ros::Rate loop_rate(30);
40 |     while (ros::ok())
41 |     {
42 |         ros::spinOnce();
43 |         loop_rate.sleep();
44 |     }
45 | 
46 |     return 0;
47 | }


--------------------------------------------------------------------------------
/chapter2/hello_blink.cpp:
--------------------------------------------------------------------------------
 1 | /*
 2 | hello_blink is a program to learn to control the output of a
 3 | Raspberry Pi GPIO pin with the PIGPIO library. This program
 4 | is an accompaniment to the book Practical Robotics in C++
 5 | written by Lloyd Brombach and published by Packt Publishing
 6 | */
 7 | 
 8 | #include <pigpiod_if2.h>
 9 | #include <iostream>
10 | 
11 | const int LED = 5;
12 | 
13 | using namespace std;
14 | 
15 | 
16 | int PigpioSetup()
17 | {
18 |    char *addrStr = NULL;
19 |    char *portStr = NULL;
20 |    //handshake with daemon and get pi handle
21 |    int pi = pigpio_start(addrStr, portStr);
22 | 
23 |    //set the pin mode and intialize to low
24 |    set_mode(pi, LED, PI_OUTPUT);
25 |    gpio_write(pi, LED, 0);
26 | 
27 |    return pi;
28 | }
29 | 
30 | int main()
31 | {
32 |    //initialize pipiod interface
33 |    int pi = PigpioSetup();
34 |    //check that handshake went ok
35 |    if(pi>=0)
36 |    {
37 |       cout<<"daemon interface started ok at "<<pi<<endl;
38 |    }
39 |    else
40 |    {
41 |       cout<<"Failed to connect to PIGPIO Daemon - Try running sudo pigpiod and try again."<<endl;
42 |       return -1;
43 |    }
44 | 
45 |    //set pin 6 high to turn Led on
46 |    gpio_write(pi, LED, 1);
47 |    //sleep for 3.2 seconds
48 |    time_sleep(3.2);
49 |    //turn led off
50 |    gpio_write(pi, LED, 0);
51 | 
52 |    //disconnect from pigpio daemon
53 |    pigpio_stop(pi);
54 |    return 0;
55 | }
56 | 


--------------------------------------------------------------------------------
/chapter2/hello_button.cpp:
--------------------------------------------------------------------------------
 1 | /*
 2 | hello_button is a program to learn to read the input of a
 3 | Raspberry Pi GPIO pin with the PIGPIO library. This program
 4 | is an accompaniment to the book Practical Robotics in C++
 5 | written by Lloyd Brombach and published by Packt Publishing
 6 | */
 7 | 
 8 | #include <pigpiod_if2.h>
 9 | #include <iostream>
10 | 
11 | const int LED = 5;
12 | const int BUTTON = 27;
13 | 
14 | using namespace std;
15 | 
16 | 
17 | int PigpioSetup()
18 | {
19 |    char *addrStr = NULL;
20 |    char *portStr = NULL;
21 |    //handshake with daemon and get pi handle
22 |    int pi = pigpio_start(addrStr, portStr);
23 | 
24 |    //set the pin 6 mode and intialize to low
25 |    set_mode(pi, LED, PI_OUTPUT);
26 |    gpio_write(pi, LED, 0);
27 | 
28 |    //set pin 27 mode as input
29 |    set_mode(pi, BUTTON, PI_INPUT);
30 |    set_pull_up_down(pi, BUTTON, PI_PUD_UP);
31 | 
32 |    return pi;
33 | }
34 | 
35 | int main()
36 | {
37 |   //initialize pipiod interface
38 |   int pi = PigpioSetup();
39 |   //check that handshake went ok
40 |   if(pi>=0)
41 |   {
42 |     cout<<"daemon interface started ok at "<<pi<<endl;
43 |   }
44 |   else
45 |   {
46 |     cout<<"Failed to connect to PIGPIO Daemon - Try running sudo pigpiod and try again."<<endl;
47 |     return -1;
48 |   }
49 | 
50 |   while(1)
51 |   {
52 |       if(gpio_read(pi, BUTTON) == false)
53 |       {
54 |          //set pin 6 high to turn Led on
55 |          gpio_write(pi, LED, 1);
56 |          //sleep for 1.5 seconds
57 |          time_sleep(1.5);
58 |          //turn led off
59 |          gpio_write(pi, LED, 0);
60 |       }
61 |   }
62 | 
63 |   //disconnect from pigpio daemon
64 |   pigpio_stop(pi);
65 |   return 0;
66 | }
67 | 


--------------------------------------------------------------------------------
/chapter2/hello_callback.cpp:
--------------------------------------------------------------------------------
 1 | /*
 2 | hello_callback is a generic program to learn to use GPIO pin event
 3 | callback functions with the PIGPIO library on a Raspberry pi.
 4 | This program is an accompaniment to the book Practical Robotics in C++
 5 | written by Lloyd Brombach and published by Packt Publishing.
 6 | */
 7 | 
 8 | #include <iostream>
 9 | #include <pigpiod_if2.h>
10 | 
11 | using namespace std;
12 | 
13 | const int BUTTON = 27;
14 | 
15 | 
16 | //this is the callback function that runs when a change of state happens on the monitored gpio pin
17 | void button_event(int pi, unsigned int gpio, unsigned int edge, unsigned int foo)
18 | {
19 |     static int i = 0;
20 |     cout<<"Button pressed. Press count = "<<i++<<endl;
21 | }
22 | 
23 | 
24 | int main()
25 | {
26 |    char *addrStr = NULL;
27 |    char *portStr = NULL;
28 |    int pi = pigpio_start(addrStr, portStr);
29 |    if(pi>=0)
30 |    {
31 |       cout<<"daemon interface started ok at "<<pi<<endl;
32 |    }
33 |    else
34 |    {
35 |       cout<<"Failed to connect to PIGPIO Daemon - Try running sudo pigpiod and try again."<<endl;
36 |       return -1;
37 |    }
38 | 
39 |    //set pin 27 mode as input
40 |    set_mode(pi, BUTTON, PI_INPUT);
41 |    set_pull_up_down(pi, BUTTON, PI_PUD_UP);
42 | 
43 |    //initalizes a callback
44 |    int callbackID=callback(pi, BUTTON, RISING_EDGE, button_event);
45 | 
46 |    //lets the program run for 60 seconds
47 |    time_sleep(60);
48 |    cout<<"60 seconds has elapsed. Program ending."<<endl;
49 | 
50 |    //cancel the callback and end the pigpio daemon interface
51 |    callback_cancel(callbackID);
52 |    pigpio_stop(pi);
53 |    return 0;
54 | }
55 | 


--------------------------------------------------------------------------------
/chapter20/odom_plus.cpp:
--------------------------------------------------------------------------------
  1 | /*
  2 | *This is a version of encoder_odom_pub.cpp ROS node that subscribes to encoder count messages and publishes odometry
  3 | *on in a simple form where orientation.z is an euler angle, then publishes again
  4 | *on a topic that uses the quaternion version of the message. This version
  5 | *subscribes to an IMU topic and adjusts the heading of this node with the IMU heading data.
  6 | *This is intended to be used with an IMU like a BN0055 with a built-in fusion algorithm
  7 | *to improve odometry. This is written to be readable for all levels and accompanies the book Practical Robotics in C++.
  8 | *
  9 | *Author: Lloyd Brombach (lbrombach2@gmail.com)
 10 | *11/7/2019
 11 | */
 12 | 
 13 | #include "ros/ros.h"
 14 | #include "std_msgs/Int16.h"
 15 | #include <nav_msgs/Odometry.h>
 16 | #include <geometry_msgs/PoseStamped.h>
 17 | #include <sensor_msgs/Imu.h>
 18 | #include <tf2/LinearMath/Quaternion.h>
 19 | #include <tf2_ros/transform_broadcaster.h>
 20 | #include <tf/transform_broadcaster.h>
 21 | #include <cmath>
 22 | 
 23 | ros::Publisher odom_pub;
 24 | ros::Publisher pub_quat;
 25 | nav_msgs::Odometry newOdom;
 26 | nav_msgs::Odometry oldOdom;
 27 | 
 28 | const double initialX = 0.0;
 29 | const double initialY = 0.0;
 30 | const double initialTheta = 0.00000000001;
 31 | const double PI = 3.141592;
 32 | 
 33 | const double ticksPerwheelRev = 508.8; //not in use. just a reference for now
 34 | const double wheelRadius = .03575; // 55.18;
 35 | const double WHEEL_BASE = .224; //223.8375mm actually
 36 | const double TICKS_PER_M = 1125*2; //1.1645; //1.365 is on hard floor. carpet avg is 1.1926. overall avg = 1.1645
 37 | 
 38 | double leftDistance = 0;
 39 | double rightDistance = 0;
 40 | bool initialPoseRecieved = false;
 41 | 
 42 | float imuHeading = 0;
 43 | float headingOffset = 0;
 44 | bool haveNewImuHeading = false;
 45 | bool imuHeadingInitialized = false;
 46 | 
 47 | using namespace std;
 48 | 
 49 | //accept initial_2d pose message form clicks or manual publisher
 50 | //at at least one is required to start this node publishing
 51 | void setInitialPose(const geometry_msgs::PoseStamped &rvizClick)
 52 | {
 53 |     cout<<"Got initial pose. Starting node"<<endl;
 54 | 
 55 |      headingOffset = rvizClick.pose.orientation.z - imuHeading;
 56 |      cout<<"heading offset = "<<rvizClick.pose.orientation.z<<" - "<<imuHeading<<" = "<<headingOffset<<endl;
 57 | 
 58 |      oldOdom.pose.pose.position.x = rvizClick.pose.position.x;
 59 |      oldOdom.pose.pose.position.y = rvizClick.pose.position.y;
 60 |      oldOdom.pose.pose.orientation.z = rvizClick.pose.orientation.z;
 61 |      initialPoseRecieved = true;
 62 |      imuHeadingInitialized = true;
 63 | }
 64 | 
 65 | //calculate distance left wheel has traveled since last cycle
 66 | void Calc_Left(const std_msgs::Int16& lCount)
 67 | {
 68 | static int lastCountL = 0;
 69 |     if(lCount.data!=0&&lastCountL!=0)
 70 |     {
 71 |         int leftTicks = (lCount.data - lastCountL);
 72 | 
 73 |         if (leftTicks > 10000)
 74 |         {
 75 |             leftTicks=0-(65535 - leftTicks);
 76 |         }
 77 |         else if (leftTicks < -10000)
 78 |         {
 79 |             leftTicks = 65535-leftTicks;
 80 |         }
 81 |         leftDistance = leftTicks/TICKS_PER_M;
 82 |     }
 83 |         lastCountL = lCount.data;
 84 | }
 85 | 
 86 | //calculate distance right wheel has traveled since last cycle
 87 | void Calc_Right(const std_msgs::Int16& rCount)
 88 | {
 89 | static int lastCountR = 0;
 90 |     if(rCount.data!=0&&lastCountR!=0)
 91 |     {
 92 |         int rightTicks = rCount.data - lastCountR;
 93 |         if (rightTicks > 10000)
 94 |         {
 95 |             rightTicks=(0-(65535 - rightTicks))/TICKS_PER_M;
 96 |         }
 97 |         else if (rightTicks < -10000)
 98 |         {
 99 |         rightTicks = 65535 - rightTicks;
100 |         }
101 |         rightDistance = rightTicks/TICKS_PER_M;
102 |     }
103 |         lastCountR=rCount.data;
104 | }
105 | 
106 | //publishes the simpler version of the odom message in full quaternion form
107 | //and includes covariance data. This covariance should be "personalized"
108 | //for a specific robot before it is used.
109 | void publish_quat()
110 | {
111 |     tf2::Quaternion q;
112 |     q.setRPY(0, 0, newOdom.pose.pose.orientation.z);
113 | 
114 |     nav_msgs::Odometry quatOdom;
115 |     quatOdom.header.stamp = newOdom.header.stamp;
116 |     quatOdom.header.frame_id = "odom";
117 |     quatOdom.child_frame_id = "base_link";
118 |     quatOdom.pose.pose.position.x = newOdom.pose.pose.position.x;
119 |     quatOdom.pose.pose.position.y = newOdom.pose.pose.position.y;
120 |     quatOdom.pose.pose.position.z = newOdom.pose.pose.position.z;
121 |     quatOdom.pose.pose.orientation.x = q.x();
122 |     quatOdom.pose.pose.orientation.y = q.y();
123 |     quatOdom.pose.pose.orientation.z = q.z();
124 |     quatOdom.pose.pose.orientation.w = q.w();
125 |     quatOdom.twist.twist.linear.x = newOdom.twist.twist.linear.x;
126 |     quatOdom.twist.twist.linear.y = newOdom.twist.twist.linear.y;
127 |     quatOdom.twist.twist.linear.z = newOdom.twist.twist.linear.z;
128 |     quatOdom.twist.twist.angular.x = newOdom.twist.twist.angular.x;
129 |     quatOdom.twist.twist.angular.y = newOdom.twist.twist.angular.y;
130 |     quatOdom.twist.twist.angular.z = newOdom.twist.twist.angular.z;
131 | 
132 |     for(int i = 0; i<36; i++)
133 |     {
134 |        if(i == 0 || i == 7 || i == 14)
135 |        {
136 |            quatOdom.pose.covariance[i] = .01;
137 |        }
138 |        else if (i == 21 || i == 28 || i== 35)
139 |        {
140 |            quatOdom.pose.covariance[i] += .165;
141 |        }
142 |        else
143 |        {
144 |            quatOdom.pose.covariance[i] = 0;
145 |        }
146 |     }
147 | 
148 |     pub_quat.publish(quatOdom);
149 | 
150 | }
151 | 
152 | //checks if rate of change is realistic or likely error
153 | bool isSafeRateOfChange(float headings[], double times[])
154 | {
155 |     double rate[3] = {0};
156 |     for(int i = 0; i<3; i++)
157 |     {
158 |         double timeElapsed = headings[i] - headings[i+1];
159 |         if(timeElapsed != 0)
160 |         {
161 |             //calculate radians per second
162 |             rate[i]=abs(headings[i]-headings[i+1]) / timeElapsed;
163 |         }
164 |         if(rate[i] > .785 || timeElapsed == 0) // .785 = about 45 degrees/sec
165 |         {
166 |             return false;
167 |         }
168 |     }
169 |     return true;
170 | }
171 | 
172 | 
173 | void update_heading(const sensor_msgs::Imu &imuMsg)
174 | {
175 |     //element zero will be most recent heading recieved from imu
176 |     static float headings[4]={0};
177 |     static double times[4] = {0};
178 | 
179 | 
180 |     if(imuMsg.orientation_covariance[0] != -1)
181 |     {
182 |         tf::Quaternion q(imuMsg.orientation.x, imuMsg.orientation.y, imuMsg.orientation.z, imuMsg.orientation.w);
183 |         tf::Matrix3x3 m(q);
184 |         double roll, pitch, yaw;
185 |         m.getRPY(roll, pitch, yaw);
186 | 
187 |         //saves 4 cycles' worth of yaw and times stamps  for next step
188 |         for(int i = 3; i>0; i++)
189 |         {
190 |             headings[i] = headings[i-1];
191 |             times[i] = times[i-1];
192 |         }
193 |         headings[0] = yaw;
194 |         times[0] = ros::Time::now().toSec();
195 | 
196 |         //if this is false, possibly errant reading. Require 3 consecutive stable readings to trust.
197 |         if(isSafeRateOfChange(headings, times))
198 |         {
199 |             if(imuHeadingInitialized == false)
200 |             {
201 |                 headingOffset = oldOdom.pose.pose.orientation.z - yaw;
202 |                 imuHeadingInitialized = true;
203 |                 cout<<"heading offset = "<<oldOdom.pose.pose.orientation.z<<" - "<<yaw<<" = "<<headingOffset<<endl;
204 |             }
205 |             else
206 |             {
207 |                 haveNewImuHeading = true;
208 |                 cout<<"using imuheading"<<endl;
209 |             }
210 |         }
211 |         else
212 |         {
213 |             haveNewImuHeading = false;
214 |         }
215 |     }
216 | }
217 | 
218 | void update_odom()
219 | {
220 |     //average distance
221 |     double cycleDistance = (rightDistance+leftDistance)/2;
222 |     //how many radians robot has turned since last cycle
223 |     double cycleAngle = asin((rightDistance-leftDistance)/WHEEL_BASE);
224 | 
225 |     //average angle during last cycle
226 |     double avgAngle = cycleAngle/2 + oldOdom.pose.pose.orientation.z;
227 |     if (avgAngle > PI)
228 |         {
229 |         avgAngle -= 2*PI;
230 |         }
231 |     else if (avgAngle < -PI)
232 |     {
233 |         avgAngle += 2*PI;
234 |     }
235 | 
236 |     //calculate new x, y, and theta
237 |     newOdom.pose.pose.position.x = oldOdom.pose.pose.position.x + cos(avgAngle)*cycleDistance;
238 |     newOdom.pose.pose.position.y = oldOdom.pose.pose.position.y + sin(avgAngle)*cycleDistance;
239 |     newOdom.pose.pose.orientation.z = cycleAngle + oldOdom.pose.pose.orientation.z;
240 | 
241 |     //prevent lockup from a single erroneous cycle
242 |     if(isnan(newOdom.pose.pose.position.x) || isnan(newOdom.pose.pose.position.y)
243 |         || isnan(newOdom.pose.pose.position.z) )
244 |     {
245 |         newOdom.pose.pose.position.x = oldOdom.pose.pose.position.x;
246 |         newOdom.pose.pose.position.y = oldOdom.pose.pose.position.y;
247 |         newOdom.pose.pose.orientation.z = oldOdom.pose.pose.orientation.z;
248 |     }
249 | 
250 |     //keep theta in range proper range
251 |     if (newOdom.pose.pose.orientation.z > PI)
252 |     {
253 |         newOdom.pose.pose.orientation.z -= 2*PI;
254 |     }
255 |     else if (newOdom.pose.pose.orientation.z < -PI)
256 |     {
257 |         newOdom.pose.pose.orientation.z += 2*PI;
258 |     }
259 | 
260 |     //calculate velocity
261 |     newOdom.header.stamp = ros::Time::now();
262 |     newOdom.twist.twist.linear.x = cycleDistance/(newOdom.header.stamp.toSec() - oldOdom.header.stamp.toSec());
263 |     newOdom.twist.twist.angular.z = cycleAngle/(newOdom.header.stamp.toSec() - oldOdom.header.stamp.toSec());
264 | 
265 |     //update orientation if we have a smart heading from IMU
266 |     if(haveNewImuHeading == true)
267 |     {
268 |         newOdom.pose.pose.orientation.z = imuHeading+headingOffset;
269 |         haveNewImuHeading = false;
270 |         for(int i = 0; i<36; i++)
271 |         {
272 |             if(i == 0 || i == 7 || i == 14)
273 |             {
274 |                 newOdom.pose.covariance[i] = .002;
275 |             }
276 |             else if (i == 21 || i == 28 || i== 35)
277 |             {
278 |                 newOdom.pose.covariance[i] = .001;
279 |             }
280 |             else
281 |             {
282 |                 newOdom.pose.covariance[i] = 0;
283 |             }
284 |         }
285 |     }
286 |     else
287 |     {
288 |         for(int i = 0; i<36; i++)
289 |         {
290 |             if(i == 0 || i == 7 || i == 14)
291 |             {
292 |                 newOdom.pose.covariance[i] = .0021;
293 |             }
294 |             else if (i == 21 || i == 28 || i== 35)
295 |             {
296 |                 if(cycleAngle != 0 )
297 |                 newOdom.pose.covariance[i] += .0001;
298 |             }
299 |             else
300 |             {
301 |                 newOdom.pose.covariance[i] = 0;
302 |             }
303 |         }
304 |     }
305 | 
306 |     //save odom x, y, and theta for use in next cycle
307 |     oldOdom.pose.pose.position.x = newOdom.pose.pose.position.x;
308 |     oldOdom.pose.pose.position.y = newOdom.pose.pose.position.y;
309 |     oldOdom.pose.pose.orientation.z = newOdom.pose.pose.orientation.z;
310 |     oldOdom.header.stamp = newOdom.header.stamp;
311 | 
312 |     //publish odom message
313 |     odom_pub.publish(newOdom);
314 | }
315 | 
316 | int main(int argc, char **argv)
317 | {
318 |     //set fixed data fields
319 |     newOdom.header.frame_id = "odom";
320 |     newOdom.pose.pose.position.z = 0;
321 |     newOdom.pose.pose.orientation.x = 0;
322 |     newOdom.pose.pose.orientation.y = 0;
323 |     newOdom.twist.twist.linear.x = 0;
324 |     newOdom.twist.twist.linear.y = 0;
325 |     newOdom.twist.twist.linear.z = 0;
326 |     newOdom.twist.twist.angular.x = 0;
327 |     newOdom.twist.twist.angular.y = 0;
328 |     newOdom.twist.twist.angular.z = 0;
329 |     oldOdom.pose.pose.position.x = initialX;
330 |     oldOdom.pose.pose.position.y = initialY;
331 |     oldOdom.pose.pose.orientation.z = initialTheta;
332 | 
333 |     //handshake with ros master and create node object
334 |     ros::init(argc, argv, "odom_plus");
335 |     ros::NodeHandle node;
336 | 
337 |     //Subscribe to topics
338 |     ros::Subscriber subForRightCounts = node.subscribe("rightWheel", 100, Calc_Right,ros::TransportHints().tcpNoDelay());
339 |     ros::Subscriber subForLeftCounts = node.subscribe("leftWheel",100, Calc_Left,ros::TransportHints().tcpNoDelay());
340 |     ros::Subscriber subInitialPose = node.subscribe("initial_2d", 1, setInitialPose);
341 |     ros::Subscriber subImu = node.subscribe("imu", 100, update_heading);
342 | 
343 |     //advertise publisher of simpler odom msg where orientation.z is an euler angle
344 |     odom_pub = node.advertise<nav_msgs::Odometry>("encoder/odom", 100);
345 | 
346 |     //advertise publisher of full odom msg where orientation is quaternion
347 |     pub_quat = node.advertise<nav_msgs::Odometry>("encoder/odom_quat", 100);
348 | 
349 | 
350 |     ros::Rate loop_rate(30);
351 |     while(ros::ok())
352 |     {
353 |         ros::spinOnce();
354 |         if(initialPoseRecieved)
355 |         {
356 |             update_odom();
357 |             publish_quat();
358 |         }
359 |         loop_rate.sleep();
360 |     }
361 | 
362 |     return 0;
363 | }
364 | 


--------------------------------------------------------------------------------
/chapter3/hello_roomba.cpp:
--------------------------------------------------------------------------------
 1 | /*
 2 | *hello_roomba.cpp is a simple program with a few sample functions for communicating with your Roomba
 3 | *using the PIGPIO library. Note that you’ll have to study the PIGPIO documentation and the Roomba OI
 4 | *documentation for this to really make sense, these are here to make it easier for you to understand
 5 | *how the OI and PIGPIO work together.
 6 | *
 7 | *From the book Practical Robotics in C++ by Lloyd Brombach 2020
 8 | */
 9 | 
10 | #include "pigpiod_if2.h"
11 | 
12 | using namespace std;
13 | 
14 | int pi = -1;
15 | int serHandle = -1;
16 | 
17 | //used to make sure roomba awake and listening
18 | void wake(){
19 |     int R1 = 23;  //This is assuming the wake relay is on pin 23
20 |     set_mode(pi, R1, PI_OUTPUT);
21 | 
22 |     gpio_write(pi, R1, 0);     //pulse wake relay
23 |     time_sleep(.1);
24 |     gpio_write(pi, R1, 1);
25 | 
26 |     serial_write_byte(pi,serHandle,128);          //send start command
27 |     time_sleep(.15);
28 |     serial_write_byte(pi,serHandle,131);          //set to safe mode
29 |     time_sleep(.15);
30 | }
31 | 
32 | //drive in reverse at 80mm/sec
33 | void rev()
34 | {
35 |    char driveString[] = {137, 255, 136, 0, 0};
36 |    serial_write(pi, serHandle, driveString, 5);
37 | }
38 | 
39 | //drive forward at 120mm/sec
40 | void fwd()
41 | {
42 |    char driveString[] = {137, 0, 120, 127, 255};
43 |    serial_write(pi, serHandle, driveString, 5);
44 | }
45 | 
46 | //stops wheel motors
47 | void stop()
48 | {
49 |    char driveString[] = {137, 0, 0, 0, 0};
50 |    serial_write(pi, serHandle, driveString, 5);
51 | }
52 | 
53 | //puts Roomba to sleep and frees program resources
54 | void shutdown()
55 | {
56 |    serial_write_byte(pi,serHandle,133);
57 |    time_sleep(.1);
58 |    serial_close(pi, serHandle);
59 |    pigpio_stop(pi);
60 | }
61 | 
62 | //main() wakes Roomba, drives it forward for 5 seconds. Pauses
63 | //for one second, reverses for 5 seconds, the shuts everything
64 | //down
65 | int main()
66 | {
67 |    char *addrStr = NULL;
68 |    char *portStr = NULL;
69 |    //handshake with daemon and get pi handle
70 |    pi = pigpio_start(addrStr, portStr);
71 | 
72 |    //open serial port
73 |    serHandle = serial_open(pi, "/dev/ttyUSB0",115200,0);
74 | 
75 |    wake();
76 |    fwd();
77 |    time_sleep(5);
78 |    stop();
79 |    time_sleep(1);
80 |    rev();
81 |    time_sleep(5);
82 |    stop();
83 |    shutdown();
84 | }
85 | 


--------------------------------------------------------------------------------
/chapter3/roomba_sleep.cpp:
--------------------------------------------------------------------------------
 1 | /*
 2 | *roomba_sleep.cpp is a simple program to help "unfreeze" a roomba during eperimentation with the
 3 | *open interface. Sometimes test code will leave Roomba waiting for new data forever, so blasting
 4 | *it with the "sleep" command should satisfy the data requirement and eventuall put it to sleep mode.
 5 | *
 6 | *From the book Practical Robotics in C++ by Lloyd Brombach, 2020
 7 | */
 8 | 
 9 | 
10 | #include "pigpiod_if2.h"
11 | 
12 | using namespace std;
13 | 
14 | 
15 | //puts Roomba to sleep
16 | {
17 |    int pi = -1;
18 |    int serHandle = -1;
19 |    char *addrStr = NULL;
20 |    char *portStr = NULL;
21 |    //handshake with daemon and get pi handle
22 |    pi = pigpio_start(addrStr, portStr);
23 | 
24 |    //open serial port
25 |    //serHandle = serial_open(pi, "/dev/ttyUSB0",115200,0);
26 |    serHandle = serial_open(pi, "/dev/ttyAMA0",115200,0);
27 | 
28 |    for(int i = 0; i < 100; i++)
29 |    {
30 |         serial_write_byte(pi, serHandle, 133);
31 |         time_sleep(.01);
32 |    }
33 | 
34 |    time_sleep(.1);
35 |    serial_close(pi, serHandle);
36 |    pigpio_stop(pi);
37 | }
38 | 


--------------------------------------------------------------------------------
/chapter4/hello_motor.cpp:
--------------------------------------------------------------------------------
 1 | /*
 2 | hello_motor is a program to learn to use a L298N dual
 3 | H Bridge motor driver with the PIGPIO library for the Raspberry Pi.
 4 | This program is an accompaniment to the book Practical Robotics in C++
 5 | written by Lloyd Brombach and published by Packt Publishing
 6 | */
 7 | 
 8 | 
 9 | #include <iostream>
10 | #include <pigpiod_if2.h>
11 | 
12 | 
13 | //define our GPIO pin assignments
14 | const int PWM_A = 21;
15 | const int MOTOR_A_FWD = 26;
16 | const int MOTOR_A_REV = 13;
17 | 
18 | //handshakes with Pigpio Daemon and sets up our pins.
19 | int pigpio_setup()
20 | {
21 |     char *addrStr = NULL;
22 |     char *portStr = NULL;
23 |     const int pi = pigpio_start(addrStr, portStr);
24 | 
25 |     //next four lines sets up our pins. Remember that high is "off"
26 |     //and we must drive in1 or in2 low to start the output to motor
27 |     set_mode(pi,PWM_A, PI_OUTPUT);
28 |     set_mode(pi,MOTOR_A_FWD, PI_OUTPUT);
29 |     set_mode(pi,MOTOR_A_REV, PI_OUTPUT);
30 | 
31 |     //initializes motor off
32 |     gpio_write(pi, MOTOR_A_FWD, 1);
33 |     gpio_write(pi, MOTOR_A_REV, 1);
34 |     return pi;
35 | }
36 | 
37 | using namespace std;
38 | 
39 | int main()
40 | {
41 |     int pi = pigpio_setup();
42 |     if(pi < 0)
43 |     {
44 |         cout<<"Failed to connect to Pigpio Daemon. Is it running?"<<endl;
45 |         return -1;
46 |     }
47 | 
48 | 
49 |     //when you're ready to start the motor
50 |     gpio_write(pi, MOTOR_A_FWD, 0);
51 | 
52 |     // starts a PWM signal to motor A enable at half speed
53 |     set_PWM_dutycycle(pi, PWM_A, 127);
54 |     time_sleep(3); //3 second delay
55 |     //starts motor at full speed
56 |     set_PWM_dutycycle(pi, PWM_A, 255);
57 |     time_sleep(3);
58 | 
59 |     //stops the motor
60 |     gpio_write(pi, MOTOR_A_FWD, 1);
61 |     time_sleep(1);
62 |     //repeats in reverse
63 |     gpio_write(pi, MOTOR_A_REV, 0);
64 |     set_PWM_dutycycle(pi, PWM_A, 127);
65 |     time_sleep(3);
66 |     set_PWM_dutycycle(pi, PWM_A, 255);
67 |     time_sleep(3);
68 |     gpio_write(pi, MOTOR_A_REV, 1);
69 | 
70 |     pigpio_stop(pi);
71 |     return 0;
72 | }
73 | 


--------------------------------------------------------------------------------
/chapter5/hello_i2c_lsm303.cpp:
--------------------------------------------------------------------------------
 1 | /*
 2 | hello_i2c_lsm303.cpp is a program to learn to use I2C devices
 3 | with the PIGPIO library for the Raspberry Pi.
 4 | This program is an accompaniment to the book Practical Robotics in C++
 5 | written by Lloyd Brombach and published by Packt Publishing
 6 | */
 7 | 
 8 | #include <iostream>
 9 | #include <pigpiod_if2.h>
10 | 
11 | using namespace std;
12 | 
13 | const int LSM303_accel=0x19; //accelerometer address
14 | const int I2Cbus=1; //RPi typical I2C bus. Find yours with "ls /dev/*i2c* "
15 | 
16 | 
17 | 
18 | int main()
19 | {
20 | 	//initialize connection with pigpio daemon
21 | 	char *addrStr = NULL;
22 | 	char *portStr = NULL;
23 | 	int pi=pigpio_start(addrStr, portStr);
24 | 	if(pi>=0)
25 |     	{
26 |     		cout<<"daemon interface started ok at "<<pi<<endl;
27 |     	}
28 |  	else
29 |  	{
30 |  		cout<<"no bueno, did you forget to start pigpio daemon (sudo pigpiod from terminal) ? "<<endl;
31 |  	return -1;
32 |  	}
33 | 
34 | 	//open I2C connection
35 | 	const int ACCEL_HANDLE=i2c_open(pi,I2Cbus, LSM303_accel,0);
36 | 	if (ACCEL_HANDLE>=0)
37 |     	{
38 |      		cout<<"Accelerometer found. Handle = "<<ACCEL_HANDLE<<endl;
39 |     	}
40 | 	else
41 |     	{
42 |      		cout<<"Unable to open I2C comms with Accelerometer"<<endl;
43 |     	}
44 | 
45 | 	//set frequency
46 | 	i2c_write_byte_data(pi, ACCEL_HANDLE, 0x20, 0x47); 
47 | 	time_sleep(.02);
48 | 
49 | 	//continuous update, LSB at lower addr, +- 2g, Hi-Res disable
50 | 	i2c_write_byte_data(pi, ACCEL_HANDLE, 0x23, 0x09); 
51 | 	time_sleep(.02);
52 | 
53 | 	//read two of the registers for the x-axis data
54 |     int xLSB = (int)i2c_read_byte_data(pi, ACCEL_HANDLE, 0x28);
55 | 	int xMSB = (int)i2c_read_byte_data(pi, ACCEL_HANDLE, 0x29);
56 | 	//combine two 8-bit bytes into a one 16-bit value
57 |     float accelX=(float)((int16_t)(xLSB | xMSB<<8)>>4);
58 | 
59 | 	//repeat above for y, then z axis
60 | 	int yLSB = (int)i2c_read_byte_data(pi, ACCEL_HANDLE, 0x2A);
61 |     int yMSB = (int)i2c_read_byte_data(pi, ACCEL_HANDLE, 0x2B);
62 | 	float accelY=(float)((int16_t)(yLSB | yMSB<<8)>>4);
63 | 
64 |     int zLSB = (int)i2c_read_byte_data(pi, ACCEL_HANDLE, 0x2C);
65 |     int zMSB = (int)i2c_read_byte_data(pi, ACCEL_HANDLE, 0x2D);
66 | 	float accelZ=(float)((int16_t)(zLSB | zMSB<<8)>>4);
67 | 
68 | 	//display data
69 | 	cout<<endl<<accelX<<"   "<<accelY<<"   "<<accelZ<<endl;
70 | 
71 | 	//checks data to determine if device is fairly level (must be stationary to be reliable)
72 | 	if(accelX < 50 && accelX > -50 && accelY < 50 && accelY > -50)
73 |     	{
74 |      		cout<<"The device is fairly level"<<endl;
75 |     	}
76 | 	else
77 |     	{
78 |      		cout<<"The device is not level"<<endl;
79 |     	}
80 | 
81 | 	//puts accelerometer into sleep mode
82 | 	i2c_write_byte_data(pi, ACCEL_HANDLE, 0x20, 0x00);
83 | 
84 | 	//close I2C connection and terminate pigpio daemon connection
85 | 	i2c_close(pi, ACCEL_HANDLE);
86 | 	pigpio_stop(pi);
87 | 
88 |     return 0;
89 | }
90 | 


--------------------------------------------------------------------------------
/chapter5/hello_serial.cpp:
--------------------------------------------------------------------------------
 1 | /*
 2 | hello_serial.cpp is a program to learn to use and test
 3 | serial communication with the PIGPIO library for the Raspberry Pi.
 4 | This program is an accompaniment to the book Practical Robotics in C++
 5 | written by Lloyd Brombach and published by Packt Publishing
 6 | */
 7 | 
 8 | #include <iostream>
 9 | #include <pigpiod_if2.h>
10 | 
11 | using namespace std;
12 | 
13 | int main()
14 | {
15 | 	//initialize connection with pigpio daemon
16 | 	char *addrStr = NULL;
17 | 	char *portStr = NULL;
18 | 	int pi=pigpio_start(addrStr, portStr);
19 | 
20 | 	//open serial port connection
21 | 	int UARTHandle = serial_open(pi, "/dev/ttyAMA0",115200,0); //for use with GPIO serial pings 14 and 15
22 | 	
23 | 	//int UARTHandle = serial_open(pi, "/dev/ttyUSB0",115200,0); //for use with FTDI USB device
24 | 	cout<<"UARTHandle = " << UARTHandle<< endl;
25 | 	time_sleep(.1);
26 | 
27 | 	//check serial buffer
28 | 	cout << "Data available start: " << serial_data_available(pi, UARTHandle)<< " bytes" << endl;
29 | 
30 | 	//write a few test bytes
31 |     serial_write_byte(pi,UARTHandle,6);
32 | 	serial_write_byte(pi,UARTHandle,'f');
33 | 	serial_write_byte(pi,UARTHandle,'F');
34 | 
35 | 	//give time to transmit 
36 | 	time_sleep(.1);
37 | 
38 | 	//check serial buffer again
39 | 	cout << "Data available after writing: " << serial_data_available(pi, UARTHandle)<< " bytes" << endl;
40 | 
41 | 	//read and display one byte
42 | 	cout <<"Byte read = " << serial_read_byte(pi, UARTHandle)<< endl;
43 | 
44 | 	//check serial buffer again
45 | 	cout << "Data available after reading a byte: " << serial_data_available(pi, UARTHandle)<< " bytes" << endl;
46 | 
47 | 	//read and display last two bytes
48 |     char inA = serial_read_byte(pi, UARTHandle);
49 | 	cout <<"Byte read = " << inA << endl;
50 | 	char inB = serial_read_byte(pi, UARTHandle);
51 |     cout <<"Byte read = " << inB<< endl;
52 | 
53 | 	//close serial device and terminate connection with pigpio daemon
54 | 	serial_close(pi, UARTHandle);
55 | 	pigpio_stop(pi);
56 | 	return 0;
57 | }


--------------------------------------------------------------------------------
/chapter9/go_to_x.cpp:
--------------------------------------------------------------------------------
  1 | /*
  2 | go_to_x.cpp is a program to learn to write ROS nodes.
  3 | This node commands the turtlesim turtle to the X location
  4 | manually entered in the const double GOAL below.
  5 | Change the goal to any value from 0 to 11 and recompile with catkin_make before running.
  6 | This program is an accompaniment to the book Practical Robotics in C++
  7 | written by Lloyd Brombach and published by Packt Publishing
  8 | */
  9 | 
 10 | #include "ros/ros.h"
 11 | #include "geometry_msgs/Twist.h"
 12 | #include "geometry_msgs/Pose2D.h"
 13 | #include "turtlesim/Pose.h"
 14 | #include <cstdlib> //for abs()
 15 | #include <iostream>
 16 | 
 17 | using namespace std;
 18 | 
 19 | //declaring variables. 
 20 | geometry_msgs::Twist cmdVel;
 21 | geometry_msgs::Pose2D current;
 22 | geometry_msgs::Pose2D desired;
 23 | 
 24 | //change GOAL to any value from 0 to 11
 25 | const double GOAL = 1.5;
 26 | 
 27 | //the coefficient (or gain) for our linear velocity calculation
 28 | const double Kl = 1;
 29 | 
 30 | //the distance we are willing to accept as “close enough”
 31 | const double distanceTolerance = .1;
 32 | 
 33 | void misc_setup()
 34 | {
 35 |     desired.x = GOAL;
 36 |     cmdVel.linear.x = 0;
 37 |     cmdVel.linear.y = 0;
 38 |     cmdVel.linear.z = 0;
 39 |     cmdVel.angular.x = 0;
 40 |     cmdVel.angular.y = 0;
 41 |     cmdVel.linear.z = 0;
 42 | }
 43 | 
 44 | // callback function to update the current location 
 45 | //we will use later. We don’t use the y or theta data members
 46 | //in this tutorial, but I included them so you could see how
 47 | //to access them in the future.
 48 | void update_pose(const turtlesim::PoseConstPtr &currentPose)
 49 | {
 50 |     current.x = currentPose->x;
 51 |     current.y = currentPose->y;
 52 |     current.theta = currentPose->theta;
 53 | }
 54 | 
 55 | double getDistanceError()
 56 | {
 57 |     return desired.x - current.x;
 58 | }
 59 | 
 60 | //if we aren’t close enough, set the cmd_vel data member
 61 | //to be published later. Else set it to zero.
 62 | void set_velocity()
 63 | {
 64 |     if (abs(getDistanceError()) > distanceTolerance)
 65 |     {
 66 |         cmdVel.linear.x = Kl * getDistanceError();
 67 |     }
 68 |     else
 69 |     {
 70 |         cout << "I'm HERE!" << endl;
 71 |         cmdVel.linear.x = 0;
 72 |     }
 73 | }
 74 | 
 75 | int main(int argc, char **argv)
 76 | {
 77 |     misc_setup();  
 78 | 
 79 |     //register node “go_to_x” with roscore, then get a nodehandle
 80 |     ros::init(argc, argv, "go_to_x");
 81 |     ros::NodeHandle node;
 82 | 
 83 |     //Subscribe to topic and set callback **See detail in figure 9.10
 84 |     ros::Subscriber subCurrentPose =
 85 | 	node.subscribe("turtle1/pose", 0, update_pose);
 86 | 
 87 |     //Register node as publisher **See detail in figure 9.10
 88 |     ros::Publisher pubVelocity =
 89 |       node.advertise<geometry_msgs::Twist>("turtle1/cmd_vel", 0);
 90 | 
 91 |     //set the frequency that you want the loop below to execute at
 92 |     ros::Rate loop_rate(10); //10 cycles per second
 93 | 
 94 |     //execute this loop until connection is lost with ROS Master 
 95 |     while (ros::ok())
 96 |     {
 97 |         //call the callbacks waiting to be called.
 98 |         ros::spinOnce();
 99 | 
100 |         //call our own function after the callbacks are done
101 |         set_velocity();
102 | 
103 |         //publish the message that we internally called cmdVel
104 |         pubVelocity.publish(cmdVel);
105 | 
106 |         //output for you entertainment
107 |         cout << "goal x = " << desired.x << endl
108 |              << "current x = " << current.x <<  endl
109 |              << "  disError = " << getDistanceError() << endl
110 |              << "cmd_vel = " << cmdVel.linear.x<< endl;
111 | 
112 |         //We set the frequency for 10Hz, this sleeps as long as it 
113 |         //takes to keep that frequency
114 |         loop_rate.sleep();
115 |     }
116 | 
117 |     return 0;
118 | }


--------------------------------------------------------------------------------
/chapter9/go_to_xy.cpp:
--------------------------------------------------------------------------------
  1 | /*
  2 | go_to_xy.cpp is a program to learn to write ROS nodes.
  3 | This node commands the turtlesim turtle to the X location
  4 | manually entered in the accompanying program simple_goal_pub.cpp.
  5 | This program is an accompaniment to the book Practical Robotics in C++
  6 | written by Lloyd Brombach and published by Packt Publishing
  7 | */
  8 | 
  9 | #include "ros/ros.h"
 10 | #include "geometry_msgs/Twist.h"
 11 | #include "geometry_msgs/Pose2D.h"
 12 | #include "turtlesim/Pose.h"
 13 | #include <cstdlib> //for abs()
 14 | #include <iostream>
 15 | 
 16 | using namespace std;
 17 | 
 18 | //declaring variables. 
 19 | ros::Publisher pubVelocity;
 20 | geometry_msgs::Twist cmdVel;
 21 | geometry_msgs::Pose2D current;
 22 | geometry_msgs::Pose2D desired;
 23 | const double PI = 3.141592;
 24 | 
 25 | //the coefficient (or gain) for our angular velocity calculation
 26 | const double Ka = .5;   
 27 | 
 28 | //the coefficient (or gain) for our linear velocity calculation
 29 | const double Klv = .5;
 30 | 
 31 | //the angle we'll call "close enough"
 32 | const double angularTolerance = .1;
 33 | 
 34 | //the distance we are willing to accept as “close enough”
 35 | const double distanceTolerance = .1;
 36 | 
 37 | //a flag that we use to to start and stop calculating and publishing commands
 38 | bool waypointActive = false;
 39 | 
 40 | void misc_setup()
 41 | {
 42 |     desired.x = 5.54;  //this is just where turtle starts
 43 |     desired.y = 5.54;  //this is just where turtle starts
 44 |     cmdVel.linear.x = 0;
 45 |     cmdVel.linear.y = 0;
 46 |     cmdVel.linear.z = 0;
 47 |     cmdVel.angular.x = 0;
 48 |     cmdVel.angular.y = 0;
 49 |     cmdVel.linear.z = 0;
 50 | }
 51 | 
 52 | // callback function to update the current location 
 53 | void update_pose(const turtlesim::PoseConstPtr &currentPose)
 54 | {
 55 |     current.x = currentPose->x;
 56 |     current.y = currentPose->y;
 57 |     current.theta = currentPose->theta;
 58 | }
 59 | 
 60 | // callback function to update goal when goal message received
 61 | void update_goal(const geometry_msgs::Pose2D &desiredPose)
 62 | {
 63 |     desired.x = desiredPose.x;
 64 |     desired.y = desiredPose.y;
 65 |     waypointActive = true;
 66 | }
 67 | 
 68 | 
 69 | // calculates straight-line distance to goal
 70 | double getDistanceError()
 71 | {
 72 |     return sqrt(pow(desired.x - current.x, 2) + pow(desired.y - current.y, 2));
 73 | }
 74 | 
 75 | // calculates error in heading angle
 76 | double getAngularError()
 77 | {
 78 |     double deltaX = desired.x - current.x;
 79 |     double deltaY = desired.y - current.y;
 80 |     double thetaBearing = atan2(deltaY, deltaX);
 81 |     double angularError = thetaBearing - current.theta;
 82 |     angularError = (angularError > PI) ? angularError - (2 * PI) : angularError;
 83 |     angularError = (angularError < -PI) ? angularError + (2 * PI) : angularError;
 84 |     return angularError;
 85 | }
 86 | 
 87 | void set_velocity()
 88 | {
 89 |     cmdVel.linear.x = 0;
 90 |     cmdVel.linear.y = 0;
 91 |     cmdVel.linear.z = 0;
 92 |     cmdVel.angular.x = 0;
 93 |     cmdVel.angular.y = 0;
 94 |     cmdVel.linear.z = 0;
 95 | 
 96 |     double angularError = getAngularError();
 97 |     double distanceError = getDistanceError();
 98 | 
 99 |     if (waypointActive == true && abs(distanceError) > .15)
100 |     {
101 |         if (abs(angularError) < .1)
102 |         {
103 |             cmdVel.linear.x = Klv * distanceError;
104 |             cmdVel.angular.z = 0;
105 |         }
106 |         else
107 |         {
108 |             cmdVel.angular.z = Ka * angularError;
109 |             cmdVel.linear.x = 0;
110 |         }
111 |     }
112 |     else
113 |     {
114 |         cout << "I'm HERE!" << endl;
115 |         cmdVel.linear.x = 0;
116 |         cmdVel.angular.z = 0;
117 |         waypointActive = false;
118 |     }
119 | 
120 |     pubVelocity.publish(cmdVel);
121 | }
122 | 
123 | int main(int argc, char **argv)
124 | {
125 |     misc_setup();  
126 | 
127 |     //register node “go_to_x” with roscore, then get a nodehandle
128 |     ros::init(argc, argv, "go_to_x");
129 |     ros::NodeHandle node;
130 | 
131 |     //Subscribe to current pose topic and set callback to "update_pose"
132 |     ros::Subscriber subCurrentPose =
133 | 	    node.subscribe("turtle1/pose", 0, update_pose);
134 | 
135 |     //subscribe to goal location topic "waypoint" and set callback to "update_goal"
136 |     ros::Subscriber subDesiredPose = node.subscribe("waypoint", 0, update_goal, ros::TransportHints().tcpNoDelay());
137 |     pubVelocity = node.advertise<geometry_msgs::Twist>("turtle1/cmd_vel", 1);
138 | 
139 |     //Register node as publisher of type geometry_msgs::Twist and topic name turtle1/cmd_vel
140 |     ros::Publisher pubVelocity =
141 |       node.advertise<geometry_msgs::Twist>("turtle1/cmd_vel", 0);
142 | 
143 |     //set the frequency that you want the loop below to execute at
144 |     ros::Rate loop_rate(10); //10 cycles per second
145 | 
146 |     //execute this loop until connection is lost with ROS Master 
147 |     while (ros::ok())
148 |     {
149 |         //call the callbacks waiting to be called.
150 |         ros::spinOnce();
151 | 
152 |         //call our own function after the callbacks are done
153 |         set_velocity();
154 | 
155 |         //publish the message that we internally called cmdVel
156 |         pubVelocity.publish(cmdVel);
157 | 
158 |         //output for you entertainment
159 |         cout << "goal x = " << desired.x << endl
160 |              << "current x = " << current.x <<  endl
161 |              << "  disError = " << getDistanceError() << endl
162 |              << "cmd_vel = " << cmdVel.linear.x<< endl;
163 | 
164 |         //We set the frequency for 10Hz, this sleeps as long as it 
165 |         //takes to keep that frequency
166 |         loop_rate.sleep();
167 |     }
168 | 
169 |     return 0;
170 | }


--------------------------------------------------------------------------------
/chapter9/simple_goal_pub.cpp:
--------------------------------------------------------------------------------
 1 | /*
 2 | simplegoal_pub.cpp is a program to learn to write ROS publisher nodes.
 3 | This node publishes the Pose2D data type required by the go_to_xy.cpp program
 4 | that drives the turtlesim turtle to the location entered here. 
 5 | This program is an accompaniment to the book Practical Robotics in C++
 6 | written by Lloyd Brombach and published by Packt Publishing
 7 | */
 8 | 
 9 | #include "ros/ros.h"
10 | #include "geometry_msgs/Pose2D.h"
11 | #include <iostream>
12 | 
13 | geometry_msgs::Pose2D goal;
14 | 
15 | using namespace std;
16 | 
17 | void set_goal()
18 | {
19 |     cout << "enter goal x: ";
20 |     cin >> goal.x;
21 |     cout << goal.x << " now y: " << endl;
22 |     cin >> goal.y;
23 |     cout << endl;
24 | }
25 | 
26 | int main(int argc, char **argv)
27 | {
28 |     ros::init(argc, argv, "simple_goal_pub");
29 |     ros::NodeHandle node;
30 | 
31 |     ros::Publisher pubGoal = node.advertise<geometry_msgs::Pose2D>("waypoint", 0);
32 | 
33 |     while (ros::ok())
34 |     {
35 | 
36 |         set_goal();
37 |         pubGoal.publish(goal);
38 |         cout << "publishing" << endl;
39 |    
40 |     }
41 | 
42 |     return 0;
43 | }


--------------------------------------------------------------------------------