I'm trying to publish to the topic /joy/set_feedback.
I had this in my include:
#include <sensor_msgs/JoyFeedbackArray.h>
I had my Nodehandle and Publisher like this :
feed_pub = nh->advertise<sensor_msgs::JoyFeedbackArray>("/joy/set_feedback", 1);
Now my problem is that i want to fill up the feed_msg
sensor_msgs::JoyFeedbackArray feed_msg;
In the documentation sensor_msgs/JoyFeedbackArray Message
it says it's an array. No matter what i did wrote there, I always get an error.
It would be very helpful if i could get a correct example to fill up this array.
I'll be needing a vibration signal with type: 1 / id: 0 / intensity: 1.0.
ROS messages can handle something like Arrays. But when you are working with a ROS Message-"Array" in Python or C++, you need to use a different data type. In your case, you need a std::Vector<sensor_msgs::JoyFeedback>.
For other cases, the ROS Wiki has created a table of translations between ROS Message, C++ and Python:
You can find a detailed explaination of the different datatypes and their conversion between ROS Message and Python or C++ here: http://wiki.ros.org/msg
Related
I have an issue working with DiagramBuilder and ManipulationStation classes.
It appears to me, that c++ API and the python bindings work differently in my case.
C++ API behaves as expected, while the python bindings result in the runtime error:
DiagramBuilder: Cannot operate on ports of System plant until it has been registered using AddSystem
How I use C++ API
In one of the ManipulationStation::Setup...() methods I inject a block of code, that adds an extra manipuland
const std::string sdf_path = FindResourceOrThrow("drake/examples/manipulation_station/models/bolt_n_nut.sdf");
RigidTransform<double> X_WC(RotationMatrix<double>::Identity(), Vector3d(0.0, -0.3, 0.1));
bolt_n_nut_ = internal::AddAndWeldModelFrom(sdf_path, "nut_and_bolt", lant_->world_frame(), "bolt", X_WC, plant_);
I inject another block of code into the method ManipulationStation::Finalize:
auto zero_torque = builder.template AddSystem<systems::ConstantVectorSource<double>>(Eigen::VectorXd::Zero(plant_->num_velocities(bolt_n_nut_)));
builder.Connect(zero_torque->get_output_port(), plant_->get_actuation_input_port(bolt_n_nut_));
With these changes, the simulation runs as expected.
How I use python bindings
plant = station.get_multibody_plant()
manipuland_path = get_manipuland_resource_path()
bolt_with_nut = Parser(plant=plant).AddModelFromFile(manipuland_path)
X_WC = RigidTransform(RotationMatrix.Identity(), [0.0, -0.3, 0.1])
plant.WeldFrames(plant.world_frame(), plant.GetFrameByName('bolt', bolt_with_nut), X_WC)
...
station.Finalize()
zero_torque = builder.AddSystem(ConstantValueSource(AbstractValue.Make([0.])))
builder.Connect(zero_torque.get_output_port(), plant.get_actuation_input_port(bolt_with_nut_model))
This triggers a RuntimeError with a message as above; The port, which causes this error is nut_and_bolt_actuation.
My vague understanding of the problem is the (in) visibility of nut_and_bolt System, due to having two distinct DiagramBuilders in a process: 1) a one is inside ManipulationStation 2) another is in the python code, that instantiates this ManipulationStation object.
Using ManipulationStation via python bindings is a preference for me, because that way I would've avoided depending on a custom build of drake library.
Thanks for your insight!
I agree with your assessment: you have two different DiagramBuilder objects here. This does not have anything to due with C++ or Python; the ManipulationStation is itself a Diagram (created using its own DiagramBuilder), and you have a second DiagramBuilder (in either c++ or python) that is connecting the ManipulationStation together with other elements. You are trying to connect a system that is in the external diagram to a port that is in the internal diagram, but is not exposed.
The solution would be to have the ManipulationStation diagram expose the extra nut and bolt actuation port so that you can connect to it from the second builder.
If you prefer Python, I've switched my course to using a completely python version of the manipulation station. I find this version is much easier to adapt to different student projects. (To be clear, the setup is in python, but at simulation time all of the elements are c++ and it doesn't call back to python; so the performance is almost identical.)
I would like to visualize pointcloud in drake-visualizer using python binding.
I imitated how to publish images through lcm from here, and checked out these two issues (14985, 14991). The snippet is as follows :
point_cloud_to_lcm_point_cloud = builder.AddSystem(PointCloudToLcm())
point_cloud_to_lcm_point_cloud.set_name('pointcloud_converter')
builder.Connect(
station.GetOutputPort('camera0_point_cloud'),
point_cloud_to_lcm_point_cloud.get_input_port()
)
point_cloud_lcm_publisher = builder.AddSystem(
LcmPublisherSystem.Make(
channel="DRAKE_POINT_CLOUD_camera0",
lcm_type=lcmt_point_cloud,
lcm=None,
publish_period=0.2,
# use_cpp_serializer=True
)
)
point_cloud_lcm_publisher.set_name('point_cloud_publisher')
builder.Connect(
point_cloud_to_lcm_point_cloud.get_output_port(),
point_cloud_lcm_publisher.get_input_port()
)
However, I got the following runtime error:
RuntimeError: DiagramBuilder::Connect: Mismatched value types while connecting output port lcmt_point_cloud of System pointcloud_converter (type drake::lcmt_point_cloud) to input port lcm_message of System point_cloud_publisher (type drake::pydrake::Object)
When I set 'use_cpp_serializer=True', the error becomes
LcmPublisherSystem.Make(
File "/opt/drake/lib/python3.8/site-packages/pydrake/systems/_lcm_extra.py", line 71, in _make_lcm_publisher
serializer = _Serializer_[lcm_type]()
File "/opt/drake/lib/python3.8/site-packages/pydrake/common/cpp_template.py", line 90, in __getitem__
return self.get_instantiation(param)[0]
File "/opt/drake/lib/python3.8/site-packages/pydrake/common/cpp_template.py", line 159, in get_instantiation
raise RuntimeError("Invalid instantiation: {}".format(
RuntimeError: Invalid instantiation: _Serializer_[lcmt_point_cloud]
I saw the cpp example here, so maybe this issue is specific to python binding.
I also saw this python example, but thought using 'PointCloudToLcm' might be more convenient.
P.S.
I am aware of the development in recent commits on MeshcatVisualizerCpp and MeshcatPointCloudVisualizerCpp, but I am still on the drake-dev stable build 0.35.0-1 and want to stay on drake visualizer until the meshcat c++ is more mature.
The old version in pydrake.systems.meshcat_visualizer.MeshcatVisualizer is a bit too slow on my current use-case (multiple objects drop). I can visualize the pointcloud with this visualization setting, but it took too much machine resources.
Only the message types that are specifically bound in lcm_py_bind_cpp_serializers.cc can be used on an LCM message input/output port connection between C++ and Python. For all other LCM message types, the input/output port connection must be from a Python system to a Python system or a C++ System to a C++ System.
The lcmt_image_array is listed there, but not the lcmt_point_cloud.
If you're stuck using Drake's v0.35.0 capabilities, then I don't see any great solutions. Some options:
(1) Write your own PointCloudToLcm system in Python (by re-working the C++ code into Python, possibly with a narrower set of supported features / channels for simplicity).
(2) Write your own small C++ helper function MakePointCloudPublisherSystem(...) that calls LcmPublisherSystem::Make<lcmt_point_cloud> function in C++, and bind it into Python. Then your Python code can call MakePointCloudPublisherSystem() and successfully connect that to the existing C++ PointCloudToLcm.
I have a ROS node that allows you to "publish" a data structure to it, to which it responds by publishing an output. The timestamp of what I published and what it publishes is matched.
Is there a mechanism for a blocking function where I send/publish and output, and it waits until I receive an output?
I think you need the ROS_Services (client/server) pattern instead of the publisher/subscriber.
Here is a simple example to do that in Python:
Client code snippet:
import rospy
from test_service.srv import MySrvFile
rospy.wait_for_service('a_topic')
try:
send_hi = rospy.ServiceProxy('a_topic', MySrvFile)
print('Client: Hi, do you hear me?')
resp = send_hi('Hi, do you hear me?')
print("Server: {}".format(resp.response))
except rospy.ServiceException, e:
print("Service call failed: %s"%e)
Server code snippet:
import rospy
from test_service.srv import MySrvFile, MySrvFileResponse
def callback_function(req):
print(req)
return MySrvFileResponse('Hello client, your message received.')
rospy.init_node('server')
rospy.Service('a_topic', MySrvFile, callback_function)
rospy.spin()
MySrvFile.srv
string request
---
string response
Server out:
request: "Hi, do you hear me?"
Client out:
Client: Hi, do you hear me?
Server: Hello client, your message received.
Learn more in ros-wiki
Project repo on GitHub.
[UPDATE]
If you are looking for fast communication, TCP-ROS communication is not your purpose because it is slower than a broker-less communicator like ZeroMQ (it has low latency and high throughput):
ROS-Service pattern equivalent in ZeroMQ is REQ/REP (client/server)
ROS publisher/subscriber pattern equivalent in ZeroMQ is PUB/SUB
ROS publisher/subscriber with waitformessage equivalent in ZeroMQ is PUSH/PULL
ZeroMQ is available in both Python and C++
Also, to transfer huge amounts of data (e.g. pointcloud), there is a mechanism in ROS called nodelet which is supported only in C++. This communication is based on shared memory on a machine instead of TCP-ROS socket.
What exactly is a nodelet?
Since you want to stick with publish/ subscribers, assuming from your comment, that services are to slow I would have a look at waitForMessage (Documentation).
And for an example on how to use it you can have a look at this ros answers question.
All you need to do is to publish your data and immediately call waitForMessage on the output topic and manually pass the received message to your "callback".
I hope this is what you were looking for.
To get this request/reply behaviour ROS has a mechanism called ROS service.
You can specify the input and output of your service in a service file similar to a ROS message definition. You can then call the service of a node with your input and the call will receive an output when the service is finished.
Here is a tutorial how to use this mechanism in python. If you prefer C++ there is also one, you should find it.
I installed ROS melodic version in Ubuntu 18.04.
I'm running a rosbag in the background to mock cameras in messages rostopics.
I set the camera names in rosparams and iterated through it to capture each camera topics.
I'm using message_filter ApproximateTimeSynchronizer to get time synchronized data as mentioned in the official documentation,
http://wiki.ros.org/message_filters
But most of the time the callback function to ApproximateTimeSynchronizer is not being called/is having delay. The code snippet I'm using is given below:
What am I doing wrong here?
def camera_callback(*args):
pass # Other logic comes here
rospy.init_node('my_listener', anonymous=True)
camera_object_data = []
for camera_name in rospy.get_param('/my/cameras'):
camera_object_data.append(message_filters.Subscriber(
'/{}/hd/camera_info'.format(camera_name), CameraInfo))
camera_object_data.append(message_filters.Subscriber(
'/{}/hd/image_color_rect'.format(camera_name), Image))
camera_object_data.append(message_filters.Subscriber(
'/{}/qhd/image_depth_rect'.format(camera_name), Image))
camera_object_data.append(message_filters.Subscriber(
'/{}/qhd/points'.format(camera_name), PointCloud2)
topic_list = [filter_obj for filter_obj in camera_object_data]
ts = message_filters.ApproximateTimeSynchronizer(topic_list, 10, 1, allow_headerless=True)
ts.registerCallback(camera_callback)
rospy.spin()
Looking at your code, it seems correct. There is, however, a trouble with perhaps bad timestamps and ergo this synchronizer as well, see http://wiki.ros.org/message_filters/ApproximateTime for algorithm assumptions.
My recommendation is to write a corresponding node that publishes empty versions of these four msgs all at the same time. If it's still not working in this perfect scenario, there is an issue with the code above. If it is working just fine, then you need to pay attention to the headers.
Given that you have it as a bag file, you can step through the msgs on the command line and observe the timestamps as well. (Can also step within python).
$ rosbag play --pause recorded1.bag # step through msgs by pressing 's'
On time-noisy msgs with small payloads, I've just written a node to listen to all these msgs, and republish them all with the latest time found on any of them (for sync'd logging to csv). Not optimal, but it should reveal where the issue lies.
Hi I am using LabView 2012, Delphi XE7 and GPIB (I think 488.2), Win7 SP1 and Agilent 53131A.
I used the given NI examples.
NI Labview example - Found in LabVIEW's help - GPIB.vi.
I tried writing and reading to query frequencies from 2 channels and they are successful.
They are are sent and read in succession.
*IDN?
:FUNC 'FREQ 1'
:READ:FREQ?
If they are successful, that meant GPIB for Agilent and NI MAX and driver are successfully installed and configured.
I am also able to use KeySight Connection Expert's to write and read, Again it is also successful.
However, When I used the given NI example in Delphi. Orginally it was saved as Delphi 3 or 4.
I used the Scope Simple example for universal counter. I used it mostly for writing and reading in the simple way. All it needs initialization, read/write and cleanup
I changed the following codes as shown below, in SimpleForm.pas
The detected device is at GPIB0::3::INSTR so, at line 32,
PRIMARY_ADDR_OF_COUNTER = 3;
String to write and read so, at line 132,
CommandBox.Text := '*IDN?';
then it was compiled with no error and run.
String to write was successfully
But upon reading, it was not successfully.
The string output is supposed to be ' HEWLETT-PACKARD,53131A,0,4806'.
The error at the end of the program is as follows below:-
Unable to read from device
ibsta = SC000 <ERR TMO>
iberr = 6 <EABO>
ibcntl = 0
From these readings, I figured out as :-
EABO means abort
I am not familiar with working of GPIB. Kindly advise.
You are correct that EABO is the identifier for an abort. In addition, we can see from ibsta = SC000 <ERR TMO> that the cause of the abort was a GPIB timeout error. I am not familiar with Keysight Connection Expert or your instrument, but since the error was from GPIB timeout, the most likely causes are:
The query was improperly formatted and the instrument thought it was just a write statement with no response needed. (That's probably why the write function had no error, but the read function timed out.)
The query was improperly formatted and the instrument returned an error.
Instrument needs to have 'Talker' capability enabled to send data. (Most instruments do this automatically with queries.)
For more information on generic GPIB commands, see this reference from the folks at National Instruments.