1、示例程序:
最近在做6个han's cute机器人的协同控制,在这里记录一下roslaunch的配置技巧。
先上代码和分行解释:
<launch>
<arg name="cute_robot_1" default="true"/> <!--定义一个开关变量,用于是否启动对应机器人 -->
<arg name="cute_robot_2" default="true"/>
<arg name="cute_robot_3" default="true"/>
<arg name="cute_robot_4" default="true"/>
<arg name="cute_robot_5" default="true"/>
<arg name="cute_robot_6" default="true"/>
<group if="$(arg cute_robot_1)" ns="cute_robot_1"><!--如果cute_robot_1变量为真,则执行后续,在这里我们建立了一个group组,定义了句柄ns,这样在这个group内启动的所有节点topic都会被加上这个ns的前缀-->
<!-- There are 3 options for servo: dynamixel, xqtor_0, xqtor_1 -->
<!-- xqtor_0: the early version of the xQtor servo before 2017-->
<!-- xqtor_1: the new version of the xQtor servo -->
<arg name="servo" default="xqtor_1"/>
<param name="cute_servo_version" value="$(arg servo)"/>
<node name="dynamixel_manager" pkg="cute_servo_controllers" type="controller_manager.py" required="true" output="screen">
<rosparam>
namespace: dxl_manager
serial_ports:
pan_tilt_port:
port_name: "/dev/ttyUSB0" <!--注意修改接口名-->
baud_rate: 250000
min_motor_id: 0
max_motor_id: 25
update_rate: 50
</rosparam>
</node>
<include file="$(find cute_bringup)/launch/cute_spawner.launch"/>
<node pkg="rosservice" type="rosservice" name="cute_go_home" args="call --wait /cute_robot_1/cute_go_home true"/>
<node pkg="rosservice" type="rosservice" name="claw_controller" args="call --wait /cute_robot_1/claw_controller/torque_enable true"/>
</group>
<group if="$(arg cute_robot_2)" ns="cute_robot_2">
<!-- There are 3 options for servo: dynamixel, xqtor_0, xqtor_1 -->
<!-- xqtor_0: the early version of the xQtor servo before 2017-->
<!-- xqtor_1: the new version of the xQtor servo -->
<arg name="servo" default="xqtor_1"/>
<param name="cute_servo_version" value="$(arg servo)"/>
<node name="dynamixel_manager" pkg="cute_servo_controllers" type="controller_manager.py" required="true" output="screen">
<rosparam>
namespace: dxl_manager
serial_ports:
pan_tilt_port:
port_name: "/dev/ttyUSB1"
baud_rate: 250000
min_motor_id: 0
max_motor_id: 25
update_rate: 50
</rosparam>
</node>
<include file="$(find cute_bringup)/launch/cute_spawner.launch"/>
<node pkg="rosservice" type="rosservice" name="cute_go_home" args="call --wait /cute_robot_2/cute_go_home true"/>
<node pkg="rosservice" type="rosservice" name="claw_controller" args="call --wait /cute_robot_2/claw_controller/torque_enable true"/>
</group>
<group if="$(arg cute_robot_3)" ns="cute_robot_3">
<!-- There are 3 options for servo: dynamixel, xqtor_0, xqtor_1 -->
<!-- xqtor_0: the early version of the xQtor servo before 2017-->
<!-- xqtor_1: the new version of the xQtor servo -->
<arg name="servo" default="xqtor_1"/>
<param name="cute_servo_version" value="$(arg servo)"/>
<node name="dynamixel_manager" pkg="cute_servo_controllers" type="controller_manager.py" required="true" output="screen">
<rosparam>
namespace: dxl_manager
serial_ports:
pan_tilt_port:
port_name: "/dev/ttyUSB2"
baud_rate: 250000
min_motor_id: 0
max_motor_id: 25
update_rate: 50
</rosparam>
</node>
<include file="$(find cute_bringup)/launch/cute_spawner.launch"/>
<node pkg="rosservice" type="rosservice" name="cute_go_home" args="call --wait /cute_robot_3/cute_go_home true"/>
<node pkg="rosservice" type="rosservice" name="claw_controller" args="call --wait /cute_robot_3/claw_controller/torque_enable true"/>
</group>
<group if="$(arg cute_robot_4)" ns="cute_robot_4">
<!-- There are 3 options for servo: dynamixel, xqtor_0, xqtor_1 -->
<!-- xqtor_0: the early version of the xQtor servo before 2017-->
<!-- xqtor_1: the new version of the xQtor servo -->
<arg name="servo" default="xqtor_1"/>
<param name="cute_servo_version" value="$(arg servo)"/>
<node name="dynamixel_manager" pkg="cute_servo_controllers" type="controller_manager.py" required="true" output="screen">
<rosparam>
namespace: dxl_manager
serial_ports:
pan_tilt_port:
port_name: "/dev/ttyUSB3"
baud_rate: 250000
min_motor_id: 0
max_motor_id: 25
update_rate: 50
</rosparam>
</node>
<include file="$(find cute_bringup)/launch/cute_spawner.launch"/>
<node pkg="rosservice" type="rosservice" name="cute_go_home" args="call --wait /cute_robot_4/cute_go_home true"/>
<node pkg="rosservice" type="rosservice" name="claw_controller" args="call --wait /cute_robot_4/claw_controller/torque_enable true"/>
</group>
<group if="$(arg cute_robot_5)" ns="cute_robot_5">
<!-- There are 3 options for servo: dynamixel, xqtor_0, xqtor_1 -->
<!-- xqtor_0: the early version of the xQtor servo before 2017-->
<!-- xqtor_1: the new version of the xQtor servo -->
<arg name="servo" default="xqtor_1"/>
<param name="cute_servo_version" value="$(arg servo)"/>
<node name="dynamixel_manager" pkg="cute_servo_controllers" type="controller_manager.py" required="true" output="screen">
<rosparam>
namespace: dxl_manager
serial_ports:
pan_tilt_port:
port_name: "/dev/ttyUSB4"
baud_rate: 250000
min_motor_id: 0
max_motor_id: 25
update_rate: 50
</rosparam>
</node>
<include file="$(find cute_bringup)/launch/cute_spawner.launch"/>
<node pkg="rosservice" type="rosservice" name="cute_go_home" args="call --wait /cute_robot_5/cute_go_home true"/>
<node pkg="rosservice" type="rosservice" name="claw_controller" args="call --wait /cute_robot_5/claw_controller/torque_enable true"/>
</group>
<group if="$(arg cute_robot_6)" ns="cute_robot_6">
<!-- There are 3 options for servo: dynamixel, xqtor_0, xqtor_1 -->
<!-- xqtor_0: the early version of the xQtor servo before 2017-->
<!-- xqtor_1: the new version of the xQtor servo -->
<arg name="servo" default="xqtor_1"/>
<param name="cute_servo_version" value="$(arg servo)"/>
<node name="dynamixel_manager" pkg="cute_servo_controllers" type="controller_manager.py" required="true" output="screen">
<rosparam>
namespace: dxl_manager
serial_ports:
pan_tilt_port:
port_name: "/dev/ttyUSB5"
baud_rate: 250000
min_motor_id: 0
max_motor_id: 25
update_rate: 50
</rosparam>
</node>
<include file="$(find cute_bringup)/launch/cute_spawner.launch"/>
<node pkg="rosservice" type="rosservice" name="cute_go_home" args="call --wait /cute_robot_6/cute_go_home true"/>
<node pkg="rosservice" type="rosservice" name="claw_controller" args="call --wait /cute_robot_6/claw_controller/torque_enable true"/>
</group>
</launch>
2、注意事项
在写机器人内部的ros话题时需要注意,话题或者动作服务器等名应该写成相对的,比如:
cute_arm_controller/follow_joint_trajectory,这样在我们的group下才能在其前面加上自动加上前缀,变成:
cute_robot_1/cute_arm_controller/follow_joint_trajectory
如果是这种绝对的就不可以:
/cute_arm_controller/follow_joint_trajectory
所以说我们平时编写机器人的时候为了后续的开发性也应该注意这一点。
3、在一个launch中启动多个键盘遥控器(teleop_keyboard)时的表现:
<launch>
<arg name="cute_robot_1" default="true"/>
<arg name="cute_robot_2" default="false"/>
<arg name="cute_robot_3" default="false"/>
<arg name="cute_robot_4" default="false"/>
<arg name="cute_robot_5" default="false"/>
<arg name="cute_robot_6" default="false"/>
<group if="$(arg cute_robot_1)" ns="cute_robot_1">
<node pkg="cute_teleop" type="cute_teleop_keyboard" name="cute_teleop_keyboard" output="screen"/>
</group>
<group if="$(arg cute_robot_2)" ns="cute_robot_2">
<node pkg="cute_teleop" type="cute_teleop_keyboard" name="cute_teleop_keyboard" output="screen"/>
</group>
<group if="$(arg cute_robot_3)" ns="cute_robot_3">
<node pkg="cute_teleop" type="cute_teleop_keyboard" name="cute_teleop_keyboard" output="screen"/>
</group>
<group if="$(arg cute_robot_4)" ns="cute_robot_4">
<node pkg="cute_teleop" type="cute_teleop_keyboard" name="cute_teleop_keyboard" output="screen"/>
</group>
<group if="$(arg cute_robot_5)" ns="cute_robot_5">
<node pkg="cute_teleop" type="cute_teleop_keyboard" name="cute_teleop_keyboard" output="screen"/>
</group>
<group if="$(arg cute_robot_6)" ns="cute_robot_6">
<node pkg="cute_teleop" type="cute_teleop_keyboard" name="cute_teleop_keyboard" output="screen"/>
</group>
</launch>
我们启动了多个键盘控制在同一个窗口,然后发现这样竟然可以将键盘按键分发下去,同时控制6个机械臂进行动作,倒是省的我们进行映射了,算是个意外收获。
4、同时给多个端口开启权限
因为我们的机器人是usb端口连接上来的,需要在ubuntu给读写权限。
sudo chmod 777 /dev/ttyUSB0
但是这个权限在重启后会失效,而在需要控制多个机器人的时候需要每次都把6个端口权限打开,很麻烦,这里使用了c语言调用shell编程的技巧,通过写一个脚本来替我们完成,这样后来每次只需要执行一次这个脚本程序就行:
//shell.cpp
#include <stdlib.h>
#include <stdio.h>
#include <iostream>
#include <string>
using namespace std;
string get_output_of_cmd(const string &cmd)
{
int32_t count(2048);
char s[2048];
string ret;
FILE* stream = popen(cmd.c_str(), "r");
if (stream != NULL)
{
// 每次从stream中读取指定大小的内容
while (fgets(s, count, stream))
ret += s;
pclose(stream);
}
return ret;
}
bool get_exit_status_of_cmd(const string &cmd)
{
return (system(cmd.c_str()) == 0);
}
int main (int argc, char** argv)
{
int run = system("sudo chmod 777 /dev/ttyUSB0 /dev/ttyUSB1 /dev/ttyUSB2 /dev/ttyUSB3 /dev/ttyUSB4 /dev/ttyUSB5"); /* 执行命令*/
cout << run << endl;
string get_run = get_output_of_cmd("sudo chmod 777 /dev/ttyUSB0 /dev/ttyUSB1 /dev/ttyUSB2 /dev/ttyUSB3 /dev/ttyUSB4 /dev/ttyUSB5");
cout << get_run << endl;
return 0;
}
编译一下:
g++ shell.cpp -o shell.out
然后执行./shell.out就行。