Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

fix to support namespace #440

Merged
merged 5 commits into from Jul 21, 2021
Merged

fix to support namespace #440

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
679052c
enable to run pr2-ri-test with ROS_NAMESPACE
k-okada Jun 30, 2020
c0654a8
split pr2-ri-test.launch with pr2-ri-test-bringup.launch
k-okada Jun 30, 2020
10c245c
add pr2-ri-test-namespace.launch to check #439
k-okada Jun 30, 2020
c566425
[pr2eus] fix to use namespace
Naoki-Hiraoka Jun 28, 2020
7e7a1e2
Merge branch 'master' into support_namespace
k-okada Jul 21, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
1 change: 1 addition & 0 deletions pr2eus/CMakeLists.txt
View file
Open in desktop
Expand Up @@ -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
Expand Down
12 changes: 6 additions & 6 deletions pr2eus/pr2-interface.l
View file
Open in desktop
Expand Up @@ -60,21 +60,21 @@
;;
(ros::advertise "j_robotsound" sound_play::SoundRequest 5)
;;
(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)
Expand Down
13 changes: 7 additions & 6 deletions pr2eus/robot-interface.l
View file
Open in desktop
Expand Up @@ -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)
))
;;
Expand Down Expand Up @@ -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]
Expand Down
147 changes: 147 additions & 0 deletions pr2eus/test/pr2-ri-test-bringup.launch
View file
Open in desktop
@@ -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>
74 changes: 74 additions & 0 deletions pr2eus/test/pr2-ri-test-namespace.l
View file
Open in desktop
@@ -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)


12 changes: 12 additions & 0 deletions pr2eus/test/pr2-ri-test-namespace.launch
View file
Open in desktop
@@ -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>