Travis Deyle's profile - activity

I've noticed a peculiar behavior in sbpl_lattice_planner, where ~50% of actionlib requests (/map frame) result in a final heading that is consistently offset by +45-degrees. The other 50% of the time, the correct heading is obtained. In all cases, the correct (x,y) position is achieved. Thoughts?

I have verified that the correct heading is being sent (rviz markers and using /move_base_simple/goal), but the issue persists.

The pertinent launch files / parameters are below.

SBPLLatticePlanner:
  environment_type: XYThetaLattice
  planner_type: ARAPlanner
  allocated_time: 5.0
  initial_epsilon: 3.0
  forward_search: false
  lethal_obstacle: 5
  nominalvel_mpersecs: 0.1
  timetoturn45degsinplace_secs: 1.5

TrajectoryPlannerROS:
  #Independent settings for the local costmap
  transform_tolerance: 0.3
  sim_time: 1.7
  sim_granularity: 0.025
  dwa: true
  vx_samples: 20
  vtheta_samples: 20
  max_vel_x: 0.250
  #max_vel_x: 0.65
  min_vel_x: 0.1
  max_rotational_vel: 1.0
  min_in_place_rotational_vel: 0.4
  xy_goal_tolerance: 0.10
  yaw_goal_tolerance: 0.06
  goal_distance_bias: 0.2
  path_distance_bias: .9
  occdist_scale: 0.05
  heading_lookahead: 0.325
  oscillation_reset_dist: 0.05
  holonomic_robot: true
  acc_lim_th: 3.2
  acc_lim_x: 2.5
  acc_lim_y: 2.5
  heading_scoring: false
  heading_scoring_timestep: 0.8
  holonomic_robot: true
  simple_attractor: false

launch
  node pkg="pr2_move_base" name="pr2_move_base_node" type="pr2_move_base.py" machine="c2"

  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="move_base" type="move_base" respawn="false" name="move_base_node" output="screen"
    remap from="odom" to="base_odometry/odom"
    remap from="cmd_vel" to="navigation/cmd_vel"
    param name="footprint_padding" value="0.10"
    param name="controller_frequency" value="10.0" 
    param name="controller_patience" value="100.0" 
    param name="base_global_planner" value="SBPLLatticePlanner" 
    param name="SBPLLatticePlanner/primitive_filename" value="$(find sbpl)/matlab/mprim/pr2.mprim" 

  /node
launch

I'm running up against a wall in the form of tf's tree restrictions. Namely, I'd like to have both gmapping and amcl running simultaneously (long story), but both publish XXX => odom_combined (naturally, the parent frame is param-configurable). The problem is, that tf (apparently) does not allow multiple parents (eg. it forces it to be a tree), and there doesn't seem to be a way to flip the directionality of the transform (odom_combined => XXX).

I thought perhaps tf_remap or tf_prefix would fit the bill... but after a few hours of experimentation, both approaches still end up in this same problem. I guess... what I'm really looking for is something akin to topic_tools/mux only for tf topics. Then I could just flip the mux when I want one transform or another.

Has anyone found a good solution to this sort of problem?

I've run across some curious behavior within smach, and I was curious if others have observed similar issues.

The issue arises when using a Concurrency container _and_ the smach_viewer. In essence, when running my state machine with smach_viewer and without concurrency, the state transitions are almost instantaneous. The same applies when running without the smach_viewer and with the concurrency section. However, when I run both the concurrency container and smach_viewer simultaneously, _all_ state transitions take 7+ seconds. If I start off without the viewer, the transitions are instantaneous, but degrade as soon as the viewer launches.

Any thoughts or suggestions?

PS -- Occasionally I get the following smach_viewer error (though I believe it is unrelated, as it occurs even when everything functions normally):

[ERROR] [WallTime: 1303127819.346248] bad callback: <bound method="" smachviewerframe._status_msg_update="" of="" <__main__.smachviewerframe;="" proxy="" of="" <swig="" object="" of="" type="" 'wxframe="" *'="" at="" 0x953f420=""> >>
Traceback (most recent call last):
  File "/opt/ros/diamondback/stacks/ros_comm/clients/rospy/src/rospy/topics.py", line 563, in _invoke_callback
    cb(msg)
  File "/opt/ros/diamondback/stacks/executive_smach_visualization/smach_viewer/smach_viewer.py", line 659, in _status_msg_update
    if container.update_status(msg):
  File "/opt/ros/diamondback/stacks/executive_smach_visualization/smach_viewer/smach_viewer.py", line 146, in update_status
    self._local_data._data = pickle.loads(msg.local_data)
  File "/usr/lib/python2.6/pickle.py", line 1374, in loads
    return Unpickler(file).load()
  File "/usr/lib/python2.6/pickle.py", line 858, in load
    dispatch[key](self)
  File "/usr/lib/python2.6/pickle.py", line 1090, in load_global
    klass = self.find_class(module, name)
  File "/usr/lib/python2.6/pickle.py", line 1124, in find_class
    __import__(module)
