more tuning/fixes

diff --git a/config/base_local_planner_params.yaml b/config/base_local_planner_params.yaml
index a24e4b8d43f8ba7aca1b62e514510ae30e39cda2..7d670be12befaf38416cd6cf277987e8e01a0e5b 100644 (file)
--- a/config/base_local_planner_params.yaml
+++ b/config/base_local_planner_params.yaml
@@ -9,3 +9,5 @@ TrajectoryPlannerROS:
   acc_lim_th: 1.5
 
   holonomic_robot: false
+
+  yaw_goal_tolerance: 0.2

diff --git a/config/global_costmap_params.yaml b/config/global_costmap_params.yaml
index 01a3c8feda3a0fe67e413d27c3ee6db180dc8b1e..87b765957bdd978c1721efb838abbd51cae40a37 100644 (file)
--- a/config/global_costmap_params.yaml
+++ b/config/global_costmap_params.yaml
@@ -3,3 +3,4 @@ global_costmap:
   robot_base_frame: base_link
   update_frequency: 5.0
   static_map: true
+  rolling_window: true

diff --git a/move_base.launch b/move_base.launch
index ff0425d0f290966fe5e73d4f27c5c7f0651f5fe5..6c750989ee4332ffad6e245d7a3279ef4179af6e 100644 (file)
--- a/move_base.launch
+++ b/move_base.launch
@@ -10,8 +10,8 @@
        <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
                <rosparam file="$(find roboint)/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
                <rosparam file="$(find roboint)/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
-               <rosparam file="$(find roboint)/config/local_costmap_params.yaml" command="load" />
                <rosparam file="$(find roboint)/config/global_costmap_params.yaml" command="load" />
+               <rosparam file="$(find roboint)/config/local_costmap_params.yaml" command="load" />
                <rosparam file="$(find roboint)/config/base_local_planner_params.yaml" command="load" />
        </node>
 </launch>

diff --git a/scripts/robo_explorer.py b/scripts/robo_explorer.py
index 6cf73e6802fc74176d8e8ec686ef9433ea8c8519..36ec82870a4844c2cbd62a358363f1c614ad7118 100755 (executable)
--- a/scripts/robo_explorer.py
+++ b/scripts/robo_explorer.py
@@ -23,6 +23,10 @@ class RoboExplorer:
                self.last_in = None
                self.tf_broadcaster = tf.TransformBroadcaster()
                self.last_time = rospy.Time.now()
+               self.input_count = 0
+               self.x_last = 0
+               self.y_last = 0
+               self.alpha_last = 0
 
                self.pub_motor = rospy.Publisher("ft/set_motor", Motor)
                self.pub_scan = rospy.Publisher("scan", LaserScan)
@@ -35,22 +39,28 @@ class RoboExplorer:
 
        def inputsReceived(self, msg):
                current_time = rospy.Time.now()
+               self.input_count+=1
 
-               self.tf_broadcaster.sendTransform((0.0, 0.0, 0.0), (0.0, 0.0, 0.0, 1.0), current_time, "odom", "map");
-               self.send_odometry(msg, current_time)
-               self.send_laser_scan(msg, current_time)
+               self.update_odometry(msg, current_time)
+               if self.input_count >= 10:
+                       self.input_count = 0
+                       self.tf_broadcaster.sendTransform((0.0, 0.0, 0.0), (0.0, 0.0, 0.0, 1.0), current_time, "odom", "map");
+                       self.send_odometry(msg, current_time)
+                       self.send_laser_scan(msg, current_time)
 
-               self.last_time = current_time
-
-       def send_odometry(self, msg, current_time):
-               dt = (current_time - self.last_time).to_sec();
+       def update_odometry(self, msg, current_time):
                in_now = msg.input[1:3]
                if self.last_in is not None:
                        in_diff = [abs(a - b) for a, b in zip(in_now, self.last_in)] # get changed inputs
+                       # fix in_diff from actual motor direction
                        if self.speed[0] < 0:
                                in_diff[0] = -in_diff[0]
+                       elif self.speed[0] == 0:
+                               in_diff[0] = 0
                        if self.speed[1] < 0:
                                in_diff[1] = -in_diff[1]
+                       elif self.speed[1] == 0:
+                               in_diff[1] = 0
 
                        dist_dir = (in_diff[1] - in_diff[0])*self.wheel_size*pi/8 # steps_changed in different direction => m
                        delta_alpha = dist_dir/self.wheel_dist
@@ -68,40 +78,47 @@ class RoboExplorer:
                        self.x += delta_x
                        self.y += delta_y
 
