From 7ed26bb9678da05ebd73e6299b90908760a04780 Mon Sep 17 00:00:00 2001 From: erik Date: Sat, 24 Aug 2013 22:03:49 +0200 Subject: [PATCH] more tuning/fixes --- config/base_local_planner_params.yaml | 2 + config/global_costmap_params.yaml | 1 + move_base.launch | 2 +- scripts/robo_explorer.py | 99 ++++++++++++++++----------- src/libft_adapter.cpp | 8 ++- 5 files changed, 67 insertions(+), 45 deletions(-) diff --git a/config/base_local_planner_params.yaml b/config/base_local_planner_params.yaml index a24e4b8..7d670be 100644 --- 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 01a3c8f..87b7659 100644 --- 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 ff0425d..6c75098 100644 --- a/move_base.launch +++ b/move_base.launch @@ -10,8 +10,8 @@ - + diff --git a/scripts/robo_explorer.py b/scripts/robo_explorer.py index 6cf73e6..36ec828 100755 --- 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 22c50ae..27936b7 100644 --- a/src/libft_adapter.cpp +++ b/src/libft_adapter.cpp @@ -1,3 +1,4 @@ +#include #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("ft/get_inputs", 1000); + ros::Publisher chatter_pub = n.advertise("ft/get_inputs", 10); ros::Rate loop_rate(100); while(ros::ok()) { -- 2.39.5