Thanks Stu. I should have replied to Wim's email that I got something working using a custom ROS node and a hacked version of Cartesian Pose Controller.<br><br>I have a question though: Is there a significant speed difference between setting up several realtime state publishers as shown below (twist, 2x Pose stamped, JointState, Float array) compared to making a custom message type and sending it out on one publisher? The current publish rate is 10Hz but I am thinking of increasing that to 30Hz+.<br>
<br>Also... are the real-time publishers below doing anything different than regular publishers, given that they aren't actually working in part of the pr2_controller manager? It seems I didn't have to call ros::spinOnce() anywhere to get those to work, but I ended up having to call ros::spinOnce() every 100ms anyway to get the incoming pose commands to update.<br>
<br>Thanks,<br>Adam<br><br><br><br>state_error_publisher_.reset(new realtime_tools::RealtimePublisher<geometry_msgs::Twist>(node_, "state/error", 1));<br>  state_pose_publisher_.reset(new realtime_tools::RealtimePublisher<geometry_msgs::PoseStamped>(node_, "state/pose", 1));<br>
  state_pose_desi_publisher_.reset(new realtime_tools::RealtimePublisher<geometry_msgs::PoseStamped>(node_, "state/pose_desi", 1));<br>  joint_state_publisher_.reset(new realtime_tools::RealtimePublisher<sensor_msgs::JointState>(node_, "joint_states", 1));<br>
  effort_publisher_.reset(new realtime_tools::RealtimePublisher<std_msgs::Float32MultiArray>(node_, "state/effort", 1));<br><br><br clear="all">Adam Leeper<br>Stanford University<br><a href="mailto:aleeper@stanford.edu">aleeper@stanford.edu</a><br>