-                       # speeds
-                       vx = delta_x / dt
-                       vy = delta_y / dt
-                       valpha = delta_alpha / dt
-
-                       # since all odometry is 6DOF we'll need a quaternion created from yaw
-                       odom_quat = tf.transformations.quaternion_from_euler(0, 0, self.alpha)
-
-                       # first, we'll publish the transform over tf
-                       self.tf_broadcaster.sendTransform((self.x, self.y, 0.0), odom_quat, current_time, "base_link", "odom");
-
-                       # next, we'll publish the odometry message over ROS
-                       odom = Odometry()
-                       odom.header.stamp = current_time
-                       odom.header.frame_id = "/odom"
-
-                       # set the position
-                       odom.pose.pose.position.x = self.x
-                       odom.pose.pose.position.y = self.y
-                       odom.pose.pose.position.z = 0.0
-                       odom.pose.pose.orientation.x = odom_quat[0]
-                       odom.pose.pose.orientation.y = odom_quat[1]
-                       odom.pose.pose.orientation.z = odom_quat[2]
-                       odom.pose.pose.orientation.w = odom_quat[3]
-
-                       # set the velocity
-                       odom.child_frame_id = "base_link";
-                       odom.twist.twist.linear.x = vx
-                       odom.twist.twist.linear.y = vy
-                       odom.twist.twist.angular.z = valpha
-
-                       # publish the message
-                       self.pub_odom.publish(odom)
                self.last_in = in_now
+
+       def send_odometry(self, msg, current_time):
+               # speeds
+               dt = (current_time - self.last_time).to_sec()
+               vx = (self.x - self.x_last) / dt
+               vy = (self.y - self.y_last) / dt
+               valpha = (self.alpha - self.alpha_last) / dt
+               self.last_time = current_time
+               self.x_last = self.x
+               self.y_last = self.y
+               self.alpha_last = self.alpha
+
+               # since all odometry is 6DOF we'll need a quaternion created from yaw
+               odom_quat = tf.transformations.quaternion_from_euler(0, 0, self.alpha)
+
+               # first, we'll publish the transform over tf
+               self.tf_broadcaster.sendTransform((self.x, self.y, 0.0), odom_quat, current_time, "base_link", "odom");
+
+               # next, we'll publish the odometry message over ROS
+               odom = Odometry()
+               odom.header.stamp = current_time
+               odom.header.frame_id = "/odom"
+
+               # set the position
+               odom.pose.pose.position.x = self.x
+               odom.pose.pose.position.y = self.y
+               odom.pose.pose.position.z = 0.0
+               odom.pose.pose.orientation.x = odom_quat[0]
+               odom.pose.pose.orientation.y = odom_quat[1]
+               odom.pose.pose.orientation.z = odom_quat[2]
+               odom.pose.pose.orientation.w = odom_quat[3]
+
+               # set the velocity
+               odom.child_frame_id = "base_link";
+               odom.twist.twist.linear.x = vx
+               odom.twist.twist.linear.y = vy
+               odom.twist.twist.angular.z = valpha
+
+               # publish the message
+               self.pub_odom.publish(odom)
                
        def send_laser_scan(self, msg, current_time):
                # first, we'll publish the transform over tf
@@ -114,7 +131,7 @@ class RoboExplorer:
                scan.angle_min = -pi/4;
                scan.angle_max = pi/4;
                scan.angle_increment = pi/4;
-               scan.time_increment = 0.01;
+               scan.time_increment = 0.1;
                scan.range_min = 0.0;
                scan.range_max = 4.0;
                for i in range(3):

diff --git a/src/libft_adapter.cpp b/src/libft_adapter.cpp
index 22c50ae064be63d86845949ee9652f7f90a520ec..27936b7f4fcb8aa90760dc87d6529b29767e7fa5 100644 (file)
--- a/src/libft_adapter.cpp
+++ b/src/libft_adapter.cpp
@@ -1,3 +1,4 @@
+#include <stdlib.h>
 #include "roboint.h"
 #include "ros/ros.h"
 #include "roboint/Output.h"
@@ -20,14 +21,15 @@ void cb_set_output(const ::roboint::OutputConstPtr& msg) {
 
 void cb_set_motor(const ::roboint::MotorConstPtr& msg) {
        unsigned char iDirection = 0;
+       int speed = abs(msg->speed);
 
        if (msg->speed > 0) iDirection = 0x1;
        else if (msg->speed < 0) iDirection = 0x2;
 
        transfer_area->M_Main &= ~(3<<(msg->num-1)*2);
        transfer_area->M_Main |= iDirection<<(msg->num-1)*2;
-       transfer_area->MPWM_Main[(msg->num-1)*2] = msg->speed;
-       transfer_area->MPWM_Main[(msg->num-1)*2 + 1] = msg->speed;
+       transfer_area->MPWM_Main[(msg->num-1)*2] = speed;
+       transfer_area->MPWM_Main[(msg->num-1)*2 + 1] = speed;
 }
 
 
@@ -92,7 +94,7 @@ int main(int argc, char **argv)
         * than we can send them, the number here specifies how many messages to
         * buffer up before throwing some away.
         */
-       ros::Publisher chatter_pub = n.advertise<roboint::Inputs>("ft/get_inputs", 1000);
+       ros::Publisher chatter_pub = n.advertise<roboint::Inputs>("ft/get_inputs", 10);
        ros::Rate loop_rate(100);
 
        while(ros::ok()) {