Permalink
Browse files

preparing local navigation stack

  • Loading branch information...
1 parent 9241909 commit 3a3671d3ed9097762449a8650fa6e252067445c7 @zakharov zakharov committed Nov 3, 2011
@@ -44,9 +44,6 @@ INCLUDE_DIRECTORIES(
${Boost_INCLUDE_DIR}
)
-rosbuild_add_executable(lowpass_filter
- src/lowpass_filter.cpp
-)
rosbuild_add_executable(youbot_oodl
src/youbot_oodl.cpp
@@ -11,17 +11,11 @@ or enable the USE_SETCAP flag in the cmake file and recompile again.
-->
<launch>
- <param name="T" type="double" value="0.3"/>
- <param name="dt" type="double" value="0.1"/>
-
- <node name="lowpass_filter" pkg="youbot_oodl" type="lowpass_filter" output="screen"/>
-
-
- <param name="youBotHasBase" type="bool" value="true"/>
+ <aram name="youBotHasBase" type="bool" value="true"/>
<param name="youBotHasArm" type="bool" value="true"/>
<node name="youbot_oodl_driver" pkg="youbot_oodl" type="youbot_oodl" output="screen">
- <remap from="cmd_vel" to="cmd_vel_filtered" />
+
</node>
@@ -1,95 +0,0 @@
-/******************************************************************************
-* Copyright (c) 2011
-* Locomotec
-*
-* Author:
-* Alexey Zakharov
-*
-*
-* This software is published under a dual-license: GNU Lesser General Public
-* License LGPL 2.1 and BSD license. The dual-license implies that users of this
-* code may choose which terms they prefer.
-*
-* Redistribution and use in source and binary forms, with or without
-* modification, are permitted provided that the following conditions are met:
-*
-* * Redistributions of source code must retain the above copyright
-* notice, this list of conditions and the following disclaimer.
-* * Redistributions in binary form must reproduce the above copyright
-* notice, this list of conditions and the following disclaimer in the
-* documentation and/or other materials provided with the distribution.
-* * Neither the name of Locomotec nor the names of its
-* contributors may be used to endorse or promote products derived from
-* this software without specific prior written permission.
-*
-* This program is free software: you can redistribute it and/or modify
-* it under the terms of the GNU Lesser General Public License LGPL as
-* published by the Free Software Foundation, either version 2.1 of the
-* License, or (at your option) any later version or the BSD license.
-*
-* This program is distributed in the hope that it will be useful,
-* but WITHOUT ANY WARRANTY; without even the implied warranty of
-* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-* GNU Lesser General Public License LGPL and the BSD license for more details.
-*
-* You should have received a copy of the GNU Lesser General Public
-* License LGPL and BSD license along with this program.
-*
-******************************************************************************/
-
-#include "ros/ros.h"
-#include "geometry_msgs/Twist.h"
-
-double T = 0; // time constant in sec
-double dt = 0; // time interval in sec
-
-double x_ = 0; // linear velocity
-double y_ = 0; // linear velocity
-double z_ = 0; // angular velocity
-
-ros::Publisher twistPublisher;
-
-double lowPassFilter(double x, double y0, double dt, double T) //Low-pass filter : 1/(Tp + 1)
-{
- return y0 + (x - y0) * (dt/(dt+T)); // y0 - previous output value, x - input value
-}
-
-void twistCallback(const geometry_msgs::Twist& twist){
-
- double x = twist.linear.x; // current value of linear velocity
- double y = twist.linear.y; // current value of linear velocity
- double z = twist.angular.z; // current value of angular velocity
-
- x_ = lowPassFilter(x, x_, dt, T);
- y_ = lowPassFilter(y, y_, dt, T);
- z_ = lowPassFilter(z, z_, dt, T);
-
- geometry_msgs::Twist t;
- t.linear.x = x_;
- t.linear.y = y_;
- t.angular.z = z_;
- twistPublisher.publish(t);
-
-}
-
-int main(int argc, char **argv)
-{
-
- ros::init(argc, argv, "lowpass_filter");
- ros::NodeHandle n;
-
- n.param("T", T, 0.3); // setting default values in seconds
- n.param("dt", dt, 0.1);
-
- ros::Subscriber twistSubscriber = n.subscribe("move_base/cmd_vel", 1000, &twistCallback);
- twistPublisher = n.advertise<geometry_msgs::Twist>("cmd_vel",1);
-
- ros::Rate rate(1.0/dt); //setting update rate in Hz
- while (n.ok()){
- ros::spinOnce();
- rate.sleep();
- }
-
- return 0;
-}
-
@@ -14,16 +14,16 @@ xy_goal_tolerance: 0.1
yaw_goal_tolerance: 0.1
# Forward Simulation Parameters
-#sim_time: 1.7
-#sim_granularity: 0.025
+sim_time: 1.7
+sim_granularity: 0.025
vx_samples: 3
-vtheta_samples: 10
+vtheta_samples: 6
# Trajectory Scoring Parameters
-#goal_distance_bias: 0.8
-#path_distance_bias: 0.6
-#occdist_scale: 0.01
-#heading_lookahead: 0.325
+goal_distance_bias: 0.8
+path_distance_bias: 0.6
+occdist_scale: 0.01
+heading_lookahead: 0.325
dwa: false
# Oscillation Prevention Parameters
@@ -2,7 +2,7 @@ map_type: costmap
transform_tolerance: 0.2
obstacle_range: 2.5
raytrace_range: 3.0
-inflation_radius: 0.20
+inflation_radius: 0.25
observation_sources: base_scan
@@ -10,6 +10,6 @@ footprint: [[0.26, 0.18],
controller_frequency: 10.0
controller_patience: 15.0
-clearing_radius: 0.59
+clearing_radius: 0.25
footprint_padding: 0.03
@@ -1,94 +1,69 @@
-/******************************************************************************
-* Copyright (c) 2011
-* Locomotec
-*
-* Author:
-* Alexey Zakharov
-*
-*
-* This software is published under a dual-license: GNU Lesser General Public
-* License LGPL 2.1 and BSD license. The dual-license implies that users of this
-* code may choose which terms they prefer.
-*
-* Redistribution and use in source and binary forms, with or without
-* modification, are permitted provided that the following conditions are met:
-*
-* * Redistributions of source code must retain the above copyright
-* notice, this list of conditions and the following disclaimer.
-* * Redistributions in binary form must reproduce the above copyright
-* notice, this list of conditions and the following disclaimer in the
-* documentation and/or other materials provided with the distribution.
-* * Neither the name of Locomotec nor the names of its
-* contributors may be used to endorse or promote products derived from
-* this software without specific prior written permission.
-*
-* This program is free software: you can redistribute it and/or modify
-* it under the terms of the GNU Lesser General Public License LGPL as
-* published by the Free Software Foundation, either version 2.1 of the
-* License, or (at your option) any later version or the BSD license.
-*
-* This program is distributed in the hope that it will be useful,
-* but WITHOUT ANY WARRANTY; without even the implied warranty of
-* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-* GNU Lesser General Public License LGPL and the BSD license for more details.
-*
-* You should have received a copy of the GNU Lesser General Public
-* License LGPL and BSD license along with this program.
-*
-******************************************************************************/
-
#include "ros/ros.h"
#include "geometry_msgs/Twist.h"
-double T = 0; // time constant in sec
-double dt = 0; // time interval in sec
+double T = 0.3; //sec
+double dt = 0.1; //ms
-double x_ = 0; // linear velocity in m/sec
-double y_ = 0; // linear velocity in m/sec
-double z_ = 0; // angular velocity in rad/sec
+double x_ = 0;
+double y_ = 0;
+double z_ = 0;
+double epsilon = 0.0001;
ros::Publisher twistPublisher;
-double lowPassFilter(double x, double y0, double dt, double T) //Low-pass filter : 1/(Tp + 1)
+double lowPassFilter(double x, double y0, double dt, double T) // Taken from http://en.wikipedia.org/wiki/Low-pass_filter
{
- return y0 + (x - y0) * (dt/(dt+T)); // y0 - previous output value, x - input value
+ double res = y0 + (x - y0) * (dt/(dt+T));
+
+ if ((res*res) <= epsilon)
+ res = 0;
+ return res;
}
void twistCallback(const geometry_msgs::Twist& twist){
-
- double x = twist.linear.x; // current value of linear velocity
- double y = twist.linear.y; // current value of linear velocity
- double z = twist.angular.z; // current value of angular velocity
+ double x = twist.linear.x;
+ double y = twist.linear.y;
+ double z = twist.angular.z;
+
+ x_ = lowPassFilter(x, x_, dt, T);
+ y_ = lowPassFilter(y, y_, dt, T);
+ z_ = lowPassFilter(z, z_, dt, T);
+
- x_ = lowPassFilter(x, x_, dt, T);
- y_ = lowPassFilter(y, y_, dt, T);
- z_ = lowPassFilter(z, z_, dt, T);
+ geometry_msgs::Twist t;
+ t.linear.x = x_;
+ t.linear.y = y_;
+ t.angular.z = z_;
- geometry_msgs::Twist t; // publishing filtered signal
- t.linear.x = x_;
- t.linear.y = y_;
- t.angular.z = z_;
- twistPublisher.publish(t);
+ twistPublisher.publish(t);
}
int main(int argc, char **argv)
{
- ros::init(argc, argv, "lowpass_filter");
- ros::NodeHandle n;
+ /// Receives Twist messages for the base.
+ ros::Subscriber baseCommandSubscriber;
+ /// Publishes Odometry messages
+ ros::Publisher baseOdometryPublisher;
+
+ ros::init(argc, argv, "lowpass_filter");
+ ros::NodeHandle n;
+ /* setup input/output communication */
+
+ n.param("T", T, 0.3);
+ n.param("dt", dt, 0.1);
- n.param("T", T, 0.3); // setting default values in seconds
- n.param("dt", dt, 0.1);
+ ros::Subscriber twistSubscriber = n.subscribe("move_base/cmd_vel", 1000, &twistCallback);
+ twistPublisher = n.advertise<geometry_msgs::Twist>("cmd_vel",1);
- ros::Subscriber twistSubscriber = n.subscribe("move_base/cmd_vel", 1000, &twistCallback);
- twistPublisher = n.advertise<geometry_msgs::Twist>("cmd_vel",1);
+ /* coordination */
+ ros::Rate rate(100); //Input and output at the same time... (in Hz)
+ while (n.ok()){
+ ros::spinOnce();
- ros::Rate rate(1.0/dt); //setting update rate in Hz
- while (n.ok()){
- ros::spinOnce();
- rate.sleep();
- }
+ rate.sleep();
+ }
return 0;
}
@@ -1,2 +0,0 @@
-rxplot /cmd_vel/linear/x,/cmd_vel_filtered/linear/x /cmd_vel/angular/z,/cmd_vel_filtered/angular/z
-
@@ -9,6 +9,6 @@ global_costmap:
rolling_window: true
width: 20.0
height: 20.0
- resolution: 0.025
+ resolution: 0.05
origin_x: 0.0
origin_y: 0.0
@@ -2,7 +2,12 @@
<!-- Throttle the voxel grid that is being published for rviz -->
<!-- <node ns="move_base_node/local_costmap" name="voxel_grid_throttle" pkg="topic_tools" type="throttle" args="messages voxel_grid 3.0 voxel_grid_throttled" /> -->
- <node pkg="youbot_navigation_common" type="lowpass_filter" respawn="false" name="lowpass_filter" output="screen"/>
+ <param name="T" type="double" value="0.2"/>
+ <param name="dt" type="double" value="0.1"/>
+ <node pkg="youbot_navigation_common" type="lowpass_filter" respawn="false" name="lowpass_filter" output="screen">
+
+ </node>
+
<!-- for moving -->
<node pkg="move_base" type="move_base" respawn="false" name="move_base_node" output="screen">

0 comments on commit 3a3671d

Please sign in to comment.