movement server and tray pose (#1)

CMakeLists.txt

@@ -14,6 +14,8 @@ find_package(catkin REQUIRED COMPONENTS
   geometry_msgs
   message_generation
   naoqi_bridge_msgs
+  actionlib
+  actionlib_msgs
 )
 
 
@@ -62,17 +64,20 @@ add_service_files(
 )
 
 ## Generate actions in the 'action' folder
-# add_action_files(
-#   FILES
-#   # Speech_processing.action
-# )
+add_action_files(
+  DIRECTORY 
+  action 
+  FILES 
+  Movement.action
+)
 
 ## Generate added messages and services with any dependencies listed here
 generate_messages(
    DEPENDENCIES
    std_msgs
    geometry_msgs  # Or other packages containing msgs
    naoqi_bridge_msgs
+   actionlib_msgs
 )
 
 ################################################
@@ -176,6 +181,8 @@ catkin_install_python(PROGRAMS
   scripts/look_above_wall.py
   scripts/look_up.py
   scripts/point_to_position.py
+  scripts/movement_client.py
+  scripts/movement_server.py
   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
 

README.md

@@ -10,3 +10,33 @@ chmod +x ./install.sh && ./install.sh
 ## 3. Usage
 
 ## 4. Roadmap
+
+## Movement actions
+
+Possible actions : pose_middle, pose_dialog, raise_hand, pose_restaurant, stop_pose_restaurant
+
+Send actions with rosrun :
+
+```bash
+rosrun manipulation_pepper movement_client.py <ACTION_NAME>
+```
+
+Send actions by publishing a message ont the `/Movement/Goal` topic :
+
+```bash
+rostopic pub /Movement/goal manipulation_pepper/MovementActionGoal "header:
+  seq: 0
+  stamp:
+    secs: 0
+    nsecs: 0
+  frame_id: ''
+goal_id:
+  stamp:
+    secs: 0
+    nsecs: 0
+  id: ''
+goal:
+  order: '<ACTION_NAME>'"
+```
+
+Replace <ACTION_NAME> with one of the possible actions (pose_middle, pose_dialog, raise_hand, pose_restaurant, stop_pose_restaurant)

action/Movement.action

@@ -0,0 +1,8 @@
+#goal definition
+string order
+---
+#result definition
+bool success
+---
+#feedback
+string feedback

launch/manipulation_pepper.launch

@@ -6,4 +6,5 @@
     <node pkg="manipulation_pepper" type="look_above_wall.py" name="look_above_wall" output="screen"/>
     <node pkg="manipulation_pepper" type="look_down_stickler.py" name="look_down_stickler" output="screen"/>
     <node pkg="manipulation_pepper" type="point_to_position.py" name="point_in_front" output="screen"/>
+    <node pkg="manipulation_pepper" type="movement_server.py" name="movement_server" output="screen"/>
 </launch>

package.xml

@@ -65,6 +65,10 @@
 	<build_export_depend>naoqi_bridge_msgs</build_export_depend>
 	<exec_depend>naoqi_bridge_msgs</exec_depend>
 
+  <build_depend>actionlib</build_depend>
+	<build_export_depend>actionlib</build_export_depend>
+	<exec_depend>actionlib</exec_depend>
+
 	<!-- <build_depend>libqi</build_depend>
 	<build_export_depend>libqi</build_export_depend>
 	<exec_depend>libqi</exec_depend> -->

scripts/movement.py

@@ -0,0 +1,113 @@
+from actionlib import SimpleActionClient
+from naoqi_bridge_msgs.msg import JointTrajectoryAction, JointAnglesWithSpeed
+from geometry_msgs.msg import Twist
+from std_msgs.msg import String
+import numpy as np
+from threading import Thread
+import rospy
+import time
+
+## Movement class contains all variables and functions for the movements of the Pepper robot
+
+class Movement :
+
+	##Constructor of the Movement class
+	#Initialise multiple attributes which are set by default
+	#@param self
+	def __init__(self):
+		rospy.init_node("movement")
+
+		self.client_traj = SimpleActionClient("joint_trajectory",JointTrajectoryAction)
+		self.pub_turn = rospy.Publisher("cmd_vel",Twist,queue_size=10)
+		self.pub_angles = rospy.Publisher("joint_angles",JointAnglesWithSpeed,queue_size=10)
+		while self.pub_turn.get_num_connections()!=0 or self.pub_angles.get_num_connections()!=0 :
+			pass
+		self.joint_names = ["HeadYaw", "HeadPitch", "LShoulderPitch", "LShoulderRoll", "LElbowYaw", "LElbowRoll", "LWristYaw", "RShoulderPitch", "RShoulderRoll", "RElbowYaw", "RElbowRoll", "RWristYaw"]
+		self.dialog = [0.018, -0.382, 1.766, 0.074, -1.720, -0.114, 0.048, 1.740, -0.074, 1.670, 0.104, -0.026]
+		self.detection = [0.018, 0.288, 1.766, 0.074, -1.720, -0.114, 0.048, 1.740, -0.074, 1.670, 0.104, -0.026]
+		self.middle = [0.018, -0.047, 1.766, 0.074, -1.720, -0.114, 0.048, 1.740, -0.074, 1.670, 0.104, -0.026]
+
+		self.joint_names_handup = ["HeadYaw","HeadPitch", "RShoulderPitch", "RShoulderRoll", "RElbowYaw", "RElbowRoll", "RWristYaw"]
+		self.handup_joints = [0.018, -0.382, 1.293, -0.350, 1.704, 1.273, 1.539]
+
+		self.joint_rarm = ["RShoulderPitch", "RShoulderRoll", "RElbowYaw", "RElbowRoll", "RWristYaw"]
+		self.joint_larm = ["LShoulderPitch", "LShoulderRoll", "LElbowYaw", "LElbowRoll", "LWristYaw"]
+		self.joint_head = ["HeadYaw","HeadPitch"]
+		self.joint_rhand = ["RHand"]
+		self.joint_lhand = ["LHand"]
+
+		self.thread_pose_restaurant = None
+	##Destructor of a Movement object
+	#@param self
+	def __del__(self):
+		if self.thread_pose_restaurant != None :
+			self.thread_pose_restaurant.join()
+
+	##Function to make the robot rise his hand up
+	#@param self
+	def move_hand_up(self):
+		print("thread hand up")
+		msg = JointAnglesWithSpeed()
+		msg.joint_names = self.joint_names_handup
+		msg.joint_angles = self.handup_joints
+		msg.speed = 0.2
+		#while self.bool_hand_up :
+		print(f"Publishing : {msg}")
+		self.pub_angles.publish(msg)
+		print("end thread hand up")
+
+	def close_hand(self):
+		goal = 20
+		msg = JointAnglesWithSpeed()
+		msg.joint_names = self.joint_rhand
+		msg.joint_angles = [np.deg2rad(goal)]
+		msg.speed = 0.2
+		print(f"Publishing : {msg}")
+		self.pub_angles.publish(msg)
+
+	def pose_middle(self):
+		msg = JointAnglesWithSpeed()
+		msg.joint_names = self.joint_names
+		msg.joint_angles = self.middle
+		msg.speed = 0.1
+		print(f"Publishing : {msg}")
+		self.pub_angles.publish(msg)
+
+	def pose_dialog(self):
+		msg = JointAnglesWithSpeed()
+		msg.joint_names = self.joint_names
+		msg.joint_angles = self.dialog
+		msg.speed = 0.1
+		print(f"Publishing : {msg}")
+		self.pub_angles.publish(msg)
+
+	def pose_restaurant_task(self):
+		#left = [0.610865,0.0872665,-1.48353,-0.523599,-1.74533] #35/5/-85/-30/-100
+		#right = [0.610865,-0.0872665,1.48353,0.523599,1.74533] #35/-5/85/30/100
+
+		left = [0.314159,0.0523599,-0.785398,-0.610865,-1.8326]
+		right = [0.314159,-0.0523599,0.785398,0.610865,1.8326] #18/-3/45/35/105
+
+		msgl = JointAnglesWithSpeed()
+		msgl.joint_names = self.joint_larm
+		msgl.joint_angles = left
+		msgl.speed = 0.1
+
+		msgr = JointAnglesWithSpeed()
+		msgr.joint_names = self.joint_rarm
+		msgr.joint_angles = right
+		msgr.speed = 0.1
+
+		while(self.holding_pose_restaurant):
+			self.pub_angles.publish(msgl)
+			self.pub_angles.publish(msgr)
+			time.sleep(1)
+
+	def pose_restaurant(self):
+		self.holding_pose_restaurant = True
+		self.thread_pose_restaurant = Thread(target=self.pose_restaurant_task)
+		self.thread_pose_restaurant.start()
+
+	def stop_pose_restaurant(self):
+		self.holding_pose_restaurant = False
+		self.thread_pose_restaurant.join()

scripts/movement_client.py

@@ -0,0 +1,30 @@
+import rospy
+import actionlib
+import sys
+import argparse
+from std_msgs.msg import String
+from manipulation_pepper.msg import MovementAction, MovementGoal
+
+def movement_client(order):
+    client = actionlib.SimpleActionClient('Movement', MovementAction)
+    client.wait_for_server()
+
+    goal = MovementGoal(order=order)
+
+    client.send_goal(goal)
+    client.wait_for_result()
+    return client.get_result()
+
+if __name__ == '__main__':
+    parser = argparse.ArgumentParser(
+        description="Send an order to the movement server.")
+    parser.add_argument("order", help="Order to send (raise_hand, pose_dialog, pose_middle, pose_restaurant, stop_pose_restaurant)")
+    args = parser.parse_args()
+
+    if args.order:
+        try:
+            rospy.init_node('movement_client')
+            result = movement_client(args.order)
+            print(f"Result : {result}")
+        except rospy.ROSInterruptException:
+            print("program interrupted before completion", file=sys.stderr)

scripts/movement_server.py

@@ -0,0 +1,53 @@
+import rospy
+import actionlib
+from std_msgs.msg import String
+from manipulation_pepper.msg import MovementAction, MovementResult, MovementFeedback
+from movement import Movement
+
+class MovementActionServer(object):
+    def __init__(self):
+        self.movement = Movement()
+        self.action_server = actionlib.SimpleActionServer("Movement", MovementAction, execute_cb=self.execute_cb, auto_start = False)
+        self.action_server.start()
+
+        rospy.loginfo("Movement server started")
+
+    def execute_cb(self, goal):
+        rospy.loginfo(f"Received order {goal.order}.")
+
+        #rospy.sleep(1)
+
+        if(goal.order =="pose_middle"):
+            rospy.loginfo("Executing pose middle")
+            self.movement.pose_middle()
+            result = MovementResult(success=True)
+        elif(goal.order=="pose_dialog"):
+            rospy.loginfo("Executing pose dialog")
+            self.movement.pose_dialog()
+            result = MovementResult(success=True)
+        elif(goal.order=="pose_restaurant"):
+            rospy.loginfo("Executing pose restaurant")
+            self.movement.pose_restaurant()
+            self.action_server.publish_feedback(MovementFeedback(feedback="holding restaurant pose"))
+            result = MovementResult(success=True)
+        elif(goal.order=="stop_pose_restaurant"):
+            rospy.loginfo("Executing stop pose restaurant")
+            self.action_server.publish_feedback(MovementFeedback(feedback="stopping restaurant pose"))
+            self.movement.stop_pose_restaurant()
+            self.movement.pose_middle() # Remove later
+            result = MovementResult(success=True)
+        elif(goal.order=="raise_hand"):
+            rospy.loginfo("Executing raise hand")
+            self.movement.move_hand_up()
+            result = MovementResult(success=True)
+        else:
+            rospy.logwarn(f"Unrecognized order : {goal.order}")
+            result = MovementResult(success=False)
+
+        rospy.loginfo(f"Sending result : {result}")
+        self.action_server.set_succeeded(result)
+
+if __name__ == '__main__':
+    rospy.init_node('movement')
+    server = MovementActionServer()
+    rospy.spin()

setup.py

@@ -1,7 +1,7 @@
 from distutils.core import setup
 from catkin_pkg.python_setup import generate_distutils_setup
 d = generate_distutils_setup(
-    packages=['manipulation_utils'],
+    packages=['manipulation_utils','movement'],
     package_dir={'': 'scripts'}
 )
 setup(**d)