ImportError: No module named geometry_msgs.msg._PoseStamped

Sorry I didn't respond sooner. Since the theta dimension is discretized in the planner there is the possibility for some error at the goal. However it shouldn't be any larger than 11.25 degrees, since the planner uses 22.5 degree intervals. I've run your launch file using stage and didn't notice it being off by angles as large as 45 degrees (I did observe some error closer to the 11.25). This is the launch file I used...

<launch>
  <node name="map_server" pkg="map_server" type="map_server" args="$(find sbpl_lattice_planner)/worlds/willow.pgm 0.025" />

  <node pkg="stage" type="stageros" name="stageros" args="$(find sbpl_lattice_planner)/worlds/willow.world" respawn="false" >
    <param name="base_watchdog_timeout" value="0.2"/>
  </node>

  <node name="fake_localization" pkg="fake_localization" type="fake_localization" respawn="false" />

  <node pkg="pr2_move_base" name="pr2_move_base_node" type="pr2_move_base.py" />

  <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="move_base" type="move_base" respawn="false" name="move_base_node" output="screen">
    <!--
    <remap from="odom" to="base_odometry/odom"/>
    <remap from="cmd_vel" to="navigation/cmd_vel"/>
    -->
    <param name="footprint_padding" value="0.10"/>
    <param name="controller_frequency" value="10.0" />
    <param name="controller_patience" value="100.0" />
    <param name="base_global_planner" value="SBPLLatticePlanner" />
    <param name="SBPLLatticePlanner/primitive_filename" value="$(find sbpl)/matlab/mprim/pr2.mprim" />
    <!-- I loaded your 2 yaml files here. -->
  </node>
</launch>

I upgraded my ROS installation yesterday, and this OGRE bug re-materialized.

My solution: Update my Ubuntu sources.list to packages.ros.org instead of the old code.ros.org. From the current ROS installation page:

sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu lucid main" > /etc/apt/sources.list.d/ros-latest.list'
wget http://packages.ros.org/ros.key -O - | sudo apt-key add -

Apparently the location of the packages has changed recently? Whereas the code.ros.org repos showed no new packages (after "apt-get update"), the packages.ros.org repos did. I ran a dist-upgrade using the packages.ros.org repos, which pulled down a (new?) version of the visualization libraries. This eliminated the bug. YMMV.

I finally revisited this issue when building some nice visualizations. The guys' description above is spot-on. If you're using the PR2, just run this:

rosrun xacro xacro.py `rospack find pr2_description`/robots/pr2.urdf.xacro -o pr2_urdf.xml

Then in a launch file (that also plays your bag file with tf messages):

<launch>
  <param name="robot_description" command="cat $(find mypackage)/pr2_urdf.xml" />
  <node pkg="rosbag" type="play" name="rosbagplay"
    args="--clock -q -r 1.0 $(find mypackage)/tmp.bag" />
</launch>

Works great. Thanks for the feedback y'all.

Hello,

I am also wondering the same. I just began with URDF today. I did not fully explore the urdf documentation, but it did not seem so clear about the minimal configuration you would need to run a simple model without a joint_state_publisher nor a robot_state_publisher. Also, I stay curious about how to deal with partial tf trees, and tf updates from multiple sources when dealing with joint and robot publisher.

Then, I would guess only tf... provided that all the frames of your model are broadcasted by some entity(ies), it should be ok.

It seems from rviz documentation that you only need to load your urdf model as a parameter.

Robot Description: The parameter to retrieve the urdf from. Uses searchParam() to search up the parameter tree for the value specified.

For example, in a launch file:

<param name="robot_description" textfile="PATH_TO_URDF_FILE" />

Should just do it. I can tell you more tomorrow when I will be integrating my brand new urdf model.

Raph

I'm trying to determine which topics are required for robot model visualization in rviz during a rosbag playback. Any helpful hints about topics and/or nodes that need to be run would be greatly appreciated.

As Travis discovered, this problem occurs when you use large userdata structures in Smach. These userdata structures are sent to the smach_viewer over ROS, taking up a long of cycles to do the serialization and sending. Smach could be smarter about sending userdata to the smach_viewer.