Given two obstacles, make the robot goes around the first one and stops 3 cm behind the second one for 10 seconds and finally comes back, as in Figure 1.
Preliminary steps
cd mkdir projects cd projects svn co --username ros \ https://robotics.dei.unipd.it/svn/iaslab/projects/lab/base . rm -rf .svn svn co --username ros \ https://robotics.dei.unipd.it/svn/iaslab/projects/robots/nxt . rm -rf .svn
Copy the content of to_add.bashrc after your .bashrc, close your terminal and reopen it.
cmake -G"Eclipse CDT4 - Unix Makefiles" rosmake nxt_rviz_plugin nxt_unipd nxt_ros roslaunch nxt_unipd nxt_lab.launch roslaunch nxt_unipd teleop_keyboard.launch rosrun rviz rviz
To create your packages
cd ~/projects roscreate-pkg your_package rospy roscpp nxt_ros cd your_package cmake -G"Eclipse CDT4 - Unix Makefiles" rosmake your_package
Once you have written your .cpp file, add it to the CMakeLists.txt file to create the executable.
rosbuild_add_executable(executable_name src/code_file_name.cpp)
Robot configuration
nxt_lab.yaml
- type: ultrasonic
frame_id: ultrasonic_link
name: ultrasonic_sensor
port: PORT_4
spread_angle: 0.2
min_range: 0.01
max_range: 2.5
desired_frequency: 5.0
Program launcher
nxt_lab.launch
<group ns="base_parameters">
<param name="r_wheel_joint" value="r_wheel_joint"/>
<param name="l_wheel_joint" value="l_wheel_joint"/>
<param name="wheel_radius" value="0.022"/>
<param name="wheel_basis" value="0.055"/>
<param name="vel_to_eff" value="0.2"/>
<param name="l_vel_to_eff" value="0.1"/>
<param name="r_vel_to_eff" value="0.1"/>
</group>
Robot controller
advanced_control.py
{tex}v_{trans}^{est} \left[i+1\right] = \frac{1}{2} \left( v_{trans}^{est}\left[i\right] + \frac{1}{2} \left( v_{rot}^{reg}\left[i,j_{wheel}^{l} \right] + v_{rot}^{reg}\left[i,j_{wheel}^{r} \right] \right) r_{wheel} \right){/tex}
{tex}v_{rot}^{est} \left[i+1\right] = \frac{1}{2} \left( v_{rot}^{est}\left[i\right] + \frac{1}{2} \left( v_{rot}^{reg}\left[i,j_{wheel}^{l} \right] - v_{rot}^{reg}\left[i,j_{wheel}^{r} \right] \right) \frac{r_{wheel}}{b_{wheel}} \right){/tex}
where:
{tex}v_{trans}^{est} \left[i\right]{/tex} is the estimated translational velocity at the instant{tex}i_{.}{/tex}
{tex}v_{rot}^{est} \left[i\right]{/tex} is the estimated rotational velocity at the instant{tex}i_{\qquad}{/tex}
{tex}v_{rot}^{reg}\left[i,j_{wheel}^{l} \right]{/tex} is the registered rotational velocity for the joint of the left wheel at the instant{tex}i_{\qquad}{/tex}
{tex}v_{rot}^{reg}\left[i,j_{wheel}^{r} \right]{/tex} is the registered rotational velocity for the joint of the right wheel at the instant{tex}i_{\qquad}{/tex}
{tex}r_{wheel}{/tex} is the wheel radius
{tex}b_{wheel}{/tex} is the wheel basis
{tex}v_{trans}^{cmd} \left[i+1\right] = v_{trans}^{des}\left[i\right] + k_{trans} \left( v_{trans}^{des}\left[i \right] - v_{trans}^{est}\left[i +1 \right] \right){/tex}
{tex}v_{rot}^{cmd} \left[i+1\right] = v_{rot}^{des}\left[i\right] + k_{rot} \left( v_{rot}^{des}\left[i \right] - v_{rot}^{est}\left[i \right] \right){/tex}
where:
{tex}v_{trans}^{cmd} \left[i\right]{/tex} is the translational velocity applied to the joint at the instant{tex}i_{.}{/tex}
{tex}v_{rot}^{cmd} \left[i\right]{/tex} is the rotational velocity applied to the joint at the instant{tex}i_{.}{/tex}
{tex}v_{trans}^{des}\left[i \right]{/tex} is the desired transational velocity for the joint at the instant{tex}i_{.}{/tex}
{tex}v_{rot}^{des}\left[i \right]{/tex} is the desired rotational velocity for the joint at the instant{tex}i_{.}{/tex}
{tex}k_{trans}{/tex} is the trasational constant
{tex}k_{rot}{/tex} is the rotational constant
{tex}F \left[i+1\right] = k_v^F \left( v_{trans}^{cmd}\left[i + 1\right] \frac{1}{r_{wheel}} - v_{rot}^{cmd}\left[i +1 \right] \frac{b_wheel}{r_{wheel}} \right){/tex}
where:
F \left[i\right] is the effort applied to the joint at the instant{tex}i_{.}{/tex}
k_v^Fis the constant to transform velocity to effort
Robot teleoperation
nxt_key.cpp
switch(c)
{
case KEYCODE_L:
ROS_DEBUG("LEFT");
angular_ = 2.0;
break;
case KEYCODE_R:
ROS_DEBUG("RIGHT");
angular_ = -2.0;
break;
case KEYCODE_U:
ROS_DEBUG("UP");
linear_ = 0.15;
break;
case KEYCODE_D:
ROS_DEBUG("DOWN");
linear_ = -0.15;
break;
}
teleop_keyboard.launch
<node pkg="nxt_unipd" type="nxt_teleop_key" name="nxt_teleop_key" output="screen"> <param name="scale_linear" value="1" type="double"/> <param name="scale_angular" value="1" type="double"/> </node>
Sensors messages
Header header float64 range float64 range_min float64 range_max float64 spread_angle