Company Logo
  • fibremap_940x230.jpg
  • intro1b_940x230.jpg
  • intro2_940x230.jpg
  • intro3b_940x230.jpg
  • intro4_940x230.jpg


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.

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

nxt_msgs/Range Message

Header header
float64 range
float64 range_min
float64 range_max
float64 spread_angle

 




Powered by Joomla!®. Valid XHTML and CSS