Srv joint (#8)

* yes

* make python node visible and executable

* wrong attribute

* test

* update messages

* small message adjustment

* don't convert to rad

* remove prints

CMakeLists.txt

@@ -176,6 +176,7 @@ CATKIN_DEPENDS message_runtime
 catkin_install_python(PROGRAMS
   scripts/move.py
   scripts/look.py
+  scripts/joint_angle_speed_srv.py
   scripts/look_around.py
   scripts/look_down.py
   scripts/look_top_right.py

launch/manipulation_pepper.launch

@@ -6,6 +6,7 @@
     <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="look.py" name="look" output="screen"/>
+    <node pkg="manipulation_pepper" type="joint_angle_speed_srv.py" name="robobreizh_joint_angle" 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"/>
     <node pkg="manipulation_pepper" type="point_to_objects.py" name="point_to_objects" output="screen"/>

scripts/joint_angle_speed_srv.py

@@ -0,0 +1,44 @@
+import qi
+import sys
+import numpy as np
+import rospy
+from robobreizh_msgs.srv import joint_position, joint_positionResponse
+
+
+class Motion():
+    def __init__(self, app):
+        app.start()
+        session = app.session
+        self.motion_service = session.service("ALMotion")
+        rospy.init_node('look_up')
+        rospy.Service('/robobreizh/manipulation/joint_angle_speed_srv', joint_position, self.animate)
+        rospy.spin()
+
+    def animate(self, req):
+        # Example showing multiple trajectories
+        names = req.joint_names
+        angle_list = []
+        for angle_list_req in req.angle_lists.row:
+                angle_list.append(angle_list_req.col)
+
+        time_list = []
+        for time_list_req in req.time_lists.row:
+                time_list.append(time_list_req.col)
+
+
+        # set times for joints every seconds
+        isAbsolute = True
+        self.motion_service.angleInterpolation(names,angle_list ,time_list , isAbsolute)
+        return (joint_positionResponse())
+
+
+if __name__ == "__main__":
+    try:
+        connection_url = "tcp://127.0.0.1:9559"
+        app = qi.Application(
+            ["SoundProcessingModule", "--qi-url=" + connection_url])
+    except RuntimeError:
+        print("Can't connect to Naoqi at ip \"" + ip + "\" on port " + str(port) + ".\n"
+              "Please check your script arguments. Run with -h option for help.")
+        sys.exit(1)
+    motion = Motion(app)