kump's profile - activity

I'm trying to make a service that I could call to change a pose of a robotic arm using MoveIt!. I follow this tutorial, but I get an error:

Traceback (most recent call last):
  File "/home/linux/catkin_ws/src/robotic_arm_algorithms/scripts/set_joint_states_client.py", line 24, in <module>
    set_joint_states(joint_states)
  File "/home/linux/catkin_ws/src/robotic_arm_algorithms/scripts/set_joint_states_client.py", line 17, in set_joint_states
    resp = set_joint_states(msg)
  File "/opt/ros/kinetic/lib/python2.7/dist-packages/rospy/impl/tcpros_service.py", line 435, in __call__
    return self.call(*args, **kwds)
  File "/opt/ros/kinetic/lib/python2.7/dist-packages/rospy/impl/tcpros_service.py", line 495, in call
    service_uri = self._get_service_uri(request)
  File "/opt/ros/kinetic/lib/python2.7/dist-packages/rospy/impl/tcpros_service.py", line 444, in _get_service_uri
    raise TypeError("request object is not a valid request message instance")
TypeError: request object is not a valid request message instance

This seems as a problem purely in the client code, so I show the client code and the service definition bellow. I have defined the servise as 4 float numbers each with a specific name. So when I pass the client four numbers (via terminal arguments), I expect to be able to simply assign those values to the service message as shown below. But something is not right. Can you point out the mistake for me?

srv/SetJointStates.srv:

std_msgs/Float32 forearm_0
std_msgs/Float32 forearm_1
std_msgs/Float32 arm_0
std_msgs/Float32 arm_1
---

script/set_joint_states_client.py:

#!/usr/bin/env python

import sys
import rospy
from robotic_arm_algorithms.srv import SetJointStates

def set_joint_states(joint_states):
    rospy.wait_for_service('set_joint_states')
    try:
        set_joint_states = rospy.ServiceProxy('set_joint_states', SetJointStates)
        msg = SetJointStates()
        msg.forearm_0 = joint_states[0]
        msg.forearm_1 = joint_states[1]
        msg.arm_0 = joint_states[2]
        msg.arm_1 = joint_states[3]

        resp = set_joint_states(msg)
    except rospy.ServiceException, e:
        print "Service call failed: %s"%e

if __name__ == "__main__":
    if len(sys.argv) == 5:
        joint_states = [float(sys.argv[1]), float(sys.argv[2]), float(sys.argv[3]), float(sys.argv[4])]
        set_joint_states(joint_states)
    else:
        print("not enaugh arguments. Four arguments required: forearm 0, forearm 1, arm 0, arm 1")
        sys.exit(1)

EDIT 1

When I put print (dir(SetJointStates)) into set_joint_states method of the client source code, following strings prints on terminal:

['__class__', '__delattr__', '__dict__', '__doc__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_md5sum', '_request_class', '_response_class', '_type']

I'm running a script where I use the rospy.wait_for_message in a while loop to wait for a message of a topic, that is publishing at certain event.

On the first event the while loop runs as expected, sometimes even on the second. I can see by running rostopic echo /event_topic that the messages are being published after the code stopped inside the while loop at the instruction rospy.wait_for_message("/event_topic", Bool, timeout=None), but the instruction is never executed. The program just stays at this line, waiting for messages, that are being published.

When I reset the script the first one or two events are again processed as expected and that the process halts. I cannot find any errors nor warnings printed in the terminal.

This is my script:

import rospy
import rospkg
from std_msgs.msg import Bool
from std_msgs.msg import Float64
from std_msgs.msg import Float32MultiArray

def mynode():
    rospy.init_node('mynode')
    publisher = rospy.Publisher('/command', Float64, queue_size=1)

    time.sleep(.5)

    while not rospy.is_shutdown():
        rospy.wait_for_message("/event_topic", Bool, timeout=None)

        data = rospy.wait_for_message("/data_topic", Float32MultiArray, timeout=None)
        msg = data.data[0]

        publisher.publish(msg)
        time.sleep(.2) 
        rospy.spin()

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

It also seems, that when I subscribe to the /event_topic by running rostopic echo in the terminal, the next event is being processed by the rospy.wait_for_message, but following, again, are not.

EDIT

The problem is somehow connected to the multimaster-FKIE package. I run two simulations on two ROS masters and use the multimaster-FKIE package to allow communication between them. The node shown above is supposed to read messages from topics started in one ROS master and publish messages into the topics started in the other ROS master.

When I run the node on the ROS core where the /event_topic was started, it works as expected. If I run the node on the ROS master where the /data_topic was started, the problem occurs. But I don't understand why. The topics are supposed to be shared among the ROS masters.

In the MoveIt! Setup Assistant 2.0, there is a tag "End Effectors", which content translates to the <end_effector> tags of the robot.srdf file.

What is this good for and how was it meant to be used? Given you have one simple robotic manipulator similar to the one on the picture bellow.

