Cant send two diffenrents command to the robot.

edit

Hi guys We are two bachelor student working in ros and python for the first time. We are trying to build a robot arm using software from our school. Our robot is given a xyz position depending on what number it gets from the subscriber. Here is the problem. When we are getting a new int from the subscriber the program is calculating a new position for our robot and we can observe that the program calculates the new position, but nothing happens on the robot arm. It looks like we are sending the same command twice and not overwrite the goal or something. Is there a feature to reset the goal to make sure the we are not sending the same command, but a new one to our robot or are we doing something else wrong. The robot will react on a new int from the subscriber if we reset the program. Best regards Almost Engineers

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
Created on Mon Dec  2 13:47:22 2019

@author: ros
"""
import rospy
import actionlib
from control_msgs.msg import FollowJointTrajectoryAction
from control_msgs.msg import FollowJointTrajectoryFeedback
from control_msgs.msg import FollowJointTrajectoryResult
from control_msgs.msg import FollowJointTrajectoryGoal
from trajectory_msgs.msg import JointTrajectoryPoint
from trajectory_msgs.msg import JointTrajectory
from math import cos, sin, atan2, sqrt, pow
from std_msgs.msg import UInt8
from std_msgs.msg import String

def number(data):
     print(data)

     ans = node.xyzfromdice(data) 
     if ans:
         node.send_command()

         print('Done')

         print ('motor')

def listener():
    rospy.Subscriber('chatter', UInt8, number)
    rospy.spin()

def invkin(xyz):
    """
    Python implementation of the the inverse kinematics for the crustcrawler
    Input: xyz position
    Output: Angels for each joint: q1,q2,q3,q4

    You might adjust parameters (d1,a1,a2,d4).
    The robot model shown in rviz can be adjusted accordingly by editing au_crustcrawler_ax12.urdf
    """


    d1 = 12.0; # cm (height of 2nd joint)
    a1 = 0.0; # (distance along "y-axis" to 2nd joint)
    a2 = 17.0; # (distance between 2nd and 3rd joints)
    d4 = 7.5; # (distance from 3rd joint to gripper center - all inclusive, ie. also 4th joint)

    oc = xyz;

    xc = oc[0];
    yc = oc[1];
    zc = oc[2];


    # Calcutlating q1
    q1 = atan2(yc, xc);

    #Calculating q2 and q3
    r2 = pow((xc - a1 * cos(q1)), 2) + pow((yc - a1 * sin(q1)), 2);
    s = zc - d1
    D = (r2 + pow(s, 2) - pow(a2, 2) - pow(d4, 2)) / (2 * a2 * d4);

    q3 = atan2(-sqrt(1-pow(D,2)), D);

    q2 = atan2(s, sqrt(r2)) - atan2(d4 * sin(q3), a2 + d4 * cos(q3));

    # For now, the rotational part is not looked into
    q4 = 0;
    print('return q1 q2 q3 q4')
    return q1,q2,q3,q4

class ActionExampleNode:

    N_JOINTS = 4
    def __init__(self,server_name):
        self.client = actionlib.SimpleActionClient(server_name, FollowJointTrajectoryAction)

        self.joint_positions = []
        self.names =["joint1",
                "joint2",
                "joint3",
                "joint4"
                ]

    def xyzfromdice(self, data):
        # the list of xyz points we want to plan
        if data == UInt8(1):
            xyz_positions = [
                [1.0, 1.0, -6.0]
                ]

            print("Terning 1");
            pubTwo.publish("Current diceroll is one")

        elif data == UInt8(2):
            xyz_positions = [
                [1.0, 1.0, -6.0 ...