diff --git a/jsk_pepper_robot/peppereus/pepper-interface.l b/jsk_pepper_robot/peppereus/pepper-interface.l index 64cad64f12..5ad7cb521d 100644 --- a/jsk_pepper_robot/peppereus/pepper-interface.l +++ b/jsk_pepper_robot/peppereus/pepper-interface.l @@ -2,7 +2,6 @@ (load "package://peppereus/pepper.l") (load "package://pr2eus/robot-interface.l") -(ros::load-ros-manifest "jsk_pepper_startup") (ros::load-ros-manifest "peppereus") (if (not (find-package "NAOQI_BRIDGE_MSGS")) @@ -20,10 +19,10 @@ (ros::advertise "cmd_pose" geometry_msgs::Pose2D 1) (ros::advertise "cmd_vel" geometry_msgs::Twist 1) (ros::advertise "/speech" std_msgs::String 1) - (ros::advertise "joint_angles" naoqi_bridge_msgs::JointAnglesWithSpeed 1) + (ros::advertise "/pepper_robot/pose/joint_angles" naoqi_bridge_msgs::JointAnglesWithSpeed 1) (setq joint-stiffness-trajectory-action (instance ros::simple-action-client :init - "joint_stiffness_trajectory" + "/pepper_robot/pose/joint_stiffness_trajectory" naoqi_bridge_msgs::JointTrajectoryAction)) self) ;; @@ -31,7 +30,7 @@ () (list (list - (cons :controller-action "joint_trajectory") + (cons :controller-action "/pepper_robot/pose/joint_trajectory") ;;(cons :controller-state "joint_trajectory") (cons :controller-state "dummy_state") ;; this is dummy (cons :action-type naoqi_bridge_msgs::JointTrajectoryAction) @@ -76,7 +75,7 @@ (:larm (send start_grasp_msg :joint_names (list "LHand")) (send start_grasp_msg :joint_angles (list value)))) - (ros::publish "joint_angles" start_grasp_msg) + (ros::publish "/pepper_robot/pose/joint_angles" start_grasp_msg) )) (:start-grasp (&optional (arm :arms)) @@ -176,8 +175,8 @@ :time_from_start (ros::time 1)))) (send joint-stiffness-trajectory-action :send-goal goal) )) - (:servo-on () (call-empty-service "wakeup")) - (:servo-off () (call-empty-service "rest")) + (:servo-on () (call-empty-service "pepper_robot/pose/wakeup")) + (:servo-off () (call-empty-service "pepper_robot/pose/rest")) ) (defmethod pepper-robot