Robotics StackExchange | Archived questions

Limiting the change of arm to a button press

Hey,

I am trying to control my robot arm with an xbox controller and my python code is as follows

#!/usr/bin/env python

import sys
import copy
import rospy
import moveit_commander
from moveit_msgs.msg import *
from geometry_msgs.msg import *
from sensor_msgs.msg import *
import time

j1 = 0
a = 0
flag1 = 0
flag2 = 0

def callback(data):
    global j1, a, flag1, flag2
    robot = moveit_commander.RobotCommander()
    group1 = moveit_commander.MoveGroupCommander("igus1")
    end_effector_link =  group1.get_end_effector_link()
    display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',
              moveit_msgs.msg.DisplayTrajectory,queue_size=1)

    print "============ Waiting for RVIZ..."
    print "============ Reference frame: %s" % group1.get_planning_frame()
    print "============ End effector: %s" % group1.get_end_effector_link()
    print "============ Robot Groups: %s" %robot.get_group_names()

    print "============ Printing robot state"
    print robot.get_current_state()
    print "=============="

    group1_variable_values = group1.get_current_joint_values()

    if ((data.buttons[0]==1) and (data.axes[6]==1)):
    if (flag1 == 0):
            a += 1
        flag1 = 1
        j1 = (int(a))*0.1
            print j1
        if (j1 >= 3.1): 
        group1_variable_values[0] = 3.1
        print "Limit reached joint 1"
            else:
        print "changing joint 1"
        group1_variable_values[0] = j1
    plan2 = group1.plan()
        execute2= group1.execute(plan2)
        print "plan complete"

    else:
        flag1 = 0

    if ((data.buttons[0]==1) and (data.axes[6]==-1)):
    if (flag2 == 0):
        a -= 1
        flag2 = 1
        j1 = (int(a))*0.1
            print j1
        if (j1 <= -3.1):
        group1_variable_values[0] == -3.1 
        print "Limit reached joint 1"
            else:
        print "changing joint 1"
        group1_variable_values[0] = j1
    plan2 = group1.plan()
        execute2= group1.execute(plan2)
        print "plan complete"
    else:
        flag2 = 0

    group1.set_joint_value_target(group1_variable_values)

def move_group_python_interface():
    moveit_commander.roscpp_initialize(sys.argv)
    rospy.init_node('move_group_python_interface', anonymous=True)
    rospy.Subscriber("joy", Joy, callback)
    rospy.spin()


if __name__=='__main__':
    try:
           move_group_python_interface()
    except rospy.ROSInterruptException:
        pass

First, I launch my RViz launch file and then I have made a trajectory planner similar to as described in jog_arm file that launches my python code for controlling the robot. The launch file is as follows

<launch>

  <rosparam command="load" file="$(find igus_description)/config/joint_names_igus_1.yaml" />

  <node name="joy_node" pkg="joy" type="joy_node" />

  <node name="igus_server" pkg="igus_trajectory_planning" type="igus_server.py" output="screen" />

</launch>

and the jointnamesigus_1.yaml file is

igus_server:
  simulation: false # Whether the robot is started in simulation environment
  collision_check: true # Check collisions?
  command_in_topic:  igus_server/joy
  command_frame:  base_link  # TF frame that incoming cmds are given in
  incoming_command_timeout:  5  # Stop jogging if X seconds elapse without a new cmd
  joint_topic:  joint_states
  move_group_name:  igus1
  singularity_threshold:  5.5  # Slow down when the condition number hits this (close to singularity)
  hard_stop_singularity_threshold: 12. # Stop when the condition number hits this
  command_out_topic:  igus_controller/igus_joint_speed
  planning_frame:  base_link
  low_pass_filter_coeff:  1.  # Larger --> trust the filtered data more, trust the measurements less.
  publish_period:  0.008  # 1/Nominal publish rate [seconds]
  scale:
    linear:  0.0001  # Max linear velocity. Meters per publish_period. Units is [m/s]
    rotational:  0.0001  # Max angular velocity. Rads per publish_period. Units is [rad/s]
  # Publish boolean warnings to this topic
  warning_topic:  igus_server/warning

and the output displayed on the terminal is below and it keeps printing continuously even if I don't press the buttons on xbox, which causes minute changes in the 'position' values in every cycle.

[ INFO] [1527248279.789739931]: Ready to take commands for planning group igus1.
============ Waiting for RVIZ...
============ Reference frame: /world
============ End effector: ee_link
============ Robot Groups: ['igus1']
============ Printing robot state
joint_state: 
  header: 
    seq: 0
    stamp: 
      secs: 0
      nsecs:         0
    frame_id: "/world"
  name: [j1, j2, j3, j4, ee_joint]
  position: [-0.00030459940549917587, 0.0017067149010952564, -0.000912593125272543, -0.0006251060910522942, 0.0005091622353997083]
  velocity: []
  effort: []
multi_dof_joint_state: 
  header: 
    seq: 0
    stamp: 
      secs: 0
      nsecs:         0
    frame_id: "/world"
  joint_names: []![image description](https://ftp.osuosl.org/pub/ros/download.ros.org/downloads/se_migration/ros/15272501617263554.png)
  transforms: []
  twist: []
  wrench: []
attached_collision_objects: []
is_diff: False
==============

Because of this, under the status tab in RViz, 'Changed goal state' keeps publishing continuously (picture below) despite no input from the xbox.

C:\fakepath\Screenshot from 2018-05-25 14-09-01.png

Now, what I feel is that despite no input from the controller, messages are continuously being sent to the RViz and thus, when I give the input through the controller, the messages are stored in the buffer, thus causing the delay in execution.

Is there way by which I can either eliminate the buffer size or reduce it substantially so that the moment I give the input through the controller, the bot moves in RViz without much of a delay and also make sure that when the user is not giving any input through the xbox controller, no message is being sent to the bot?

I would be grateful for your help and advices. Thanks in advance.

Asked by Kashyap.m94 on 2018-05-25 07:18:39 UTC

Comments

Answers