719.358.3804<br>
<br><br><div class="gmail_quote">On Tue, Sep 7, 2010 at 2:54 PM, Stuart Glaser <span dir="ltr"><<a href="mailto:sglaser@willowgarage.com">sglaser@willowgarage.com</a>></span> wrote:<br><blockquote class="gmail_quote" style="margin: 0pt 0pt 0pt 0.8ex; border-left: 1px solid rgb(204, 204, 204); padding-left: 1ex;">
Hi Adam,<br>
<br>
Neither the JTTeleopController nor the CartesianPoseController are<br>
necessarily PR2 specific, however, both run inside the PR2 controller<br>
manager, which is PR2 specific.  Controllers can only be run on a<br>
robot that supports the PR2 controller manager.<br>
<br>
Wim's email from Aug 31 describes a few ways to run the pr2 controller<br>
manager on your own robot.  Following his instructions is your best<br>
bet to getting any of the controllers running on your robot.<br>
<br>
-Stu<br>
<div><div></div><div class="h5"><br>
On Tue, Aug 31, 2010 at 6:29 PM, Adam Leeper <<a href="mailto:aleeper@stanford.edu">aleeper@stanford.edu</a>> wrote:<br>
> Thanks Wim.<br>
><br>
> If robot_mechanism_controllers/CartesianPoseController is generic, is there<br>
> a sample config or launch file for setting the parameters? Sachin sent one<br>
> out earlier for JTTeleop, but I guess that is pr2-specific.<br>
><br>
> Thanks.<br>
><br>
><br>
> Adam Leeper<br>
> Stanford University<br>
> <a href="mailto:aleeper@stanford.edu">aleeper@stanford.edu</a><br>
> 719.358.3804<br>
><br>
><br>
> On Tue, Aug 31, 2010 at 6:17 PM, Wim Meeussen <<a href="mailto:meeussen@willowgarage.com">meeussen@willowgarage.com</a>><br>
> wrote:<br>
>><br>
>> > When I launch and try to check the status of the controllers using<br>
>> > pr2_controller_manager list, it hangs and if I kill it the traceback<br>
>> > says it<br>
>> > was waiting for 'pr2_controller_manager/list_controllers.' When I run<br>
>> > the<br>
>> > pr2 in gazebo, that service is advertised by gazebo itself... so<br>
>> > obviously I<br>
>> > have a problem.<br>
>><br>
>> The pr2 controller manager is a library, not an executable. So when<br>
>> you run in simulation, Gazebo creates the controller manager, and when<br>
>> you run on the pr2, pr2_ethercat creates the controller manager.  When<br>
>> you launch "controller_manager.launch", this will only load the<br>
>> configuration for the controller manager on the parameter server, and<br>
>> run some supporting nodes (diagnostics, state publisher, etc), but<br>
>> this will not run the controller manager. Take a look at the<br>
>> pr2_etherCAT package for an example on how to start the controller<br>
>> manager.<br>
>><br>
>><br>
>> > What is the correct procedure for interfacing my own hardware with ros?<br>
>> > I<br>
>> > know at some point I need to hook all this up to actual joint data<br>
>> > coming in<br>
>> > off my hardware. Can someone point me to an example of how that is<br>
>> > published? Also, do the CartesianPoseController and JTTeleopController<br>
>> > work<br>
>> > with a generic urdf, or are they pr2-specific?<br>
>><br>
>> There are multiple ways to do this:<br>
>>  * You can create a ROS node for each piece of hardware, and allow<br>
>> other nodes in your system to interact with the hardware over<br>
>> topics/services/actions. This is a very easy approach. The<br>
>> disadvantage is that you won't be able to create a closed loop in hard<br>
>> realtime, and you won't be able to reuse the controllers we built for<br>
>> the PR2. But I would think that most people have used this approach<br>
>> when porting their robot to ROS.<br>
>><br>
>>  * You can implement the hardware interface (pr2_hardware_interface)<br>
>> for your hardware, and use the controller manager. The pr2 hardware<br>
>> interface is however PR2 specific, so this approach will only work for<br>
>> robots that are in some ways similar to the PR2. The hardware<br>
>> interface for example only supports effort controlled joints.  All the<br>
>> controllers we built, are using the "JointState" object of the<br>
>> hardware interface. So, take a look at the class, and see if you could<br>
>> implement this for your robot.<br>
>><br>
>> The controllers that are not PR2 specific live in stacks that don't<br>
>> have the pr2-prefix. The robot_mechanism_controllers package for<br>
>> example contains a set of non-pr2-specific controllers.<br>
>><br>
>><br>
>> Hope this helps,<br>
>><br>
>> Wim<br>
>><br>
>><br>
>><br>
>><br>
>> --<br>
>> --<br>
>> Wim Meeussen<br>
>> Willow Garage Inc.<br>
>> <<a href="http://www.willowgarage.com" target="_blank">http://www.willowgarage.com</a>)<br>
>> _______________________________________________<br>
>> ros-users mailing list<br>
>> <a href="mailto:ros-users@code.ros.org">ros-users@code.ros.org</a><br>
>> <a href="https://code.ros.org/mailman/listinfo/ros-users" target="_blank">https://code.ros.org/mailman/listinfo/ros-users</a><br>
><br>
><br>
> _______________________________________________<br>
> ros-users mailing list<br>
> <a href="mailto:ros-users@code.ros.org">ros-users@code.ros.org</a><br>
> <a href="https://code.ros.org/mailman/listinfo/ros-users" target="_blank">https://code.ros.org/mailman/listinfo/ros-users</a><br>
><br>
><br>
<br>
<br>
<br>
</div></div><font color="#888888">--<br>
Stuart Glaser<br>
sglaser -at- willowgarage -dot- com<br>
</font><div><div></div><div class="h5"><a href="http://www.willowgarage.com" target="_blank">www.willowgarage.com</a><br>
_______________________________________________<br>
ros-users mailing list<br>
<a href="mailto:ros-users@code.ros.org">ros-users@code.ros.org</a><br>
<a href="https://code.ros.org/mailman/listinfo/ros-users" target="_blank">https://code.ros.org/mailman/listinfo/ros-users</a><br>
</div></div></blockquote></div><br>