|
1 | | -//This file handles communication between the gazebo simulation and the rover |
| 1 | +// This file handles communication between the gazebo simulation and the rover |
2 | 2 |
|
3 | | -//Since the simulation cannot get data from the real rovers, this package converts |
4 | | -//the behavior package data into readable data for the gazebo simulation to understand |
| 3 | +// Since the simulation cannot get data from the real rovers, this package converts |
| 4 | +// the behavior package data into readable data for the gazebo simulation to understand |
5 | 5 | #include "sbridge.h" |
6 | 6 |
|
7 | | -//sbridge constructor |
| 7 | +// sbridge constructor |
8 | 8 | sbridge::sbridge(std::string publishedName) { |
9 | 9 |
|
10 | | - //Create a new node that will communicate with ROS |
| 10 | + // Create a new node that will communicate with ROS |
11 | 11 | ros::NodeHandle sNH; |
12 | 12 |
|
13 | 13 |
|
14 | | - //Create a drive control subscriber |
15 | | - //publishedName + '/driveControl" == the topic to subscribe to |
16 | | - //10 == the incoming message queue size |
17 | | - //&sbridge::cmdHandler == callback to class method first parameter |
18 | | - //this == calllback to class method second parameter |
| 14 | + // Create a drive control subscriber |
| 15 | + // publishedName + '/driveControl" == the topic to subscribe to |
| 16 | + // 10 == the incoming message queue size |
| 17 | + // &sbridge::cmdHandler == callback to class method first parameter |
| 18 | + // this == calllback to class method second parameter |
19 | 19 | driveControlSubscriber = sNH.subscribe((publishedName + "/driveControl"), 10, &sbridge::cmdHandler, this); |
20 | 20 |
|
21 | | - //Create a heatbeat publisher to show that the node is still alive and running |
22 | | - //publishedName + "/sbridge/heartbeat" == topic to publish to |
23 | | - //1 == the outgoing message queue |
24 | | - //false == disables the latching connection. It does not save the last message nor automatically send |
25 | | - //automatically send it to future connecting subscribers |
26 | | - //this parameter is optional |
| 21 | + // Create a heatbeat publisher to show that the node is still alive and running |
| 22 | + // publishedName + "/sbridge/heartbeat" == topic to publish to |
| 23 | + // 1 == the outgoing message queue |
| 24 | + // false == disables the latching connection. It does not save the last message nor automatically send |
| 25 | + // automatically send it to future connecting subscribers |
| 26 | + // this parameter is optional |
27 | 27 | heartbeatPublisher = sNH.advertise<std_msgs::String>((publishedName + "/sbridge/heartbeat"), 1, false); |
28 | 28 |
|
29 | | - //Create a skidsteer publisher to |
| 29 | + // Create a skidsteer publisher to |
30 | 30 | skidsteerPublish = sNH.advertise<geometry_msgs::Twist>((publishedName + "/skidsteer"), 10); |
31 | 31 |
|
32 | | - //Create a infoLog publisher to send log information to the log box in the GUI |
| 32 | + // Create a infoLog publisher to send log information to the log box in the GUI |
33 | 33 | infoLogPublisher = sNH.advertise<std_msgs::String>("/infoLog", 1, true); |
34 | 34 |
|
35 | | - //Publishes a heartbeat every 2 seconds |
| 35 | + // Publishes a heartbeat every 2 seconds |
36 | 36 | float heartbeat_publish_interval = 2; |
37 | 37 |
|
38 | | - //Schedules a callback at the heartbeat_publish_interval |
39 | | - //This is not a realtime timer |
40 | | - //ros::Duration(heartbeat_publish_interval) == period between calls to the callback |
41 | | - //&sbridge::publishHeartBeatTimerEventHandler == callback to class method first parameter |
42 | | - //this == callback to class method second parameter |
| 38 | + // Schedules a callback at the heartbeat_publish_interval |
| 39 | + // This is not a realtime timer |
| 40 | + // ros::Duration(heartbeat_publish_interval) == period between calls to the callback |
| 41 | + // &sbridge::publishHeartBeatTimerEventHandler == callback to class method first parameter |
| 42 | + // this == callback to class method second parameter |
43 | 43 | publish_heartbeat_timer = sNH.createTimer(ros::Duration(heartbeat_publish_interval), &sbridge::publishHeartBeatTimerEventHandler, this); |
44 | 44 |
|
45 | 45 | ROS_INFO("constructor"); |
46 | 46 |
|
47 | 47 | } |
48 | 48 |
|
49 | | -//Command Handler |
50 | | -//PWM (pulse with modulation) values to linear values |
| 49 | +// Command Handler |
| 50 | +// PWM (pulse with modulation) values to linear values |
51 | 51 | void sbridge::cmdHandler(const geometry_msgs::Twist::ConstPtr& message) { |
52 | | - //Gets and sets the linear PWM value |
| 52 | + // Gets and sets the linear PWM value |
53 | 53 | double left = (message->linear.x); |
54 | | - //Gets and sets the angular PWM value |
| 54 | + // Gets and sets the angular PWM value |
55 | 55 | double right = (message->angular.z); |
56 | 56 |
|
57 | | - //Set max values |
58 | | - float max_turn_rate = 4.5; //radians per second |
59 | | - float max_linear_velocity = 0.65; // meters per second |
| 57 | + // Set max values |
| 58 | + float max_turn_rate = 4.5; // radians per second |
| 59 | + float max_linear_velocity = 0.65; // meters per second |
60 | 60 |
|
61 | | - //Temperary Variables to store the velocity values |
62 | | - //Angular |
| 61 | + // Temperary Variables to store the velocity values |
| 62 | + // Angular |
63 | 63 | float turn = 0; |
64 | | - //Linear |
| 64 | + // Linear |
65 | 65 | float forward = 0; |
66 | 66 |
|
67 | 67 | float linearVel = (left + right)/2; |
68 | 68 | float angularVel = (right-left)/2; |
69 | 69 |
|
70 | | - //Converts the PWM values to velocity values |
71 | | - //Necessary arbitrary conversion value |
| 70 | + // Converts the PWM values to velocity values |
| 71 | + // Necessary hand-tuned conversion value |
72 | 72 | turn = angularVel/55; |
73 | | - //Necessary arbitrary conversion value |
| 73 | + // Necessary hand-tuned conversion value |
74 | 74 | forward = linearVel/390; |
75 | 75 |
|
76 | | - //If the rover is moving forward and turning at the same time, it will not have enough power to carry out |
77 | | - //each of those at it's full values, so we need to lower this value so the PIDs can compensate for both |
78 | | - //the linear and angular velocity |
| 76 | + // If the rover is moving forward and turning at the same time, it will not have enough power to carry out |
| 77 | + // each of those at it's full values, so we need to lower this value so the PIDs can compensate for both |
| 78 | + // the linear and angular velocity |
79 | 79 | if (forward >= 150){ |
80 | | - //Lower forward velocity value by necessary arbitrary conversion value |
| 80 | + // Lower forward velocity value by necessary hand-tuned conversion value |
81 | 81 | forward -= (abs(turn)/5); |
82 | 82 | } |
83 | 83 |
|
84 | | - //If the linear velocity (PWM value) is moving in the forward direction (value is greater than or equal to 0), |
85 | | - //but forward (velocity) gets converted to a non-positive value (for reverse movement) |
| 84 | + // If the linear velocity (PWM value) is moving in the forward direction (value is greater than or equal to 0), |
| 85 | + // but forward (velocity) gets converted to a non-positive value (for reverse movement) |
86 | 86 | if (linearVel >= 0 && forward <= 0) |
87 | 87 | { |
88 | | - //Do not move (set forward velocity to 0) |
| 88 | + // Do not move (set forward velocity to 0) |
89 | 89 | forward = 0; |
90 | 90 | } |
91 | 91 |
|
92 | | - //If the linear velocity (PWM value) is moving in the reverse direction (value is greater than or equal to 0), |
93 | | - //but the foward (velocity) variable gets converted to a non-negative value (for forwards movement) |
| 92 | + // If the linear velocity (PWM value) is moving in the reverse direction (value is greater than or equal to 0), |
| 93 | + // but the foward (velocity) variable gets converted to a non-negative value (for forwards movement) |
94 | 94 | if (linearVel <= 0 && forward >= 0) |
95 | 95 | { |
96 | | - //Do not move (set forward velocity to 0) |
| 96 | + // Do not move (set forward velocity to 0) |
97 | 97 | forward = 0; |
98 | 98 | } |
99 | 99 |
|
100 | | - //If the absolute value of the forward value (float) is greater than or equal to the max_linear_velocity |
| 100 | + // If the absolute value of the forward value (float) is greater than or equal to the max_linear_velocity |
101 | 101 | if (fabs(forward) >= max_linear_velocity) { |
102 | 102 | forward = forward/fabs(forward) * max_linear_velocity; |
103 | 103 | } |
104 | 104 |
|
105 | | - if (fabs(turn) >= max_turn_rate) { //max value needs tuning |
| 105 | + if (fabs(turn) >= max_turn_rate) { // max value needs tuning |
106 | 106 | turn = turn/fabs(turn) * max_turn_rate; |
107 | 107 | } |
108 | 108 |
|
109 | | - //Sets the converted (PWM to velocity) values into the variable that the simulation can read |
110 | | - //Linear velocity |
| 109 | + // Sets the converted (PWM to velocity) values into the variable that the simulation can read |
| 110 | + // Linear velocity |
111 | 111 | velocity.linear.x = forward, |
112 | | - //Angular velocity |
| 112 | + // Angular velocity |
113 | 113 | velocity.angular.z = turn; |
114 | | - //Publish velocity information to rotary wheel movement |
| 114 | + // Publish velocity information to rotary wheel movement |
115 | 115 | skidsteerPublish.publish(velocity); |
116 | 116 | } |
117 | 117 |
|
118 | 118 |
|
119 | 119 | void sbridge::publishHeartBeatTimerEventHandler(const ros::TimerEvent& event) { |
120 | 120 | std_msgs::String msg; |
121 | 121 | msg.data = ""; |
122 | | - //Publish the heartbeat message |
| 122 | + // Publish the heartbeat message |
123 | 123 | heartbeatPublisher.publish(msg); |
124 | 124 |
|
125 | | - //Log information for the timer in seconds and nanoseconds |
| 125 | + // Log information for the timer in seconds and nanoseconds |
126 | 126 | ROS_INFO("%ds, %dnsec", event.last_real.sec, event.last_real.nsec); |
127 | 127 | } |
128 | 128 |
|
129 | | -//Destructor |
| 129 | +// Destructor |
130 | 130 | sbridge::~sbridge() { |
131 | 131 | } |
0 commit comments