133 lines
35 KiB
HTML
133 lines
35 KiB
HTML
<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN" "http://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd">
|
|
<html xmlns="http://www.w3.org/1999/xhtml">
|
|
<head>
|
|
<meta http-equiv="Content-Type" content="text/xhtml;charset=UTF-8"/>
|
|
<meta http-equiv="X-UA-Compatible" content="IE=9"/>
|
|
<meta name="generator" content="Doxygen 1.8.11"/>
|
|
<title>OMPL Plannification: /home/blue/GitLab Repo/OMPL_Planning/Source Files/mapping_planning_tutorial/control_turtlebot/src/controller_quadrotor.py Source File</title>
|
|
<link href="tabs.css" rel="stylesheet" type="text/css"/>
|
|
<script type="text/javascript" src="jquery.js"></script>
|
|
<script type="text/javascript" src="dynsections.js"></script>
|
|
<link href="navtree.css" rel="stylesheet" type="text/css"/>
|
|
<script type="text/javascript" src="resize.js"></script>
|
|
<script type="text/javascript" src="navtreedata.js"></script>
|
|
<script type="text/javascript" src="navtree.js"></script>
|
|
<script type="text/javascript">
|
|
$(document).ready(initResizable);
|
|
$(window).load(resizeHeight);
|
|
</script>
|
|
<link href="search/search.css" rel="stylesheet" type="text/css"/>
|
|
<script type="text/javascript" src="search/searchdata.js"></script>
|
|
<script type="text/javascript" src="search/search.js"></script>
|
|
<script type="text/javascript">
|
|
$(document).ready(function() { init_search(); });
|
|
</script>
|
|
<link href="doxygen.css" rel="stylesheet" type="text/css" />
|
|
</head>
|
|
<body>
|
|
<div id="top"><!-- do not remove this div, it is closed by doxygen! -->
|
|
<div id="titlearea">
|
|
<table cellspacing="0" cellpadding="0">
|
|
<tbody>
|
|
<tr style="height: 56px;">
|
|
<td id="projectlogo"><img alt="Logo" src="flylab.png"/></td>
|
|
<td id="projectalign" style="padding-left: 0.5em;">
|
|
<div id="projectname">OMPL Plannification
|
|
</div>
|
|
<div id="projectbrief">Path planning with OMPL</div>
|
|
</td>
|
|
</tr>
|
|
</tbody>
|
|
</table>
|
|
</div>
|
|
<!-- end header part -->
|
|
<!-- Generated by Doxygen 1.8.11 -->
|
|
<script type="text/javascript">
|
|
var searchBox = new SearchBox("searchBox", "search",false,'Search');
|
|
</script>
|
|
<div id="navrow1" class="tabs">
|
|
<ul class="tablist">
|
|
<li><a href="index.html"><span>Main Page</span></a></li>
|
|
<li><a href="annotated.html"><span>Classes</span></a></li>
|
|
<li class="current"><a href="files.html"><span>Files</span></a></li>
|
|
<li>
|
|
<div id="MSearchBox" class="MSearchBoxInactive">
|
|
<span class="left">
|
|
<img id="MSearchSelect" src="search/mag_sel.png"
|
|
onmouseover="return searchBox.OnSearchSelectShow()"
|
|
onmouseout="return searchBox.OnSearchSelectHide()"
|
|
alt=""/>
|
|
<input type="text" id="MSearchField" value="Search" accesskey="S"
|
|
onfocus="searchBox.OnSearchFieldFocus(true)"
|
|
onblur="searchBox.OnSearchFieldFocus(false)"
|
|
onkeyup="searchBox.OnSearchFieldChange(event)"/>
|
|
</span><span class="right">
|
|
<a id="MSearchClose" href="javascript:searchBox.CloseResultsWindow()"><img id="MSearchCloseImg" border="0" src="search/close.png" alt=""/></a>
|
|
</span>
|
|
</div>
|
|
</li>
|
|
</ul>
|
|
</div>
|
|
<div id="navrow2" class="tabs2">
|
|
<ul class="tablist">
|
|
<li><a href="files.html"><span>File List</span></a></li>
|
|
</ul>
|
|
</div>
|
|
</div><!-- top -->
|
|
<div id="side-nav" class="ui-resizable side-nav-resizable">
|
|
<div id="nav-tree">
|
|
<div id="nav-tree-contents">
|
|
<div id="nav-sync" class="sync"></div>
|
|
</div>
|
|
</div>
|
|
<div id="splitbar" style="-moz-user-select:none;"
|
|
class="ui-resizable-handle">
|
|
</div>
|
|
</div>
|
|
<script type="text/javascript">
|
|
$(document).ready(function(){initNavTree('controller__quadrotor_8py_source.html','');});
|
|
</script>
|
|
<div id="doc-content">
|
|
<!-- window showing the filter options -->
|
|
<div id="MSearchSelectWindow"
|
|
onmouseover="return searchBox.OnSearchSelectShow()"
|
|
onmouseout="return searchBox.OnSearchSelectHide()"
|
|
onkeydown="return searchBox.OnSearchSelectKey(event)">
|
|
</div>
|
|
|
|
<!-- iframe showing the search results (closed by default) -->
|
|
<div id="MSearchResultsWindow">
|
|
<iframe src="javascript:void(0)" frameborder="0"
|
|
name="MSearchResults" id="MSearchResults">
|
|
</iframe>
|
|
</div>
|
|
|
|
<div class="header">
|
|
<div class="headertitle">
|
|
<div class="title">controller_quadrotor.py</div> </div>
|
|
</div><!--header-->
|
|
<div class="contents">
|
|
<div class="fragment"><div class="line"><a name="l00001"></a><span class="lineno"> 1</span> <span class="comment">#!/usr/bin/env python</span></div><div class="line"><a name="l00002"></a><span class="lineno"> 2</span> </div><div class="line"><a name="l00003"></a><span class="lineno"> 3</span> <span class="comment"># ROS imports</span></div><div class="line"><a name="l00004"></a><span class="lineno"> 4</span> <span class="keyword">import</span> roslib; roslib.load_manifest(<span class="stringliteral">'control_turtlebot'</span>)</div><div class="line"><a name="l00005"></a><span class="lineno"> 5</span> <span class="keyword">import</span> rospy</div><div class="line"><a name="l00006"></a><span class="lineno"> 6</span> <span class="keyword">import</span> tf</div><div class="line"><a name="l00007"></a><span class="lineno"> 7</span> <span class="keyword">import</span> math</div><div class="line"><a name="l00008"></a><span class="lineno"> 8</span> </div><div class="line"><a name="l00009"></a><span class="lineno"> 9</span> <span class="comment">#ROS messages</span></div><div class="line"><a name="l00010"></a><span class="lineno"> 10</span> <span class="keyword">from</span> geometry_msgs.msg <span class="keyword">import</span> Twist</div><div class="line"><a name="l00011"></a><span class="lineno"> 11</span> <span class="keyword">from</span> nav_msgs.msg <span class="keyword">import</span> Odometry</div><div class="line"><a name="l00012"></a><span class="lineno"> 12</span> <span class="keyword">from</span> control_turtlebot.srv <span class="keyword">import</span> GotoWaypoint, GotoWaypointResponse, FindPathToGoal, FindPathToGoalResponse, FindPathToGoalRequest</div><div class="line"><a name="l00013"></a><span class="lineno"> 13</span> </div><div class="line"><a name="l00014"></a><span class="lineno"> 14</span> <span class="comment">#Numpy</span></div><div class="line"><a name="l00015"></a><span class="lineno"> 15</span> <span class="keyword">import</span> numpy <span class="keyword">as</span> np</div><div class="line"><a name="l00016"></a><span class="lineno"> 16</span> </div><div class="line"><a name="l00017"></a><span class="lineno"><a class="line" href="classcontroller__quadrotor_1_1_controller.html"> 17</a></span> <span class="keyword">class </span><a class="code" href="classcontroller__quadrotor_1_1_controller.html">Controller</a>(object):</div><div class="line"><a name="l00018"></a><span class="lineno"> 18</span> </div><div class="line"><a name="l00019"></a><span class="lineno"> 19</span>  <span class="keyword">def </span>__init__(self):</div><div class="line"><a name="l00020"></a><span class="lineno"> 20</span>  self.<a class="code" href="classcontroller__quadrotor_1_1_controller.html#a0ec3e8972ee4f5b89280e641f4225462">odometry_sub_</a> = rospy.Subscriber(<span class="stringliteral">"/ground_truth/state"</span>, Odometry, self.<a class="code" href="classcontroller__quadrotor_1_1_controller.html#a62d58ec161deafcdc5a047dccefd5704">odomCallback</a>, queue_size = 1)</div><div class="line"><a name="l00021"></a><span class="lineno"> 21</span>  self.<a class="code" href="classcontroller__quadrotor_1_1_controller.html#adc8b42d7259128b78881acda3115e3e8">control_input_pub_</a> = rospy.Publisher(<span class="stringliteral">"/cmd_vel"</span>, Twist, queue_size = 1)</div><div class="line"><a name="l00022"></a><span class="lineno"> 22</span>  </div><div class="line"><a name="l00023"></a><span class="lineno"> 23</span>  self.<a class="code" href="classcontroller__quadrotor_1_1_controller.html#a3bd6ac9c2af10b852ba1689fa4017220">serv_</a> = rospy.Service(<span class="stringliteral">'/controller_turtlebot/goto'</span>, </div><div class="line"><a name="l00024"></a><span class="lineno"> 24</span>  GotoWaypoint, </div><div class="line"><a name="l00025"></a><span class="lineno"> 25</span>  self.<a class="code" href="classcontroller__quadrotor_1_1_controller.html#ac8bc2e178fc4d10cbf428c96718c207d">calculateControlInput</a>)</div><div class="line"><a name="l00026"></a><span class="lineno"> 26</span>  </div><div class="line"><a name="l00027"></a><span class="lineno"> 27</span>  self.<a class="code" href="classcontroller__quadrotor_1_1_controller.html#a38abbce68e678d0b90a6356557d82738">current_position_</a> = np.zeros(3)</div><div class="line"><a name="l00028"></a><span class="lineno"> 28</span>  self.<a class="code" href="classcontroller__quadrotor_1_1_controller.html#a6f0119ecb45333a44c85e3342623565a">current_orientation_</a> = 0.0</div><div class="line"><a name="l00029"></a><span class="lineno"> 29</span>  </div><div class="line"><a name="l00030"></a><span class="lineno"> 30</span>  self.<a class="code" href="classcontroller__quadrotor_1_1_controller.html#ac538fae52d472b815a3e33d9562c01d9">desired_position_</a> = np.zeros(3)</div><div class="line"><a name="l00031"></a><span class="lineno"> 31</span>  self.<a class="code" href="classcontroller__quadrotor_1_1_controller.html#a2ad92e83d7e173798eea5c18f1d2cae2">desired_orientation_</a> = 0.0</div><div class="line"><a name="l00032"></a><span class="lineno"> 32</span>  </div><div class="line"><a name="l00033"></a><span class="lineno"> 33</span>  </div><div class="line"><a name="l00034"></a><span class="lineno"> 34</span>  rospy.wait_for_service(<span class="stringliteral">'/controller_turtlebot/find_path_to_goal'</span>)</div><div class="line"><a name="l00035"></a><span class="lineno"> 35</span>  <span class="keywordflow">try</span>:</div><div class="line"><a name="l00036"></a><span class="lineno"> 36</span>  self.<a class="code" href="classcontroller__quadrotor_1_1_controller.html#af37f521c7b8f389dabaea148a647b525">find_path_to_goal_serv_</a> = rospy.ServiceProxy(<span class="stringliteral">'/controller_turtlebot/find_path_to_goal'</span>, FindPathToGoal)</div><div class="line"><a name="l00037"></a><span class="lineno"> 37</span>  <span class="keywordflow">except</span> rospy.ServiceException, e:</div><div class="line"><a name="l00038"></a><span class="lineno"> 38</span>  <span class="keywordflow">print</span> <span class="stringliteral">"Service call failed: %s"</span>%e</div><div class="line"><a name="l00039"></a><span class="lineno"> 39</span>  </div><div class="line"><a name="l00040"></a><span class="lineno"> 40</span>  <span class="keywordflow">return</span></div><div class="line"><a name="l00041"></a><span class="lineno"> 41</span>  </div><div class="line"><a name="l00042"></a><span class="lineno"> 42</span>  <span class="keyword">def </span>calculateControlInput(self, req):</div><div class="line"><a name="l00043"></a><span class="lineno"> 43</span>  </div><div class="line"><a name="l00044"></a><span class="lineno"> 44</span>  planner_request = FindPathToGoalRequest()</div><div class="line"><a name="l00045"></a><span class="lineno"> 45</span>  planner_request.new_goal = <span class="keyword">True</span></div><div class="line"><a name="l00046"></a><span class="lineno"> 46</span>  planner_request.goal_state_x = req.goal_state_x</div><div class="line"><a name="l00047"></a><span class="lineno"> 47</span>  planner_request.goal_state_y = req.goal_state_y</div><div class="line"><a name="l00048"></a><span class="lineno"> 48</span>  planner_request.goal_state_z = req.goal_state_z</div><div class="line"><a name="l00049"></a><span class="lineno"> 49</span>  </div><div class="line"><a name="l00050"></a><span class="lineno"> 50</span>  planner_response = self.<a class="code" href="classcontroller__quadrotor_1_1_controller.html#af37f521c7b8f389dabaea148a647b525">find_path_to_goal_serv_</a>(planner_request)</div><div class="line"><a name="l00051"></a><span class="lineno"> 51</span>  </div><div class="line"><a name="l00052"></a><span class="lineno"> 52</span>  <span class="keywordflow">for</span> pose <span class="keywordflow">in</span> planner_response.poses:</div><div class="line"><a name="l00053"></a><span class="lineno"> 53</span>  <span class="keywordflow">print</span> pose</div><div class="line"><a name="l00054"></a><span class="lineno"> 54</span>  control_input = Twist()</div><div class="line"><a name="l00055"></a><span class="lineno"> 55</span>  self.<a class="code" href="classcontroller__quadrotor_1_1_controller.html#ac538fae52d472b815a3e33d9562c01d9">desired_position_</a>[0] = pose.position.x</div><div class="line"><a name="l00056"></a><span class="lineno"> 56</span>  self.<a class="code" href="classcontroller__quadrotor_1_1_controller.html#ac538fae52d472b815a3e33d9562c01d9">desired_position_</a>[1] = pose.position.y</div><div class="line"><a name="l00057"></a><span class="lineno"> 57</span>  self.<a class="code" href="classcontroller__quadrotor_1_1_controller.html#ac538fae52d472b815a3e33d9562c01d9">desired_position_</a>[2] = pose.position.z</div><div class="line"><a name="l00058"></a><span class="lineno"> 58</span>  </div><div class="line"><a name="l00059"></a><span class="lineno"> 59</span>  loop_rate = rospy.Rate(100) <span class="comment"># 10Hz</span></div><div class="line"><a name="l00060"></a><span class="lineno"> 60</span>  orientation_approach = <span class="keyword">False</span></div><div class="line"><a name="l00061"></a><span class="lineno"> 61</span>  <span class="keywordflow">while</span> <span class="keywordflow">not</span> rospy.is_shutdown():</div><div class="line"><a name="l00062"></a><span class="lineno"> 62</span>  inc_x = self.<a class="code" href="classcontroller__quadrotor_1_1_controller.html#ac538fae52d472b815a3e33d9562c01d9">desired_position_</a>[0] - self.<a class="code" href="classcontroller__quadrotor_1_1_controller.html#a38abbce68e678d0b90a6356557d82738">current_position_</a>[0]</div><div class="line"><a name="l00063"></a><span class="lineno"> 63</span>  inc_y = self.<a class="code" href="classcontroller__quadrotor_1_1_controller.html#ac538fae52d472b815a3e33d9562c01d9">desired_position_</a>[1] - self.<a class="code" href="classcontroller__quadrotor_1_1_controller.html#a38abbce68e678d0b90a6356557d82738">current_position_</a>[1]</div><div class="line"><a name="l00064"></a><span class="lineno"> 64</span>  inc_z = self.<a class="code" href="classcontroller__quadrotor_1_1_controller.html#ac538fae52d472b815a3e33d9562c01d9">desired_position_</a>[2] - self.<a class="code" href="classcontroller__quadrotor_1_1_controller.html#a38abbce68e678d0b90a6356557d82738">current_position_</a>[2]</div><div class="line"><a name="l00065"></a><span class="lineno"> 65</span>  </div><div class="line"><a name="l00066"></a><span class="lineno"> 66</span>  self.<a class="code" href="classcontroller__quadrotor_1_1_controller.html#a2ad92e83d7e173798eea5c18f1d2cae2">desired_orientation_</a> = wrapAngle(math.atan2(inc_y, inc_x))</div><div class="line"><a name="l00067"></a><span class="lineno"> 67</span>  yaw_error = wrapAngle(self.<a class="code" href="classcontroller__quadrotor_1_1_controller.html#a2ad92e83d7e173798eea5c18f1d2cae2">desired_orientation_</a> - self.<a class="code" href="classcontroller__quadrotor_1_1_controller.html#a6f0119ecb45333a44c85e3342623565a">current_orientation_</a>)</div><div class="line"><a name="l00068"></a><span class="lineno"> 68</span>  distance_to_goal = math.sqrt(math.pow(inc_x, 2.0) + math.pow(inc_y, 2.0))</div><div class="line"><a name="l00069"></a><span class="lineno"> 69</span>  </div><div class="line"><a name="l00070"></a><span class="lineno"> 70</span>  <span class="keywordflow">if</span> abs(yaw_error) > 0.04 <span class="keywordflow">and</span> <span class="keywordflow">not</span> orientation_approach:</div><div class="line"><a name="l00071"></a><span class="lineno"> 71</span>  control_input.angular.x = 0.0</div><div class="line"><a name="l00072"></a><span class="lineno"> 72</span>  control_input.angular.y = 0.0</div><div class="line"><a name="l00073"></a><span class="lineno"> 73</span>  control_input.angular.z = yaw_error * 1.0</div><div class="line"><a name="l00074"></a><span class="lineno"> 74</span>  </div><div class="line"><a name="l00075"></a><span class="lineno"> 75</span>  control_input.linear.x = 0.0</div><div class="line"><a name="l00076"></a><span class="lineno"> 76</span>  control_input.linear.y = 0.0</div><div class="line"><a name="l00077"></a><span class="lineno"> 77</span>  control_input.linear.z = 0.0</div><div class="line"><a name="l00078"></a><span class="lineno"> 78</span>  <span class="keywordflow">else</span>:</div><div class="line"><a name="l00079"></a><span class="lineno"> 79</span>  control_input.angular.x = 0.0</div><div class="line"><a name="l00080"></a><span class="lineno"> 80</span>  control_input.angular.y = 0.0</div><div class="line"><a name="l00081"></a><span class="lineno"> 81</span>  control_input.angular.z = 0.0</div><div class="line"><a name="l00082"></a><span class="lineno"> 82</span>  </div><div class="line"><a name="l00083"></a><span class="lineno"> 83</span>  orientation_approach = <span class="keyword">True</span></div><div class="line"><a name="l00084"></a><span class="lineno"> 84</span>  liner_speed = abs(distance_to_goal) * 0.5</div><div class="line"><a name="l00085"></a><span class="lineno"> 85</span>  altitude_speed = inc_z * 0.5</div><div class="line"><a name="l00086"></a><span class="lineno"> 86</span>  </div><div class="line"><a name="l00087"></a><span class="lineno"> 87</span>  <span class="keywordflow">if</span> liner_speed < 0.1:</div><div class="line"><a name="l00088"></a><span class="lineno"> 88</span>  control_input.linear.x = 0.1</div><div class="line"><a name="l00089"></a><span class="lineno"> 89</span>  <span class="keywordflow">elif</span> liner_speed > 0.2:</div><div class="line"><a name="l00090"></a><span class="lineno"> 90</span>  control_input.linear.x = 0.2</div><div class="line"><a name="l00091"></a><span class="lineno"> 91</span>  <span class="keywordflow">else</span> :</div><div class="line"><a name="l00092"></a><span class="lineno"> 92</span>  control_input.linear.x = liner_speed</div><div class="line"><a name="l00093"></a><span class="lineno"> 93</span> </div><div class="line"><a name="l00094"></a><span class="lineno"> 94</span>  <span class="keywordflow">if</span> abs(altitude_speed) < 0.1:</div><div class="line"><a name="l00095"></a><span class="lineno"> 95</span>  <span class="keywordflow">if</span> altitude_speed ==0 :</div><div class="line"><a name="l00096"></a><span class="lineno"> 96</span>  control_input.linear.z = 0</div><div class="line"><a name="l00097"></a><span class="lineno"> 97</span>  <span class="keywordflow">elif</span> altitude_speed > 0 :</div><div class="line"><a name="l00098"></a><span class="lineno"> 98</span>  control_input.linear.z = 0.1</div><div class="line"><a name="l00099"></a><span class="lineno"> 99</span>  <span class="keywordflow">else</span>:</div><div class="line"><a name="l00100"></a><span class="lineno"> 100</span>  control_input.linear.z = -0.1</div><div class="line"><a name="l00101"></a><span class="lineno"> 101</span> </div><div class="line"><a name="l00102"></a><span class="lineno"> 102</span>  <span class="keywordflow">elif</span> abs(altitude_speed) > 0.2:</div><div class="line"><a name="l00103"></a><span class="lineno"> 103</span>  <span class="keywordflow">if</span> altitude_speed > 0 :</div><div class="line"><a name="l00104"></a><span class="lineno"> 104</span>  control_input.linear.z = 0.2</div><div class="line"><a name="l00105"></a><span class="lineno"> 105</span>  <span class="keywordflow">else</span>:</div><div class="line"><a name="l00106"></a><span class="lineno"> 106</span>  control_input.linear.z = - 0.2</div><div class="line"><a name="l00107"></a><span class="lineno"> 107</span>  <span class="keywordflow">else</span>:</div><div class="line"><a name="l00108"></a><span class="lineno"> 108</span>  control_input.linear.z = altitude_speed</div><div class="line"><a name="l00109"></a><span class="lineno"> 109</span> </div><div class="line"><a name="l00110"></a><span class="lineno"> 110</span>  control_input.linear.y = 0.0</div><div class="line"><a name="l00111"></a><span class="lineno"> 111</span>  </div><div class="line"><a name="l00112"></a><span class="lineno"> 112</span>  </div><div class="line"><a name="l00113"></a><span class="lineno"> 113</span>  self.control_input_pub_.publish(control_input)</div><div class="line"><a name="l00114"></a><span class="lineno"> 114</span>  </div><div class="line"><a name="l00115"></a><span class="lineno"> 115</span>  rospy.logdebug(<span class="stringliteral">"%s: current position: [%f, %f]\n"</span>, rospy.get_name(), self.<a class="code" href="classcontroller__quadrotor_1_1_controller.html#a38abbce68e678d0b90a6356557d82738">current_position_</a>[0], self.<a class="code" href="classcontroller__quadrotor_1_1_controller.html#a38abbce68e678d0b90a6356557d82738">current_position_</a>[1])</div><div class="line"><a name="l00116"></a><span class="lineno"> 116</span>  rospy.logdebug(<span class="stringliteral">"%s: desired position: [%f, %f]\n"</span>, rospy.get_name(), self.<a class="code" href="classcontroller__quadrotor_1_1_controller.html#ac538fae52d472b815a3e33d9562c01d9">desired_position_</a>[0], self.<a class="code" href="classcontroller__quadrotor_1_1_controller.html#ac538fae52d472b815a3e33d9562c01d9">desired_position_</a>[1])</div><div class="line"><a name="l00117"></a><span class="lineno"> 117</span>  rospy.logdebug(<span class="stringliteral">"%s: yaw_error: %f\n"</span>, rospy.get_name(), yaw_error)</div><div class="line"><a name="l00118"></a><span class="lineno"> 118</span>  rospy.logdebug(<span class="stringliteral">"%s: current orientation: %f\n"</span>, rospy.get_name(), self.<a class="code" href="classcontroller__quadrotor_1_1_controller.html#a6f0119ecb45333a44c85e3342623565a">current_orientation_</a>)</div><div class="line"><a name="l00119"></a><span class="lineno"> 119</span>  rospy.logdebug(<span class="stringliteral">"%s: desired orientation: %f\n"</span>, rospy.get_name(), self.<a class="code" href="classcontroller__quadrotor_1_1_controller.html#a2ad92e83d7e173798eea5c18f1d2cae2">desired_orientation_</a>)</div><div class="line"><a name="l00120"></a><span class="lineno"> 120</span>  rospy.logdebug(<span class="stringliteral">"%s: distance_to_goal: %f\n"</span>, rospy.get_name(), distance_to_goal)</div><div class="line"><a name="l00121"></a><span class="lineno"> 121</span>  rospy.logdebug(<span class="stringliteral">"%s: control_input.linear.x %f\n"</span>, rospy.get_name(), control_input.linear.x)</div><div class="line"><a name="l00122"></a><span class="lineno"> 122</span>  rospy.logdebug(<span class="stringliteral">"%s: control_input.angular.z %f\n"</span>, rospy.get_name(), control_input.angular.z)</div><div class="line"><a name="l00123"></a><span class="lineno"> 123</span>  </div><div class="line"><a name="l00124"></a><span class="lineno"> 124</span>  <span class="keywordflow">if</span> distance_to_goal <= 0.2:</div><div class="line"><a name="l00125"></a><span class="lineno"> 125</span>  <span class="keywordflow">break</span></div><div class="line"><a name="l00126"></a><span class="lineno"> 126</span>  loop_rate.sleep()</div><div class="line"><a name="l00127"></a><span class="lineno"> 127</span>  </div><div class="line"><a name="l00128"></a><span class="lineno"> 128</span>  <span class="keywordflow">return</span> GotoWaypointResponse()</div><div class="line"><a name="l00129"></a><span class="lineno"> 129</span> </div><div class="line"><a name="l00130"></a><span class="lineno"> 130</span>  <span class="keyword">def </span>odomCallback(self, odometry_msg):</div><div class="line"><a name="l00131"></a><span class="lineno"> 131</span>  self.<a class="code" href="classcontroller__quadrotor_1_1_controller.html#a38abbce68e678d0b90a6356557d82738">current_position_</a>[0] = odometry_msg.pose.pose.position.x</div><div class="line"><a name="l00132"></a><span class="lineno"> 132</span>  self.<a class="code" href="classcontroller__quadrotor_1_1_controller.html#a38abbce68e678d0b90a6356557d82738">current_position_</a>[1] = odometry_msg.pose.pose.position.y</div><div class="line"><a name="l00133"></a><span class="lineno"> 133</span>  self.<a class="code" href="classcontroller__quadrotor_1_1_controller.html#a38abbce68e678d0b90a6356557d82738">current_position_</a>[2] = odometry_msg.pose.pose.position.z</div><div class="line"><a name="l00134"></a><span class="lineno"> 134</span>  (r, p, y) = tf.transformations.euler_from_quaternion([odometry_msg.pose.pose.orientation.x, odometry_msg.pose.pose.orientation.y, odometry_msg.pose.pose.orientation.z, odometry_msg.pose.pose.orientation.w])</div><div class="line"><a name="l00135"></a><span class="lineno"> 135</span>  self.<a class="code" href="classcontroller__quadrotor_1_1_controller.html#a6f0119ecb45333a44c85e3342623565a">current_orientation_</a> = wrapAngle(y)</div><div class="line"><a name="l00136"></a><span class="lineno"> 136</span>  <span class="keywordflow">return</span></div><div class="line"><a name="l00137"></a><span class="lineno"> 137</span> </div><div class="line"><a name="l00138"></a><span class="lineno"> 138</span> <span class="keyword">def </span>wrapAngle(angle):</div><div class="line"><a name="l00139"></a><span class="lineno"> 139</span>  <span class="stringliteral">"""wrapAngle</span></div><div class="line"><a name="l00140"></a><span class="lineno"> 140</span> <span class="stringliteral"> </span></div><div class="line"><a name="l00141"></a><span class="lineno"> 141</span> <span class="stringliteral"> Calculates angles values between 0 and 2pi"""</span></div><div class="line"><a name="l00142"></a><span class="lineno"> 142</span>  <span class="keywordflow">return</span> (angle + ( 2.0 * math.pi * math.floor( ( math.pi - angle ) / ( 2.0 * math.pi ) ) ) )</div><div class="line"><a name="l00143"></a><span class="lineno"> 143</span> </div><div class="line"><a name="l00144"></a><span class="lineno"> 144</span> <span class="keywordflow">if</span> __name__ == <span class="stringliteral">'__main__'</span>:</div><div class="line"><a name="l00145"></a><span class="lineno"> 145</span>  rospy.init_node(<span class="stringliteral">'control_quadrotor'</span>, log_level=rospy.INFO)</div><div class="line"><a name="l00146"></a><span class="lineno"> 146</span>  rospy.loginfo(<span class="stringliteral">"%s: starting quadrotor controller"</span>, rospy.get_name())</div><div class="line"><a name="l00147"></a><span class="lineno"> 147</span> </div><div class="line"><a name="l00148"></a><span class="lineno"> 148</span>  controller = <a class="code" href="classcontroller__quadrotor_1_1_controller.html">Controller</a>()</div><div class="line"><a name="l00149"></a><span class="lineno"> 149</span>  rospy.spin()</div><div class="ttc" id="classcontroller__quadrotor_1_1_controller_html_ac8bc2e178fc4d10cbf428c96718c207d"><div class="ttname"><a href="classcontroller__quadrotor_1_1_controller.html#ac8bc2e178fc4d10cbf428c96718c207d">controller_quadrotor.Controller.calculateControlInput</a></div><div class="ttdeci">def calculateControlInput(self, req)</div><div class="ttdef"><b>Definition:</b> <a href="controller__quadrotor_8py_source.html#l00042">controller_quadrotor.py:42</a></div></div>
|
|
<div class="ttc" id="classcontroller__quadrotor_1_1_controller_html_a2ad92e83d7e173798eea5c18f1d2cae2"><div class="ttname"><a href="classcontroller__quadrotor_1_1_controller.html#a2ad92e83d7e173798eea5c18f1d2cae2">controller_quadrotor.Controller.desired_orientation_</a></div><div class="ttdeci">desired_orientation_</div><div class="ttdef"><b>Definition:</b> <a href="controller__quadrotor_8py_source.html#l00031">controller_quadrotor.py:31</a></div></div>
|
|
<div class="ttc" id="classcontroller__quadrotor_1_1_controller_html_adc8b42d7259128b78881acda3115e3e8"><div class="ttname"><a href="classcontroller__quadrotor_1_1_controller.html#adc8b42d7259128b78881acda3115e3e8">controller_quadrotor.Controller.control_input_pub_</a></div><div class="ttdeci">control_input_pub_</div><div class="ttdef"><b>Definition:</b> <a href="controller__quadrotor_8py_source.html#l00021">controller_quadrotor.py:21</a></div></div>
|
|
<div class="ttc" id="classcontroller__quadrotor_1_1_controller_html"><div class="ttname"><a href="classcontroller__quadrotor_1_1_controller.html">controller_quadrotor.Controller</a></div><div class="ttdef"><b>Definition:</b> <a href="controller__quadrotor_8py_source.html#l00017">controller_quadrotor.py:17</a></div></div>
|
|
<div class="ttc" id="classcontroller__quadrotor_1_1_controller_html_a3bd6ac9c2af10b852ba1689fa4017220"><div class="ttname"><a href="classcontroller__quadrotor_1_1_controller.html#a3bd6ac9c2af10b852ba1689fa4017220">controller_quadrotor.Controller.serv_</a></div><div class="ttdeci">serv_</div><div class="ttdef"><b>Definition:</b> <a href="controller__quadrotor_8py_source.html#l00023">controller_quadrotor.py:23</a></div></div>
|
|
<div class="ttc" id="classcontroller__quadrotor_1_1_controller_html_a0ec3e8972ee4f5b89280e641f4225462"><div class="ttname"><a href="classcontroller__quadrotor_1_1_controller.html#a0ec3e8972ee4f5b89280e641f4225462">controller_quadrotor.Controller.odometry_sub_</a></div><div class="ttdeci">odometry_sub_</div><div class="ttdef"><b>Definition:</b> <a href="controller__quadrotor_8py_source.html#l00020">controller_quadrotor.py:20</a></div></div>
|
|
<div class="ttc" id="classcontroller__quadrotor_1_1_controller_html_a38abbce68e678d0b90a6356557d82738"><div class="ttname"><a href="classcontroller__quadrotor_1_1_controller.html#a38abbce68e678d0b90a6356557d82738">controller_quadrotor.Controller.current_position_</a></div><div class="ttdeci">current_position_</div><div class="ttdef"><b>Definition:</b> <a href="controller__quadrotor_8py_source.html#l00027">controller_quadrotor.py:27</a></div></div>
|
|
<div class="ttc" id="classcontroller__quadrotor_1_1_controller_html_a62d58ec161deafcdc5a047dccefd5704"><div class="ttname"><a href="classcontroller__quadrotor_1_1_controller.html#a62d58ec161deafcdc5a047dccefd5704">controller_quadrotor.Controller.odomCallback</a></div><div class="ttdeci">def odomCallback(self, odometry_msg)</div><div class="ttdef"><b>Definition:</b> <a href="controller__quadrotor_8py_source.html#l00130">controller_quadrotor.py:130</a></div></div>
|
|
<div class="ttc" id="classcontroller__quadrotor_1_1_controller_html_a6f0119ecb45333a44c85e3342623565a"><div class="ttname"><a href="classcontroller__quadrotor_1_1_controller.html#a6f0119ecb45333a44c85e3342623565a">controller_quadrotor.Controller.current_orientation_</a></div><div class="ttdeci">current_orientation_</div><div class="ttdef"><b>Definition:</b> <a href="controller__quadrotor_8py_source.html#l00028">controller_quadrotor.py:28</a></div></div>
|
|
<div class="ttc" id="classcontroller__quadrotor_1_1_controller_html_af37f521c7b8f389dabaea148a647b525"><div class="ttname"><a href="classcontroller__quadrotor_1_1_controller.html#af37f521c7b8f389dabaea148a647b525">controller_quadrotor.Controller.find_path_to_goal_serv_</a></div><div class="ttdeci">find_path_to_goal_serv_</div><div class="ttdef"><b>Definition:</b> <a href="controller__quadrotor_8py_source.html#l00036">controller_quadrotor.py:36</a></div></div>
|
|
<div class="ttc" id="classcontroller__quadrotor_1_1_controller_html_ac538fae52d472b815a3e33d9562c01d9"><div class="ttname"><a href="classcontroller__quadrotor_1_1_controller.html#ac538fae52d472b815a3e33d9562c01d9">controller_quadrotor.Controller.desired_position_</a></div><div class="ttdeci">desired_position_</div><div class="ttdef"><b>Definition:</b> <a href="controller__quadrotor_8py_source.html#l00030">controller_quadrotor.py:30</a></div></div>
|
|
</div><!-- fragment --></div><!-- contents -->
|
|
</div><!-- doc-content -->
|
|
<!-- start footer part -->
|
|
<div id="nav-path" class="navpath"><!-- id is needed for treeview function! -->
|
|
<ul>
|
|
<li class="navelem"><a class="el" href="dir_a2ed51f7c7095589dc0c78a28547d1d4.html">Source Files</a></li><li class="navelem"><a class="el" href="dir_50758af9fa44b143c39a433e879132a5.html">mapping_planning_tutorial</a></li><li class="navelem"><a class="el" href="dir_2b04689da8f92abcb133442f492c8f6b.html">control_turtlebot</a></li><li class="navelem"><a class="el" href="dir_5fee08efe95e6d722a092087df25e95a.html">src</a></li><li class="navelem"><b>controller_quadrotor.py</b></li>
|
|
<li class="footer">Generated by
|
|
<a href="http://www.doxygen.org/index.html">
|
|
<img class="footer" src="doxygen.png" alt="doxygen"/></a> 1.8.11 </li>
|
|
</ul>
|
|
</div>
|
|
</body>
|
|
</html>
|