Merge branch 'master' into remove-unnecessary-advertise

pr2eus/CMakeLists.txt

@@ -50,6 +50,7 @@ if(CATKIN_ENABLE_TESTING)
     find_package(pr2_gazebo QUIET)
     if(pr2_gazebo_FOUND AND (NOT pr2_gazebo_VERSION VERSION_LESS 2.0.7)) # test only supports indigo
       add_rostest(test/pr2-ri-test.launch)
+      add_rostest(test/pr2-ri-test-namespace.launch)
     endif()
     add_rostest(test/pr2-ri-test-simple.launch)
     add_rostest(test/robot-init-test.test) # this uses pr2

pr2eus/pr2-interface.l

@@ -58,21 +58,21 @@
        (setf (gethash type controller-table) (list action))
        ))
    ;;
-   (ros::subscribe "/pressure/r_gripper_motor" pr2_msgs::PressureState
+   (ros::subscribe (if namespace (format nil "~A/~A" namespace "pressure/r_gripper_motor") "/pressure/r_gripper_motor") pr2_msgs::PressureState
                    #'send self :pr2-fingertip-callback :rarm-pressure :groupname groupname)
-   (ros::subscribe "/pressure/l_gripper_motor" pr2_msgs::PressureState
+   (ros::subscribe (if namespace (format nil "~A/~A" namespace "pressure/l_gripper_motor") "/pressure/l_gripper_motor") pr2_msgs::PressureState
                    #'send self :pr2-fingertip-callback :larm-pressure :groupname groupname)
-   (ros::subscribe "/r_gripper_controller/state" pr2_controllers_msgs::JointControllerState
+   (ros::subscribe (if namespace (format nil "~A/~A" namespace "r_gripper_controller/state") "/r_gripper_controller/state") pr2_controllers_msgs::JointControllerState
                    #'send self :pr2-gripper-state-callback :rarm :groupname groupname)
-   (ros::subscribe "/l_gripper_controller/state" pr2_controllers_msgs::JointControllerState
+   (ros::subscribe (if namespace (format nil "~A/~A" namespace "l_gripper_controller/state") "/l_gripper_controller/state") pr2_controllers_msgs::JointControllerState
                    #'send self :pr2-gripper-state-callback :larm :groupname groupname)
    ;;
    (setq r-gripper-action (instance ros::simple-action-client :init
-                                    "/r_gripper_controller/gripper_action"
+                                    (if namespace (format nil "~A/~A" namespace "r_gripper_controller/gripper_action") "/r_gripper_controller/gripper_action")
                                     pr2_controllers_msgs::Pr2GripperCommandAction
                                     :groupname groupname))
    (setq l-gripper-action (instance ros::simple-action-client :init
-                                    "/l_gripper_controller/gripper_action"
+                                    (if namespace (format nil "~A/~A" namespace "l_gripper_controller/gripper_action") "/l_gripper_controller/gripper_action")
                                     pr2_controllers_msgs::Pr2GripperCommandAction
                                     :groupname groupname))
    ;; wait for pr2-action server (except move_base)
@@ -752,3 +752,6 @@ Example: (send self :gripper :rarm :position) => 0.00"
   (let ((req (instance topic_tools::MuxSelectRequest :init
                        :topic (if enable "tilt_scan_filtered" "empty_cloud"))))
     (ros::service-call "/tilt_laser_mux/select" req)))
+
+
+(provide :pr2-interface "pr2-interface.l")

pr2eus/pr2-utils.l

@@ -362,4 +362,4 @@
   )
 
 
-
+(provide :pr2-utils "pr2-utils.l")

pr2eus/robot-interface.l

@@ -1269,19 +1269,20 @@ Return value is a list of interpolatingp for all controllers, so (null (some #'i
      (setq odom-topic odom-topic-name)
      (setq cmd-vel-topic cmd-vel-topic-name)
      (setq move-base-action (instance ros::simple-action-client :init
-                                      move-base-action-name move_base_msgs::MoveBaseAction
+                                      (if namespace (format nil "~A/~A" namespace move-base-action-name) move-base-action-name)
+                                      move_base_msgs::MoveBaseAction
                                       :groupname groupname))
      (when base-controller-action-name
        (setq move-base-trajectory-action
              (instance ros::simple-action-client :init
-                       base-controller-action-name
+                       (if namespace (format nil "~A/~A" namespace base-controller-action-name) base-controller-action-name)
                        control_msgs::FollowJointTrajectoryAction
                        :groupname groupname))
        (unless (send move-base-trajectory-action :wait-for-server 3)
          (ros::ros-warn "move-base-trajectory-action is not found")
          (setq move-base-trajectory-action nil))
        (setq move-base-trajectory-joint-names base-controller-joint-names))
-     (ros::subscribe odom-topic-name nav_msgs::Odometry
+     (ros::subscribe (if namespace (format nil "~A/~A" namespace odom-topic-name) odom-topic-name) nav_msgs::Odometry
 		     #'send self :odom-callback :groupname groupname)
      ))
   ;;
@@ -1525,14 +1526,14 @@ Return value is a list of interpolatingp for all controllers, so (null (some #'i
    (x y d) ;; [m] [m] [degree]
    (when (send self :simulation-modep)
      (return-from :send-cmd-vel-raw t))
-   (unless (ros::get-topic-publisher cmd-vel-topic)
-     (ros::advertise cmd-vel-topic geometry_msgs::Twist 1)
+   (unless (ros::get-topic-publisher (if namespace (format nil "~A/~A" namespace cmd-vel-topic) cmd-vel-topic))
+     (ros::advertise (if namespace (format nil "~A/~A" namespace cmd-vel-topic) cmd-vel-topic) geometry_msgs::Twist 1)
      (unix:sleep 1))
    (let ((msg (instance geometry_msgs::Twist :init)))
      (send msg :linear :x x)
      (send msg :linear :y y)
      (send msg :angular :z d)
-     (ros::publish cmd-vel-topic msg)))
+     (ros::publish (if namespace (format nil "~A/~A" namespace cmd-vel-topic) cmd-vel-topic) msg)))
   (:go-velocity
    (x y d ;; [m/sec] [m/sec] [rad/sec]
     &optional (msec 1000) ;; msec is total animation time [msec]

pr2eus/test/pr2-ri-test-bringup.launch

@@ -0,0 +1,147 @@
+<launch>
+  <!-- start up empty world : expand following launch files to respawn gazebo -->
+  <!-- include file="$(find pr2_gazebo)/launch/pr2_empty_world.launch" -->
+  <!-- include file="$(find gazebo_worlds)/launch/empty_world_paused.launch" -->
+  <!-- set use_sim_time flag -->
+  <param name="/use_sim_time" value="true" />
+  <arg name="gui" default="false"/>
+  <env name="DISPLAY" value=":0.0" if="$(arg gui)"/>
+  <env name="DISPLAY" value="" unless="$(arg gui)"/>
+
+  <arg name="launch_pr2_base_trajectory_action" default="false"
+       doc="launch pr2_base_trajectory_action for debugging" />
+
+  <!-- start empty world -->
+  <node name="gazebo" pkg="gazebo_ros" type="gzserver" args="-r worlds/empty.world" respawn="true" output="log"/>
+  <group if="$(arg gui)">
+    <node name="gazebo_gui" pkg="gazebo_ros" type="gzclient" respawn="false" output="screen"/>
+  </group>
+
+  <!-- start pr2 robot without image processing-->
+  <!-- <include file="$(find pr2_gazebo)/launch/pr2.launch" /> -->
+  <!-- Startup PR2 without any mechanism controllers -->
+  <!-- <include file="$(find pr2_gazebo)/launch/pr2_no_controllers.launch" pass_all_args="true"/> -->
+  <!-- send pr2 urdf to param server -->
+  <include file="$(find pr2_description)/robots/upload_pr2.launch" />
+
+  <!-- push robot_description to factory and spawn robot in gazebo -->
+  <arg name="ROS_NAMESPACE" default="$(optenv ROS_NAMESPACE)" />
+  <node name="spawn_pr2_model" pkg="gazebo_ros" type="spawn_model" args="$(optenv ROBOT_INITIAL_POSE) -unpause -urdf -param /$(arg ROS_NAMESPACE)/robot_description -gazebo_namespace /$(arg ROS_NAMESPACE)/gazebo -model pr2" respawn="false" output="screen" />
+
+  <!-- default bringup script -->
+  <!-- <include file="$(find pr2_gazebo)/launch/pr2_bringup.launch" /> -->
+  <!-- Controller Manager -->
+  <include file="$(find pr2_controller_manager)/controller_manager.launch" />
+
+  <!-- Fake Calibration -->
+  <node pkg="rostopic" type="rostopic" name="fake_joint_calibration"
+        args="pub /calibrated std_msgs/Bool true" />
+
+  <!-- Stereo image processing -->
+  <!-- <include file="$(find pr2_gazebo)/config/dualstereo_camera.launch" /> -->
+
+  <!-- Start image_proc inside camera namespace-->
+  <!-- <include file="$(find pr2_gazebo)/config/r_forearm_cam.launch" /> -->
+  <!-- <include file="$(find pr2_gazebo)/config/l_forearm_cam.launch" /> -->
+
+  <!-- diagnostics aggregator -->
+  <node pkg="diagnostic_aggregator" type="aggregator_node" name="diag_agg" args="Robot" />
+  <group ns="diag_agg">
+    <include file="$(find pr2_gazebo)/config/pr2_analyzers_simple.launch" />
+    <!--
+    <include file="$(find pr2_gazebo)/config/pr2_analyzers.launch" />
+    -->
+  </group>
+
+  <!-- Dashboard aggregation -->
+  <node pkg="pr2_dashboard_aggregator" type="dashboard_aggregator.py" name="pr2_dashboard_aggregator"/>
+
+  <!-- Robot pose ekf -->
+  <node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf" output="screen">
+    <param name="freq" value="30.0"/>
+    <param name="sensor_timeout" value="1.0"/>
+    <param name="publish_tf" value="true"/>
+    <param name="odom_used" value="true"/>
+    <param name="imu_used" value="true"/>
+    <param name="vo_used" value="false"/>
+    <remap from="odom" to="base_odometry/odom" />
+    <remap from="imu_data" to="torso_lift_imu/data" />
+  </node>
+
+  <!-- Base Laser dynamic_reconfigure -->
+  <node pkg="gazebo_plugins" type="hokuyo_node" name="base_hokuyo_node">
+    <param name="port" type="string" value="/etc/ros/sensors/base_hokuyo" />
+    <param name="frame_id" type="string" value="base_laser_link" />
+    <param name="min_ang" type="double" value="-2.2689" />
+    <param name="max_ang" type="double" value="2.2689" />
+    <param name="skip" type="int" value="1" />
+    <param name="intensity" value="false" />
+  </node>
+
+  <!-- Tilt Laser dynamic_reconfigure -->
+  <!-- <node pkg="gazebo_plugins" type="hokuyo_node" name="tilt_hokuyo_node"> -->
+  <!--   <param name="port" type="string" value="/etc/ros/sensors/tilt_hokuyo" /> -->
+  <!--   <param name="frame_id" type="string" value="laser_tilt_link" /> -->
+  <!--   <param name="min_ang" type="double" value="-0.829" /> -->
+  <!--   <param name="max_ang" type="double" value="0.829" /> -->
+  <!--   <param name="skip" type="int" value="1" /> -->
+  <!--   <param name="intensity" value="true" /> -->
+  <!-- </node> -->
+
+  <!-- Buffer Server -->
+  <node pkg="tf2_ros" type="buffer_server" name="tf2_buffer_server" output="screen">
+    <param name="buffer_size" value="120.0"/>
+  </node>
+
+  <!-- Spawns the synchronizer -->
+  <!-- <node type="camera_synchronizer" name="camera_synchronizer_node" pkg="gazebo_plugins" output="screen" /> -->
+
+  <!-- testing only: simulate torso counter weight spring to reduce load on the torso joint
+  <node name="simulate_torso_spring" pkg="pr2_gazebo" type="pr2_simulate_torso_spring.py" respawn="false" output="screen" />
+  -->
+
+  <!-- Load and Start Default Controllers -->
+  <include file="$(find pr2_controller_configuration_gazebo)/launch/pr2_default_controllers.launch" />
+
+  <!-- fix gripper threshold -->
+  <param name="/l_gripper_controller/gripper_action_node/stall_velocity_threshold" value="1.0" /> <!-- 0.33 -->
+  <param name="/r_gripper_controller/gripper_action_node/stall_velocity_threshold" value="1.0" />
+
+  <!-- for :move-to and :go-velocity tests -->
+  <node pkg="fake_localization" type="fake_localization" name="fake_localization" >
+    <param name="~odom_frame_id" value="odom_combined" />
+  </node>
+  <node name="map_server" pkg="map_server" type="map_server" args="$(find pr2eus)/test/blank_map.yaml"/>
+
+  <node pkg="move_base" type="move_base" name="move_base_node" output="screen">
+    <remap from="odom" to="base_odometry/odom" />
+    <remap from="cmd_vel" to="base_controller/command" />
+
+    <!-- Use the dwa local planner for the PR2 -->
+    <param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS" />
+
+    <!-- Load common configuration files -->
+    <rosparam file="$(find pr2_navigation_config)/move_base/move_base_params.yaml" command="load" />
+    <rosparam file="$(find pr2_navigation_config)/move_base/costmap_common_params.yaml" command="load" ns="global_costmap" />
+    <rosparam file="$(find pr2_navigation_config)/move_base/costmap_common_params.yaml" command="load" ns="local_costmap" />
+    <rosparam file="$(find pr2_navigation_config)/move_base/dwa_local_planner.yaml" command="load" ns="DWAPlannerROS" />
+    <rosparam file="$(find pr2_navigation_config)/move_base/recovery_behaviors.yaml" command="load" />
+
+    <!-- Load slam navigation specific parameters -->
+    <rosparam file="$(find pr2_navigation_slam)/config/move_base_params.yaml" command="load" />
+    <rosparam file="$(find pr2_navigation_slam)/config/global_costmap_params.yaml" command="load" />
+    <rosparam file="$(find pr2_navigation_slam)/config/local_costmap_params.yaml" command="load"  />
+    <rosparam>
+      global_costmap:
+        observation_sources: base_scan
+      local_costmap:
+        observation_sources: base_scan
+    </rosparam>
+  </node>
+  <node name="tf_world_publisher" pkg="tf" type="static_transform_publisher"
+	args="0 0 0 0 0 0 /world /map 100" />
+
+  <include if="$(arg launch_pr2_base_trajectory_action)"
+           file="$(find pr2_base_trajectory_action)/launch/pr2_base_trajectory_action.launch" />
+
+</launch>

pr2eus/test/pr2-ri-test-namespace.l

@@ -0,0 +1,74 @@
+(require :unittest "lib/llib/unittest.l")
+(load "package://pr2eus/pr2-interface.l")
+
+(init-unit-test)
+
+(setq *ri* (instance pr2-interface :init :namespace "namespace"))
+
+(defmacro assert (pred &optional (message "") &rest args)
+  `(let (failure ret)
+     (warning-message 4 ";; Check ~A ~A~%" ',pred ,message)
+     (setq ret ,pred)
+     ;; lisp::step could not work with macros..
+     ;;     (if (and (listp ',pred) (functionp (car ',pred)))
+     ;;	 (setq ret (lisp::step ,pred))
+     ;;       (setq ret ,pred))
+     (if (not ret)
+         ;; escape <> for xml
+         (send *unit-test* :increment-failure ',pred (format nil ,message ,@args)
+               (escape-xml-string (subseq (send *error-output* :buffer) 0 (or (position 0 (send *error-output* :buffer)) (length (send *error-output* :buffer)))))))
+     ))
+
+(deftest test-subscriber
+  (let ()
+    (send *ri* :spin-once)
+
+    (send *ri* :send-cmd-vel-raw 0 0 0) ;; publish /base_controller/command
+
+    (let ((topic (if (*ri* . namespace) (format nil "/~A/base_controller/command" (*ri* . namespace))
+                   "/base_controller/command")))
+      (assert (and (ros::get-num-subscribers topic)
+                   (> (ros::get-num-subscribers topic) 0)) "/base_controller/command [geometry_msgs/Twist]"))
+
+    ;; * /j_robotsound [sound_play/SoundRequest]
+    ;; * /robot_interface_marker_array [visualization_msgs/MarkerArray]
+
+    (let ((topic (if (*ri* . namespace) (format nil "/~A/base_controller/follow_joint_trajectory/status" (*ri* . namespace))
+                   "/base_controller/follow_joint_trajectory/status")))
+      (ros::advertise topic actionlib_msgs::GoalStatusArray)
+      (assert (and (ros::get-num-subscribers topic)
+                   (> (ros::get-num-subscribers topic) 0)) "/base_controller/follow_joint_trajectory/status [unknown type]"))
+
+    (assert (send *ri* :state :odom) "/base_odometry/odom [nav_msgs/Odometry]")
+    (assert (send *ri* :state :head_traj_controller/state) "/head_traj_controller/state [pr2_controllers_msgs/JointTrajectoryControllerState]")
+    (assert (send *ri* :state :potentio-vector) "/joint_states [sensor_msgs/JointState]")
+    (assert (send *ri* :state :l_arm_controller/state) "/l_arm_controller/state [pr2_controllers_msgs/JointTrajectoryControllerState]")
+    (assert (send (*ri* . l-gripper-action) :wait-for-server 5) "/l_gripper_controller/gripper_action/status [actionlib_msgs/GoalStatusArray]")
+    (assert (send *ri* :state :larm-position) "/l_gripper_controller/state [pr2_controllers_msgs/JointControllerState]")
+    (assert (send (*ri* . move-base-action) :wait-for-server 5) "/move_base/status [actionlib_msgs/GoalStatusArray]")
+
+    ;; * /pressure/l_gripper_motor [unknown type]
+    (let ((topic (if (*ri* . namespace) (format nil "/~A/pressure/l_gripper_motor" (*ri* . namespace))
+                   "/pressure/l_gripper_motor")))
+      (ros::advertise topic pr2_msgs::PressureState)
+      (assert (and (ros::get-num-subscribers topic)
+                   (> (ros::get-num-subscribers topic) 0)) "/pressure/l_gripper_motor [unknown type]"))
+    ;; * /pressure/r_gripper_motor [unknown type]
+    (let ((topic (if (*ri* . namespace) (format nil "/~A/pressure/r_gripper_motor" (*ri* . namespace))
+                   "/pressure/r_gripper_motor")))
+      (ros::advertise topic pr2_msgs::PressureState)
+      (assert (and (ros::get-num-subscribers topic)
+                   (> (ros::get-num-subscribers topic) 0)) "/pressure/r_gripper_motor [unknown type]"))
+
+    (assert (send *ri* :state :r_arm_controller/state) "/r_arm_controller/state [pr2_controllers_msgs/JointTrajectoryControllerState]")
+    (assert (send (*ri* . r-gripper-action) :wait-for-server 5) "/r_gripper_controller/gripper_action/status [actionlib_msgs/GoalStatusArray]")
+    (assert (send *ri* :state :rarm-position) "/r_gripper_controller/state [pr2_controllers_msgs/JointControllerState]")
+    ;; * /tf [tf/tfMessage]
+    ;; * /tf_static [tf2_msgs/TFMessage]
+    (assert (send *ri* :state :torso_controller/state) "/torso_controller/state [pr2_controllers_msgs/JointTrajectoryControllerState]")
+    ))
+
+(run-all-tests)
+(exit)
+
+

pr2eus/test/pr2-ri-test-namespace.launch

@@ -0,0 +1,12 @@
+<launch>
+  <arg name="gui" default="false"/>
+  <include file="$(find pr2eus)/test/pr2-ri-test-bringup.launch" ns="namespace">
+    <arg name="gui" default="$(arg gui)"/>
+    <arg name="ROS_NAMESPACE" value="namespace" />
+  </include>
+
+  <!-- start test -->
+  <test test-name="pr2_ri_test_namespace" pkg="roseus" type="roseus" retry="1"
+        args="$(find pr2eus)/test/pr2-ri-test-namespace.l"
+        time-limit="800" />
+</launch>