-navigation stack fixes
authorErik Andresen <erik@vontaene.de>
Sun, 11 Oct 2015 09:21:22 +0000 (11:21 +0200)
committerErik Andresen <erik@vontaene.de>
Sun, 11 Oct 2015 09:21:22 +0000 (11:21 +0200)
-added cmd_vel_mux
-robot_state_publisher testing
-srf sensor timing fixes
-added square.py

cfg/cmd_vel_mux.yaml [new file with mode: 0644]
config/base_local_planner_params.yaml
config/costmap_common_params.yaml
config/local_costmap_params.yaml
launch/move_base.launch
launch/robot_localization.launch [new file with mode: 0644]
launch/teleop.launch
launch/wild_thumper.launch
scripts/square.py [new file with mode: 0755]
scripts/wt_node.py

diff --git a/cfg/cmd_vel_mux.yaml b/cfg/cmd_vel_mux.yaml
new file mode 100644 (file)
index 0000000..6ed42cb
--- /dev/null
@@ -0,0 +1,16 @@
+subscribers:
+  - name:        "Default input"
+    topic:       "/cmd_vel"
+    timeout:     0.1
+    priority:    0
+    short_desc:  "The default cmd_vel, controllers unaware that we are multiplexing cmd_vel should come here"
+  - name:        "Navigation stack"
+    topic:       "/move_base/cmd_vel"
+    timeout:     0.5
+    priority:    1
+    short_desc:  "Navigation stack controller"
+  - name:        "Wii Teleop"
+    topic:       "/turtlebot_teleop_joystick/cmd_vel"
+    timeout:     0.1
+    priority:    10
+publisher:       "/cmd_vel_out"
index eaff7f4..212e2b6 100644 (file)
@@ -1,5 +1,5 @@
 TrajectoryPlannerROS:
-  min_vel_x: 0.1
+  min_vel_x: 0.2
   max_vel_x: 0.3
   min_vel_theta: -0.4
   max_vel_theta: 0.4
index 2ddecc4..6e4b370 100644 (file)
@@ -1,5 +1,5 @@
 footprint: [[0.14, 0.16], [-0.14, 0.16], [-0.14, -0.16], [0.14, -0.16]]
-inflation_radius: 0.55
+inflation_radius: 0.5
 
 obstacle_layer:
   observation_sources: laser_scan_sensor
index d4fd59e..391e338 100644 (file)
@@ -10,9 +10,10 @@ local_costmap:
   resolution: 0.05
   plugins:
   - {name: obstacle_layer, type: 'costmap_2d::ObstacleLayer'}
-  - {name: sonar_layer,   type: "range_sensor_layer::RangeSensorLayer"}
+  - {name: range_layer,   type: "range_sensor_layer::RangeSensorLayer"}
   - {name: inflation_layer, type: 'costmap_2d::InflationLayer'}
 
-  sonar_layer:
+  range_layer:
     topics: ["/range_forward", "/range_backward", "/range_left", "/range_right"]
     no_readings_timeout: 1.0
+    clear_on_max_reading: true
index ad30795..989cedc 100644 (file)
@@ -33,5 +33,6 @@
                <rosparam file="$(find wild_thumper)/config/global_costmap_params_odom.yaml" command="load" if="$(arg nomap)" />
                <rosparam file="$(find wild_thumper)/config/local_costmap_params.yaml" command="load" />
                <rosparam file="$(find wild_thumper)/config/base_local_planner_params.yaml" command="load" />
+               <remap from="/cmd_vel" to="move_base/cmd_vel"/>
        </node>
 </launch>