Say I want the possition of the link marked as 'end effector' to be controlled by the MoveIt! planner. So I tell the MoveIt! "Move the manipulator in such a way, that this link ends up at coordinates [0.2, 0.1, 1.0]." and it plans the trajectories of the whole arm to satisfy this instruction. How should I set the end effector element and planning groups?

I am using this method in my python script

rospy.wait_for_message("/my_topic", Bool, timeout=None)

The /my_topic messages are being published, I can see them when running rostopic echo /my_topic. But the script stays at the rospy.wait_for_message line as if it couldn't detect any messages. What could be the cause?

EDIT

$ rostopic echo -n1 /my_topic 
data: True
---

I have a question about use of the MoveIt! planning groups. Say I have a robotic manipulator consisting of two arms (as on the picture below), that can rotate around its axis and around an axis perpendicular to its axis.

When I define two planning groups, one for the bottom arm and one for the upper arm, in the Rviz I have to move those groups separately. And as I was quickly looking through a Python interface tutorial for MoveIt!, it seems that you would need to set the pose of each planning group individually. Now that doesn't look like something you want to do. Ideally you want to move the whole robot at once, right? But if I define the whole robot as one planning group, what is the purpose of the planning groups in the first place?

Can somebody shed some light on this matter?

Thank you.

I'm trying to optimize my code so I was measuring how long different parts of ROS process take. Specifically

rospy.wait_for_service(rospy.get_param('~service_name'))

always seems to take little over 2 ms

service = rospy.ServiceProxy(rospy.get_param('~service_name'), MySrv)

always seems to take little under 1 ms

resp = service(MySrvRequest())

always seems to take little over 1 ms

This all causes the one service call to result in about 4 ms of delay and I want to get my program to 1 ms precision. What are the variables that effect how long those service calls take? Can I adjust the setting somewhere to make it faster?

Ok, the reason the this->rosNode->getParam("/parameter_from_node", parameter_from_node) returned false was because

I want to load parameters from the ROS param server in the Gazebo model plugin. I do not have any problem with getting the parameters thet were set in the launch files, but I run a node that uploads a list of parameters to the rosparam server later in the run. I can see and read the parameters using terminal, but the gazebo plugin does not see the new parameters (I check for them inside the plugin on every update).

Can you provide any insight into why is this happening?

EDIT

I am accessing the parameters from gazebo plugin using

std::string param_name;
this->rosNode->getParam("/param_name", param_name);

EDIT 2

code snippet

namespace gazebo {
  // Constructor
  MyPlugin::MyPlugin() {}

  // Default plugin init call.
  void MyPlugin::Init() {}

  void MyPlugin::Load(physics::ModelPtr _model,
                                  sdf::ElementPtr _sdf) {

    this->updateConnection = event::Events::ConnectWorldUpdateBegin(
            boost::bind(&MyPlugin::OnUpdate, this));

    // Initialize ros, if it has not already been initialized.
    std::string node_name = std::string("my_node");
    if (!ros::isInitialized()) {
      int argc = 0;
      char **argv = nullptr;

      ros::init(argc, argv, node_name,
                ros::init_options::NoSigintHandler);
    }

    // Create ROS node.
    this->rosNode.reset(new ros::NodeHandle(node_name));

    ros::AsyncSpinner spinner(2); // Use 2 threads
    spinner.start(); // spin() will not return until the node has been shutdown
  }


  void MyPlugin::loadParameters()
  {
    this->parameters_loaded = true;
    std::string parameter_from_launch, parameter_from_node;
    if (! this->rosNode->getParam("/parameter_from_launch", parameter_from_launch))
    {
      gzmsg << "ROS parameter '/parameter_from_launch' was not found \n";
      this->parameters_loaded = false;
    }
    gzmsg << "Parameter from launch: " << parameter_from_launch << "\n";
    if (! this->rosNode->getParam("/parameter_from_node", parameter_from_node))
    {
      gzmsg << "ROS parameter /parameter_from_node was not found. \n";
      this->parameters_loaded = false;
    }
    gzmsg << "Parameter from node: " << parameter_from_node << "\n";

  }


  void MyPlugin::OnUpdate() {
    if(!this->parameters_loaded){
      MyPlugin::loadParameters();
    }
  }
}

the parameter_from_launch is found on the first try and prints the correct value. The "ROS parameter /parameter_from_node was not found. \n" keeps being printed and the gzmsg << "Parameter from node: " << parameter_from_node << "\n" returns an empty string

Ok, the reason the this->rosNode->getParam("/parameter_from_node", parameter_from_node) returned false was because

@Tahir M. @Delb I eddited the question to address your remarks.

C++ plugin cannot get parameter that has been uploaded to param server later I want to load parameters from the ROS para

C++ plugin cannot get parameter that has been uploaded to param server later I want to load parameters from the ROS para

@Delb, If I understand correctly, then that is what I am doing. On each update I call rosNode->getParam('param_name

@Delb, If I understand correctly, then that is what I am doing. On each update I call rosNode->getParam('param_name

@Delb, If I understand correctly, then that is what I am doing. On each update I call rosNode->getParam('param_name