前提
在上一篇文章中已经编译好了ROS2的Android版本,并成功实现了一个发布订阅的测试程序。现在可以开始使用ROS2的Android版本开发APP来控制OriginBot的运动了。
摇杆控件
这里控件首选当然是摇杆,不过像这种控件一般都不是现成的,属于自定义控件。当然肯定会有Android开发人员写好的,为了节约时间就不搞重复开发了,直接在网上找到一个:https://github.com/simpafac/virtual-joystick-android
控件就长下面这样:
只需要将JoystickView.java添加到自己的项目中调用就可以了。首先在layout目录下的xml文件中加入控件:
<com.example.originbot.JoystickView
android:id="@+id/joystickView_right"
android:layout_width="200dp"
android:layout_height="match_parent"
android:layout_alignParentLeft="true"
android:layout_alignParentBottom="true"
android:layout_weight="0"
app:JV_backgroundColor="#009688"
app:JV_borderColor="#00796B"
app:JV_borderWidth="8dp"
app:JV_buttonColor="#FF6E40"
app:layout_constraintBottom_toBottomOf="parent"
app:layout_constraintLeft_toLeftOf="parent" />
<com.example.originbot.JoystickView android:id="@+id/joystickView_right" android:layout_width="200dp" android:layout_height="match_parent" android:layout_alignParentLeft="true" android:layout_alignParentBottom="true" android:layout_weight="0" app:JV_backgroundColor="#009688" app:JV_borderColor="#00796B" app:JV_borderWidth="8dp" app:JV_buttonColor="#FF6E40" app:layout_constraintBottom_toBottomOf="parent" app:layout_constraintLeft_toLeftOf="parent" />
<com.example.originbot.JoystickView android:id="@+id/joystickView_right" android:layout_width="200dp" android:layout_height="match_parent" android:layout_alignParentLeft="true" android:layout_alignParentBottom="true" android:layout_weight="0" app:JV_backgroundColor="#009688" app:JV_borderColor="#00796B" app:JV_borderWidth="8dp" app:JV_buttonColor="#FF6E40" app:layout_constraintBottom_toBottomOf="parent" app:layout_constraintLeft_toLeftOf="parent" />
<com.example.originbot.JoystickView android:id="@+id/joystickView_right" android:layout_width="200dp" android:layout_height="match_parent" android:layout_alignParentLeft="true" android:layout_alignParentBottom="true" android:layout_weight="0" app:JV_backgroundColor="#009688" app:JV_borderColor="#00796B" app:JV_borderWidth="8dp" app:JV_buttonColor="#FF6E40" app:layout_constraintBottom_toBottomOf="parent" app:layout_constraintLeft_toLeftOf="parent" />
复制
在上面的代码中还可以调整摇杆控件的属性,比如颜色、大小、排列等等。其调用方式也和Android上的控件调用方式一致,比如在MainAcitivity.java中调用:
private JoystickView joystickRight;
...
joystickRight = findViewById(R.id.joystickView_right);
private JoystickView joystickRight; ... joystickRight = findViewById(R.id.joystickView_right);
private JoystickView joystickRight; ... joystickRight = findViewById(R.id.joystickView_right);
private JoystickView joystickRight; ... joystickRight = findViewById(R.id.joystickView_right);
复制
消息与控制
ROS2中用于控制的消息依然是geometry_msgs/msg/Twist,包含两个子消息:Vector3类型的linear和angular,分别表示线速度和角速度。OriginBot属于差速小车,只需要控制X轴的线速度和Z轴的角速度即可。
同时摇杆拥有两个控制变量,分别是推动角度和推动力度,我们假设小车的最大线速度为max_v,最大角速度为max_a,摇杆推动到某点时的角度为angle,力度为strength;则当前小车的线速度和角速度分别为:
double now_v = max_v * sin(angle*PI/180) * strength/100;
double now_a = max_a * cos(angle*PI/180) * strength/100;
double now_v = max_v * sin(angle*PI/180) * strength/100; double now_a = max_a * cos(angle*PI/180) * strength/100;
double now_v = max_v * sin(angle*PI/180) * strength/100; double now_a = max_a * cos(angle*PI/180) * strength/100;
double now_v = max_v * sin(angle*PI/180) * strength/100; double now_a = max_a * cos(angle*PI/180) * strength/100;
复制
这里strength除了个100是因为它做了一个小数点后两位乘100取整数的操作,目的是为了方便直观的调整参数。
将此关系式写入摇杆推动的触发函数里,然后传递给发布者,并用两个TextView显示当前的线速度和角速度:
@SuppressLint("DefaultLocale")
private void joyControl(){
mTextViewAngleRight = findViewById(R.id.textView2);
mTextViewStrengthRight = findViewById(R.id.textView3);
joystickRight = findViewById(R.id.joystickView_right);
joystickRight.setOnMoveListener((angle, strength) -> {
double now_v = max_v * sin(angle*PI/180) * strength/100; //当前摇杆所推力度对应速度值
double now_a = max_a * cos(angle*PI/180) * strength/100;
Vector3 vec_lv = new Vector3();
Vector3 vec_av = new Vector3();
vec_lv.setX(-now_v);
vec_av.setZ(-now_a);
control_node.pubVelocity(vec_lv, vec_av);
mTextViewAngleRight.setText(String.format("当前线速度:%1$.2f m/s ", now_v));
mTextViewStrengthRight.setText(String.format("当前角速度:%1$.2f rad/s ", now_a));
});
}
@SuppressLint("DefaultLocale") private void joyControl(){ mTextViewAngleRight = findViewById(R.id.textView2); mTextViewStrengthRight = findViewById(R.id.textView3); joystickRight = findViewById(R.id.joystickView_right); joystickRight.setOnMoveListener((angle, strength) -> { double now_v = max_v * sin(angle*PI/180) * strength/100; //当前摇杆所推力度对应速度值 double now_a = max_a * cos(angle*PI/180) * strength/100; Vector3 vec_lv = new Vector3(); Vector3 vec_av = new Vector3(); vec_lv.setX(-now_v); vec_av.setZ(-now_a); control_node.pubVelocity(vec_lv, vec_av); mTextViewAngleRight.setText(String.format("当前线速度:%1$.2f m/s ", now_v)); mTextViewStrengthRight.setText(String.format("当前角速度:%1$.2f rad/s ", now_a)); }); }
@SuppressLint("DefaultLocale") private void joyControl(){ mTextViewAngleRight = findViewById(R.id.textView2); mTextViewStrengthRight = findViewById(R.id.textView3); joystickRight = findViewById(R.id.joystickView_right); joystickRight.setOnMoveListener((angle, strength) -> { double now_v = max_v * sin(angle*PI/180) * strength/100; //当前摇杆所推力度对应速度值 double now_a = max_a * cos(angle*PI/180) * strength/100; Vector3 vec_lv = new Vector3(); Vector3 vec_av = new Vector3(); vec_lv.setX(-now_v); vec_av.setZ(-now_a); control_node.pubVelocity(vec_lv, vec_av); mTextViewAngleRight.setText(String.format("当前线速度:%1$.2f m/s ", now_v)); mTextViewStrengthRight.setText(String.format("当前角速度:%1$.2f rad/s ", now_a)); }); }
@SuppressLint("DefaultLocale") private void joyControl(){ mTextViewAngleRight = findViewById(R.id.textView2); mTextViewStrengthRight = findViewById(R.id.textView3); joystickRight = findViewById(R.id.joystickView_right); joystickRight.setOnMoveListener((angle, strength) -> { double now_v = max_v * sin(angle*PI/180) * strength/100; //当前摇杆所推力度对应速度值 double now_a = max_a * cos(angle*PI/180) * strength/100; Vector3 vec_lv = new Vector3(); Vector3 vec_av = new Vector3(); vec_lv.setX(-now_v); vec_av.setZ(-now_a); control_node.pubVelocity(vec_lv, vec_av); mTextViewAngleRight.setText(String.format("当前线速度:%1$.2f m/s ", now_v)); mTextViewStrengthRight.setText(String.format("当前角速度:%1$.2f rad/s ", now_a)); }); }
复制
由于在OriginBot小车上以摄像头的安装位置为前方,所以当前的线速度和角速度需要做一个方向上的调整;通过设定正负值即可调整方向。
这里我们单独写一个控制节点来发布Twist消息:
package com.example.originbot;
import org.ros2.rcljava.node.BaseComposableNode;
import org.ros2.rcljava.publisher.Publisher;
import geometry_msgs.msg.Twist;
import geometry_msgs.msg.Vector3;
public class ControlNode extends BaseComposableNode {
private static String logtag = ControlNode.class.getName();
private final String topic;
public Publisher<geometry_msgs.msg.Twist> twist_pub;
public ControlNode(final String name, final String topic) {
super(name);
this.topic = topic;
this.twist_pub = this.node.<Twist>createPublisher(
geometry_msgs.msg.Twist.class, this.topic);
}
public void pubVelocity(Vector3 vec_lv, Vector3 vec_av){
Twist m_twist = new Twist();
m_twist.setLinear(vec_lv);
m_twist.setAngular(vec_av);
this.twist_pub.publish(m_twist);
}
}
package com.example.originbot; import org.ros2.rcljava.node.BaseComposableNode; import org.ros2.rcljava.publisher.Publisher; import geometry_msgs.msg.Twist; import geometry_msgs.msg.Vector3; public class ControlNode extends BaseComposableNode { private static String logtag = ControlNode.class.getName(); private final String topic; public Publisher<geometry_msgs.msg.Twist> twist_pub; public ControlNode(final String name, final String topic) { super(name); this.topic = topic; this.twist_pub = this.node.<Twist>createPublisher( geometry_msgs.msg.Twist.class, this.topic); } public void pubVelocity(Vector3 vec_lv, Vector3 vec_av){ Twist m_twist = new Twist(); m_twist.setLinear(vec_lv); m_twist.setAngular(vec_av); this.twist_pub.publish(m_twist); } }
package com.example.originbot; import org.ros2.rcljava.node.BaseComposableNode; import org.ros2.rcljava.publisher.Publisher; import geometry_msgs.msg.Twist; import geometry_msgs.msg.Vector3; public class ControlNode extends BaseComposableNode { private static String logtag = ControlNode.class.getName(); private final String topic; public Publisher<geometry_msgs.msg.Twist> twist_pub; public ControlNode(final String name, final String topic) { super(name); this.topic = topic; this.twist_pub = this.node.<Twist>createPublisher( geometry_msgs.msg.Twist.class, this.topic); } public void pubVelocity(Vector3 vec_lv, Vector3 vec_av){ Twist m_twist = new Twist(); m_twist.setLinear(vec_lv); m_twist.setAngular(vec_av); this.twist_pub.publish(m_twist); } }
package com.example.originbot; import org.ros2.rcljava.node.BaseComposableNode; import org.ros2.rcljava.publisher.Publisher; import geometry_msgs.msg.Twist; import geometry_msgs.msg.Vector3; public class ControlNode extends BaseComposableNode { private static String logtag = ControlNode.class.getName(); private final String topic; public Publisher<geometry_msgs.msg.Twist> twist_pub; public ControlNode(final String name, final String topic) { super(name); this.topic = topic; this.twist_pub = this.node.<Twist>createPublisher( geometry_msgs.msg.Twist.class, this.topic); } public void pubVelocity(Vector3 vec_lv, Vector3 vec_av){ Twist m_twist = new Twist(); m_twist.setLinear(vec_lv); m_twist.setAngular(vec_av); this.twist_pub.publish(m_twist); } }
复制
然后在mainActivity中添加节点,并设定与OriginBot中相同的话题名称:
private ControlNode control_node;
...
control_node = new ControlNode("OriginBot_control", "/cmd_vel");
getExecutor().addNode(control_node);
private ControlNode control_node; ... control_node = new ControlNode("OriginBot_control", "/cmd_vel"); getExecutor().addNode(control_node);
private ControlNode control_node; ... control_node = new ControlNode("OriginBot_control", "/cmd_vel"); getExecutor().addNode(control_node);
private ControlNode control_node; ... control_node = new ControlNode("OriginBot_control", "/cmd_vel"); getExecutor().addNode(control_node);
复制
一般来说为了方便调节控制小车的速度,还需要有设置最大线速度和最大角速度的功能;这里使用两个SeekBar控件来控制;我这里设置默认最大线速度为0.5m/s,最高1m/s;最大角速度为0.3rad/s,最高0.5rad/s;这几个数值可以根据小车的具体情况进行更改设置。如下setMaxVelocity函数实现:
@SuppressLint({"SetTextI18n", "DefaultLocale"})
private void setMaxVelocity(){
mLinearTextView = findViewById(R.id.linear_textview);
mAngularTextView = findViewById(R.id.angle_textview);
SeekBar mLinearSeekBar = findViewById(R.id.linear_seekBar);
mLinearSeekBar.setMax(1000);
mLinearSeekBar.setProgress(500);
mLinearTextView.setText("最大线速度:" + 0.50 + " m/s ");
mLinearSeekBar.setOnSeekBarChangeListener(new SeekBar.OnSeekBarChangeListener() {
@Override
public void onProgressChanged(SeekBar seekBar, int i, boolean b) {
max_v = 1.0 * i/1000;
mLinearTextView.setText(String.format("最大线速度:%1$.2f m/s ", max_v));
}
@Override
public void onStartTrackingTouch(SeekBar seekBar) {
Toast.makeText(mContext, "开始设置", Toast.LENGTH_SHORT).show();
}
@Override
public void onStopTrackingTouch(SeekBar seekBar) {
Toast.makeText(mContext, "完成设置", Toast.LENGTH_SHORT).show();
}
});
SeekBar mAngularSeekBar = findViewById(R.id.angle_seekBar);
mAngularSeekBar.setMax(500);
mAngularSeekBar.setProgress(300);
mAngularTextView.setText("最大角速度:" + 0.30 + " rad/s ");
mAngularSeekBar.setOnSeekBarChangeListener(new SeekBar.OnSeekBarChangeListener() {
@Override
public void onProgressChanged(SeekBar seekBar, int i, boolean b) {
max_a = 1.0 * i/1000;
mAngularTextView.setText(String.format("最大角速度:%1$.2f rad/s ", max_a));
}
@Override
public void onStartTrackingTouch(SeekBar seekBar) {
Toast.makeText(mContext, "开始设置", Toast.LENGTH_SHORT).show();
}
@Override
public void onStopTrackingTouch(SeekBar seekBar) {
Toast.makeText(mContext, "完成设置", Toast.LENGTH_SHORT).show();
}
});
}
@SuppressLint({"SetTextI18n", "DefaultLocale"}) private void setMaxVelocity(){ mLinearTextView = findViewById(R.id.linear_textview); mAngularTextView = findViewById(R.id.angle_textview); SeekBar mLinearSeekBar = findViewById(R.id.linear_seekBar); mLinearSeekBar.setMax(1000); mLinearSeekBar.setProgress(500); mLinearTextView.setText("最大线速度:" + 0.50 + " m/s "); mLinearSeekBar.setOnSeekBarChangeListener(new SeekBar.OnSeekBarChangeListener() { @Override public void onProgressChanged(SeekBar seekBar, int i, boolean b) { max_v = 1.0 * i/1000; mLinearTextView.setText(String.format("最大线速度:%1$.2f m/s ", max_v)); } @Override public void onStartTrackingTouch(SeekBar seekBar) { Toast.makeText(mContext, "开始设置", Toast.LENGTH_SHORT).show(); } @Override public void onStopTrackingTouch(SeekBar seekBar) { Toast.makeText(mContext, "完成设置", Toast.LENGTH_SHORT).show(); } }); SeekBar mAngularSeekBar = findViewById(R.id.angle_seekBar); mAngularSeekBar.setMax(500); mAngularSeekBar.setProgress(300); mAngularTextView.setText("最大角速度:" + 0.30 + " rad/s "); mAngularSeekBar.setOnSeekBarChangeListener(new SeekBar.OnSeekBarChangeListener() { @Override public void onProgressChanged(SeekBar seekBar, int i, boolean b) { max_a = 1.0 * i/1000; mAngularTextView.setText(String.format("最大角速度:%1$.2f rad/s ", max_a)); } @Override public void onStartTrackingTouch(SeekBar seekBar) { Toast.makeText(mContext, "开始设置", Toast.LENGTH_SHORT).show(); } @Override public void onStopTrackingTouch(SeekBar seekBar) { Toast.makeText(mContext, "完成设置", Toast.LENGTH_SHORT).show(); } }); }
@SuppressLint({"SetTextI18n", "DefaultLocale"}) private void setMaxVelocity(){ mLinearTextView = findViewById(R.id.linear_textview); mAngularTextView = findViewById(R.id.angle_textview); SeekBar mLinearSeekBar = findViewById(R.id.linear_seekBar); mLinearSeekBar.setMax(1000); mLinearSeekBar.setProgress(500); mLinearTextView.setText("最大线速度:" + 0.50 + " m/s "); mLinearSeekBar.setOnSeekBarChangeListener(new SeekBar.OnSeekBarChangeListener() { @Override public void onProgressChanged(SeekBar seekBar, int i, boolean b) { max_v = 1.0 * i/1000; mLinearTextView.setText(String.format("最大线速度:%1$.2f m/s ", max_v)); } @Override public void onStartTrackingTouch(SeekBar seekBar) { Toast.makeText(mContext, "开始设置", Toast.LENGTH_SHORT).show(); } @Override public void onStopTrackingTouch(SeekBar seekBar) { Toast.makeText(mContext, "完成设置", Toast.LENGTH_SHORT).show(); } }); SeekBar mAngularSeekBar = findViewById(R.id.angle_seekBar); mAngularSeekBar.setMax(500); mAngularSeekBar.setProgress(300); mAngularTextView.setText("最大角速度:" + 0.30 + " rad/s "); mAngularSeekBar.setOnSeekBarChangeListener(new SeekBar.OnSeekBarChangeListener() { @Override public void onProgressChanged(SeekBar seekBar, int i, boolean b) { max_a = 1.0 * i/1000; mAngularTextView.setText(String.format("最大角速度:%1$.2f rad/s ", max_a)); } @Override public void onStartTrackingTouch(SeekBar seekBar) { Toast.makeText(mContext, "开始设置", Toast.LENGTH_SHORT).show(); } @Override public void onStopTrackingTouch(SeekBar seekBar) { Toast.makeText(mContext, "完成设置", Toast.LENGTH_SHORT).show(); } }); }
@SuppressLint({"SetTextI18n", "DefaultLocale"}) private void setMaxVelocity(){ mLinearTextView = findViewById(R.id.linear_textview); mAngularTextView = findViewById(R.id.angle_textview); SeekBar mLinearSeekBar = findViewById(R.id.linear_seekBar); mLinearSeekBar.setMax(1000); mLinearSeekBar.setProgress(500); mLinearTextView.setText("最大线速度:" + 0.50 + " m/s "); mLinearSeekBar.setOnSeekBarChangeListener(new SeekBar.OnSeekBarChangeListener() { @Override public void onProgressChanged(SeekBar seekBar, int i, boolean b) { max_v = 1.0 * i/1000; mLinearTextView.setText(String.format("最大线速度:%1$.2f m/s ", max_v)); } @Override public void onStartTrackingTouch(SeekBar seekBar) { Toast.makeText(mContext, "开始设置", Toast.LENGTH_SHORT).show(); } @Override public void onStopTrackingTouch(SeekBar seekBar) { Toast.makeText(mContext, "完成设置", Toast.LENGTH_SHORT).show(); } }); SeekBar mAngularSeekBar = findViewById(R.id.angle_seekBar); mAngularSeekBar.setMax(500); mAngularSeekBar.setProgress(300); mAngularTextView.setText("最大角速度:" + 0.30 + " rad/s "); mAngularSeekBar.setOnSeekBarChangeListener(new SeekBar.OnSeekBarChangeListener() { @Override public void onProgressChanged(SeekBar seekBar, int i, boolean b) { max_a = 1.0 * i/1000; mAngularTextView.setText(String.format("最大角速度:%1$.2f rad/s ", max_a)); } @Override public void onStartTrackingTouch(SeekBar seekBar) { Toast.makeText(mContext, "开始设置", Toast.LENGTH_SHORT).show(); } @Override public void onStopTrackingTouch(SeekBar seekBar) { Toast.makeText(mContext, "完成设置", Toast.LENGTH_SHORT).show(); } }); }
复制
在上面的代码中我还添加了两个Toast提示开始设置和完成设置,如果不需要可以忽略。
编译过程
编译过程中需要注意以下几点:
- 将上一篇文章中编译好的ROS2相关的jar文件放入app/libs目录,必须包括geometry_msgs_messages.jar文件。
- 将编译好的ROS2相关的库文件放入app/src/main/jniLibs/arm64-v8a,必须包括geometry_msgs_messages相关库文件。
- 在app/build.gradle文件中加入:
dependencies {
...
implementation fileTree(include: ['*.jar'], dir: 'libs')
}
dependencies { ... implementation fileTree(include: ['*.jar'], dir: 'libs') }
dependencies { ... implementation fileTree(include: ['*.jar'], dir: 'libs') }
dependencies { ... implementation fileTree(include: ['*.jar'], dir: 'libs') }
复制
结果展示
APP的运行界面如下:
推动摇杆后发布/cmd_vel话题,可以在同一网段下的PC端以及OriginBot的终端查看发布的消息:
同时启动OriginBot的底层控制,上下推动摇杆可看到小车前后移动,左右推动摇杆可看到小车旋转,推动摇杆的距离变化可以放慢或加快小车的速度,设置最大线/角速度可以改变速度的调整范围。
在测试过程中,直观上看,手机和小车之间的控制是非常流畅的,和使用蓝牙手柄控制相比没有太大的区别,至此我的目的也达到了。
总结延伸
在这篇文章中,我实现了APP内摇杆控制小车运动,但是那么大的手机界面只用来实现摇杆控制未免太过单调;可以看到我将APP的界面设置为横屏显示,目的就是为了获取OriginBot小车的一些数据显示在空余界面上;那么下一篇文章就来聊聊如何实现这个功能。
评论(1)
您还未登录,请登录后发表或查看评论