diff --git a/launch/robot_localization.launch b/launch/robot_localization.launch
new file mode 100644 (file)
index 0000000..683c021
--- /dev/null
@@ -0,0 +1,124 @@
+<?xml version="1.0"?>
+
+<!-- Launch file for ekf_localization_node -->
+<launch>
+
+       <!-- This node will take in measurements from odometry, IMU, stamped pose, and stamped twist messages. It tracks
+        the state of the robot, with the state vector being defined as X position, Y position, Z position,
+        roll, pitch, yaw, their respective velocites, and linear acceleration. Units for all measurements are assumed
+        to conform to the SI standard as specified in REP-103. -->
+       <node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization" clear_params="true" output="screen">
+
+               <!-- ======== STANDARD PARAMETERS ======== -->
+
+               <!-- The frequency, in Hz, at which the filter will output a position estimate. Note that
+               the filter will not begin computation until it receives at least one message from
+               one of the inputs. It will then run continuously at the frequency specified here,
+               regardless of whether it receives more measurements. Defaults to 30 if unspecified. -->
+               <param name="frequency" value="20"/>
+
+               <!-- The period, in seconds, after which we consider a sensor to have timed out. In this event, we
+                  carry out a predict cycle on the EKF without correcting it. This parameter can be thought of
+                  as the minimum frequency with which the filter will generate new output. Defaults to 1 / frequency
+                  if not specified. -->
+               <param name="sensor_timeout" value="2.0"/>
+
+               <!-- If this is set to true, no 3D information will be used in your state estimate. Use this if you
+                  are operating in a planar environment and want to ignore the effect of small variations in the
+                  ground plane that might otherwise be detected by, for example, an IMU. Defaults to false if
+                  unspecified. -->
+               <param name="two_d_mode" value="true"/>
+
+               <!-- REP-105 (http://www.ros.org/reps/rep-0105.html) specifies three principal coordinate frames: map,
+                  odom, and base_link. base_link is the coordinate frame that is affixed to the robot. The robot's
+                  position in the odom frame will drift over time, but is accurate in the short term and should be
+                  continuous. The odom frame is therefore the best frame for executing local motion plans. The map
+                  frame, like the odom frame, is a world-fixed coordinate frame, and while it contains the most
+                  globally accurate position estimate for your robot, it is subject to discrete jumps, e.g., due to
+                  the fusion of GPS data. Here is how to use the following settings:
+                    1. Set the map_frame, odom_frame, and base_link frames to the appropriate frame names for your system.
+                         * If your system does not have a map_frame, just remove it, and make sure "world_frame" is set
+                           to the value of odom_frame.
+                    2. If you are fusing continuous position data such as wheel encoder odometry, visual odometry, or IMU data,
+                       set "world_frame" to your odom_frame value. This is the default behavior for robot_localization's state
+                       estimation nodes.
+                    3. If you are fusing global absolute position data that is subject to discrete jumps (e.g., GPS or position
+                       updates from landmark observations) then:
+                         3a. Set your "world_frame" to your map_frame value
+                         3b. MAKE SURE something else is generating the odom->base_link transform. Note that this can even be
+                             another instance of robot_localization! However, that instance should *not* fuse the global data. -->
+               <!-- Defaults to "map" if unspecified -->
+               <param name="map_frame" value="map"/>
+               <!-- Defaults to "odom" if unspecified -->
+               <param name="odom_frame" value="odom"/>
+               <!-- Defaults to "base_link" if unspecified -->
+               <param name="base_link_frame" value="base_footprint"/>
+               <!-- Defaults to the value of "odom_frame" if unspecified -->
+               <param name="world_frame" value="odom"/>
+
+               <!-- Use this parameter to provide an offset to the transform generated by ekf_localization_node. This
+                  can be used for future dating the transform, which is required for interaction with some other
+                  packages. Defaults to 0.0 if unspecified. -->
+               <param name="transform_time_offset" value="0.0"/>
+
+               <!-- The filter accepts an arbitrary number of inputs from each input message type (Odometry, PoseStamped,
+                  TwistStamped, Imu). To add a new one, simply append the next number in the sequence to its base name,
+                  e.g., odom0, odom1, twist0, twist1, imu0, imu1, imu2, etc. The value should be the topic name. These
+                  parameters obviously have no default values, and must be specified. -->
+               <param name="odom0" value="odom"/>
+               <param name="imu0" value="imu"/>
+               <param name="odom1" value="gps"/>
+
+               <!-- Each sensor reading updates some or all of the filter's state. These options give you greater control over
+                  which values from each measurement are fed to the filter. For example, if you have an odometry message as input,
+                  but only want to use its Z position value, then set the entire vector to false, except for the third entry.
+                  The order of the values is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Note that not some
+                  message types lack certain variables. For example, a TwistWithCovarianceStamped message has no pose information, so
+                  the first six values would be meaningless in that case. Each vector defaults to all false if unspecified, effectively
+                  making this parameter required for each sensor. -->
+               <rosparam param="odom0_config">[true, true, false, false, false, true, true, true, false, false, false, true, true, true, false]</rosparam>
+               <rosparam param="imu0_config">[false, false, false, true, true, true, false, false, false, true, true, true, true, true, true]</rosparam>
+               <rosparam param="odom1_config">[true, true, true, false, false, false, true, true, true, false, false, false, true, true, true]</rosparam>
+
+
+               <!-- The best practice for including new sensors in robot_localization's state estimation nodes is to pass in velocity
+                  measurements and let the nodes integrate them. However, this isn't always feasible, and so the state estimation
+                  nodes support fusion of absolute measurements. If you have more than one sensor providing absolute measurements,
+                  however, you may run into problems if your covariances are not large enough, as the sensors will inevitably
+                  diverge from one another, causing the filter to jump back and forth rapidly. To combat this situation, you can
+                  either increase the covariances for the variables in question, or you can simply set the sensor's differential
+                  parameter to true. When differential mode is enabled, all absolute pose data is converted to velocity data by
+                  differentiating the absolute pose measurements. These velocities are then integrated as usual. NOTE: this only
+                  applies to sensors that provide absolute measurements, so setting differential to true for twit measurements has
+                  no effect.
+
+                  Users should take care when setting this to true for orientation variables: if you have only one source of
+                  absolute orientation data, you should not set the differential parameter to true. This is due to the fact that
+                  integration of velocities leads to slowly increasing error in the absolute (pose) variable. For position variables,
+                  this is acceptable. For orientation variables, it can lead to trouble. Users should make sure that all orientation
+                  variables have at least one source of absolute measurement. -->
+               <param name="odom0_differential" value="true"/>
+               <param name="imu0_differential" value="false"/>
+               <param name="odom1_differential" value="true"/>
+
+               <!-- When the node starts, if this parameter is true, then the first measurement is treated as a "zero point" for all
+                  future measurements. While you can achieve the same effect with the differential paremeter, the key difference is
+                  that the relative parameter doesn't cause the measurement to be converted to a velocity before integrating it. If
+                  you simply want your measurements to start at 0 for a given sensor, set this to true. -->
+               <param name="odom0_relative" value="false"/>
+               <param name="imu0_relative" value="false"/>
+               <param name="odom1_relative" value="false"/>
+
+               <!-- If we're including accelerations in our state estimate, then we'll probably want to remove any acceleration that
+                  is due to gravity for each IMU. If you don't want to, then set this to false. Defaults to false if unspecified. -->
+               <param name="imu0_remove_gravitational_acceleration" value="true"/>
+
+               <!-- If you're having trouble, try setting this to true, and then echo the /diagnostics_agg topic to see
+                  if the node is unhappy with any settings or data. -->
+               <param name="print_diagnostics" value="true"/>
+
+               <remap from="odometry/filtered" to="odom_combined"/>
+
+       </node>
+
+</launch>
index 5fb511c..18f083c 100644 (file)
@@ -6,7 +6,6 @@
                <param name="axis_deadman" value="3"/>
                <param name="axis_linear" value="1"/>
                <param name="axis_angular" value="0"/>
-               <remap from="turtlebot_teleop_joystick/cmd_vel" to="/cmd_vel"/>
        </node>
 
        <node pkg="wiimote" type="wiimote_node.py" name="wiimote" respawn="true" />
index 869f51f..567d95a 100644 (file)
@@ -2,9 +2,13 @@
 <launch>
        <arg name="use_imu" default="true"/>
 
+       <node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager"/>
+
        <param name="robot_description" command="$(find xacro)/xacro.py $(find wild_thumper)/urdf/wild_thumper.urdf.xacro"/>
 
-       <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen"/>
+       <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen">
+               <param name="publish_frequency" value="20.0" />
+       </node>
        <node pkg="joint_state_publisher" type="joint_state_publisher" name="joint_state_publisher" output="screen"/>
 
        <node pkg="diagnostic_aggregator" type="aggregator_node" name="diagnostic_aggregator" output="screen">
                <param name="use_gps_time" value="false"/>
        </node>
 
-       <node pkg="gps_common" type="utm_odometry_node" name="gps_conv">
-               <remap from="odom" to="odom_gps"/>
-               <param name="frame_id" value="base_footprint"/>
+       <node pkg="gps_common" type="utm_odometry_node" name="gps_conv" output="screen">
+               <remap from="odom" to="gps"/>
+               <param name="frame_id" value="odom"/>
+               <param name="child_frame_id" value="base_footprint"/>
        </node>
 
        <node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf" output="screen" if="$(arg use_imu)">
                <remap from="imu_data" to="imu"/>
-               <remap from="vo" to="odom_gps"/>
+               <remap from="vo" to="gps"/>
                <remap from="robot_pose_ekf/odom_combined" to="odom_combined"/>
                <param name="output_frame" value="odom"/>
                <param name="freq" value="20.0"/>
                <param name="debug" value="true"/>
                <param name="sensor_timeout" value="2.0"/>
        </node>
+       <!-- <include file="$(find wild_thumper)/launch/robot_localization.launch" if="$(arg use_imu)"/> -->
+
+       <include file="$(find yocs_cmd_vel_mux)/launch/cmd_vel_mux.launch">
+               <arg name="config_file" value="$(find wild_thumper)/cfg/cmd_vel_mux.yaml" />
+               <arg name="nodelet_manager_name" value="nodelet_manager" />
+       </include>
 </launch>
diff --git a/scripts/square.py b/scripts/square.py
new file mode 100755 (executable)
index 0000000..cd73274
--- /dev/null
@@ -0,0 +1,56 @@
+#!/usr/bin/env python
+# -*- coding: iso-8859-15 -*-
+
+import rospy
+import tf
+import actionlib
+from math import *
+from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
+from actionlib_msgs.msg import GoalStatus
+from optparse import OptionParser
+
+
+class Square:
+       def __init__(self):
+               rospy.init_node('umbmark')
+               self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
+               while not self.move_base.wait_for_server(rospy.Duration(5)):
+                       rospy.loginfo("Waiting for the move_base action server to come up")
+
+       def next_pos(self, x, y, angle):
+               odom_quat = tf.transformations.quaternion_from_euler(0, 0, angle)
+
+               rospy.loginfo("Moving to (%.2f, %2f), %d°.." % (x, y, angle*180/pi))
+
+               goal = MoveBaseGoal()
+               goal.target_pose.header.frame_id = "odom"
+               goal.target_pose.header.stamp = rospy.Time.now()
+               goal.target_pose.pose.position.x = x
+               goal.target_pose.pose.position.y = y
+               goal.target_pose.pose.orientation.x = odom_quat[0]
+               goal.target_pose.pose.orientation.y = odom_quat[1]
+               goal.target_pose.pose.orientation.z = odom_quat[2]
+               goal.target_pose.pose.orientation.w = odom_quat[3]
+               self.move_base.send_goal(goal)
+
+               self.move_base.wait_for_result()
+
+               if self.move_base.get_state() == GoalStatus.SUCCEEDED:
+                       rospy.loginfo("The base moved to (%.2f, %2f), %d°" % (x, y, angle*180/pi))
+               else:
+                       rospy.logerr("The base failed to (%.2f, %2f), %d°" % (x, y, angle*180/pi))
+                       raise
+
+       def run(self, size=1):
+               self.next_pos(0, 0, 0)
+               self.next_pos(size, 0, 0)
+               self.next_pos(size, -size, 0)
+               self.next_pos(0, size, 0)
+
+if __name__ == "__main__":
+       parser = OptionParser()
+       parser.add_option("-l", "--length", dest="length", default=2, help="Square size")
+       (options, args) = parser.parse_args()
+
+       p = Square()
+       p.run(float(options.length))
index a2a09ba..4064fb8 100755 (executable)
@@ -4,6 +4,7 @@
 import rospy
 import tf
 import struct
+import prctl
 from i2c import *
 from math import *
 from geometry_msgs.msg import Twist
@@ -16,7 +17,8 @@ WHEEL_DIST = 0.248
 class MoveBase:
        def __init__(self):
                rospy.init_node('wild_thumper')
-               rospy.Subscriber("cmd_vel", Twist, self.cmdVelReceived)
+               prctl.set_name("wild_thumper")
+               rospy.Subscriber("cmd_vel_out", Twist, self.cmdVelReceived)
                rospy.Subscriber("imu", Imu, self.imuReceived)
                enable_odom_tf = rospy.get_param("~enable_odom_tf", True)
                if enable_odom_tf:
@@ -47,8 +49,9 @@ class MoveBase:
                        self.get_voltage()
                        if i % 2:
                                self.get_dist_forward()
-                               self.get_dist_backward()
                                self.get_dist_left()
+                       else:
+                               self.get_dist_backward()
                                self.get_dist_right()
                        i+=1
                        rate.sleep()
@@ -193,13 +196,17 @@ class MoveBase:
                val = struct.unpack(">H", s)[0]
                return val
        
-       def get_dist_srf(self, num):
+       def start_dist_srf(self, num):
                dev = i2c(0x52)
                s = struct.pack("B", num)
                dev.write(s)
                dev.close()
 
-               sleep(50e-3)
+       def read_dist_srf(self, num):
+               dev = i2c(0x52)
+               s = struct.pack("B", num)
+               dev.write(s)
+               dev.close()
 
                dev = i2c(0x52)
                s = dev.read(2)
@@ -221,22 +228,24 @@ class MoveBase:
        def get_dist_left(self):
                if self.pub_range_left.get_num_connections() > 0:
                        dist = 30.553/(self.get_dist_ir(0x1) - -67.534)
-                       self.send_range(self.pub_range_left, "ir_left", Range.INFRARED, dist, 0.04, 0.3, 5)
+                       self.send_range(self.pub_range_left, "ir_left", Range.INFRARED, dist, 0.04, 0.3, 1)
 
        def get_dist_right(self):
                if self.pub_range_right.get_num_connections() > 0:
                        dist = 17.4/(self.get_dist_ir(0x3) - 69)
-                       self.send_range(self.pub_range_right, "ir_right", Range.INFRARED, dist, 0.04, 0.3, 5)
+                       self.send_range(self.pub_range_right, "ir_right", Range.INFRARED, dist, 0.04, 0.3, 1)
 
        def get_dist_forward(self):
                if self.pub_range_fwd.get_num_connections() > 0:
-                       dist = self.get_dist_srf(0x5)
+                       dist = self.read_dist_srf(0x15)
                        self.send_range(self.pub_range_fwd, "sonar_forward", Range.ULTRASOUND, dist, 0.04, 6, 30)
+                       self.start_dist_srf(0x5) # get next value
 
        def get_dist_backward(self):
                if self.pub_range_bwd.get_num_connections() > 0:
-                       dist = self.get_dist_srf(0x7)
+                       dist = self.read_dist_srf(0x17)
                        self.send_range(self.pub_range_bwd, "sonar_backward", Range.ULTRASOUND, dist, 0.04, 6, 30)
+                       self.start_dist_srf(0x7) # get next value
                
 
 if __name__ == "__main__":