記事一覧

rviz サーボを動かして実際のサーボをコントロールしてみた(ros rosserial wifi)

rviz サーボを動かして実際のサーボをコントロールしてみた(ros rosserial wifi)



こんにちはRoboTakaoです
前回、URDFファイルにFusion360で作ったSTLモデルを取り込んで、RViz上でサーボを動かしてみました
今回はRVizのJoint State Publisher GUIのスライドバーで実際のサーボを動かしてみました


rosserial_rviz01.jpg


サーボはM5Atom Liteに接続され、rosserialを使用しています。
M5Atomでrosserialを使ってWiFi経由でサーボを動かすところは以前の投稿にあるのでそちらもみていただければと思います。


今回はこちらのサイトを参考にさせていただきました。
MakerPro "How to Control a Robot Arm with ROS and Arduino"


仕組みとしてはJoint State Publisher GUIのスライドバーを動かすと、/joint_states トピックをM5Atom側で購読して
/joint_statesのpositionの値からサーボの角度を指定しています。


rosserial_rviz03.png

rosserial_rviz04.png


スライドバーを動かせばしっかり動きます。
ようやくここまでできました




M5Atom側のスケッチ


#include "M5Atom.h"
#include <ros.h>
#include <std_msgs/Int32.h>
#include <sensor_msgs/JointState.h>
#include <WiFi.h>

const uint8_t Srv0 = 25;//GPIO No.
const uint8_t srv_CH0 = 0; //チャンネル
const double PWM_Hz = 50; //PWM周波数
const uint8_t PWM_level = 16; //PWM 16bit(0~65535)

int pulseMIN = 1640; //0deg 500μsec 50Hz 16bit
int pulseMAX = 8190; //180deg 2500μsec 50Hz 16bit

float cont_min = -1500;
float cont_max = 1500;
int SrvAng = 4914; //90deg
float TARGET_JOINT_POSITIONS[1] = {0};

bool IMU6886Flag = false;

const char* ssid = "XXXXXXXXXXXXXXXX"; //SSID
const char* password = "XXXXXXXXXXXXXXXX"; //PassWord

WiFiClient client;
IPAddress server(192, 168, 10, 16); //ROS core IP adress

class WiFiHardware {
public:
WiFiHardware() {};
void init() {
client.connect(server, 11411);
}
int read() {
return client.read();
}
void write(uint8_t* data, int length) {
for (int i = 0; i < length; i++)
client.write(data[i]);
}
unsigned long time() {
return millis(); // easy; did this one for you
}
};

ros::NodeHandle_<WiFiHardware> nh;

void servo_cb(const sensor_msgs::JointState& msg){
int target_angle;
TARGET_JOINT_POSITIONS[0] = msg.position[0];
target_angle = 1000 * TARGET_JOINT_POSITIONS[0];
SrvAng = map(target_angle, cont_min, cont_max, pulseMIN, pulseMAX);
Serial.print(target_angle);
Serial.print(" ");
Serial.println(SrvAng);
ledcWrite(srv_CH0, SrvAng);
}

ros::Subscriber<sensor_msgs::JointState> sub("joint_states", servo_cb);

void setupWiFi() {
WiFi.begin(ssid, password);
Serial.print("\nConnecting to "); Serial.println(ssid);
uint8_t i = 0;
while (WiFi.status() != WL_CONNECTED && i++ < 20) delay(500);
if (i == 21) {
Serial.print("Could not connect to"); Serial.println(ssid);
while (1) delay(500);
}
Serial.print("Ready! Use ");
Serial.print(WiFi.localIP());
Serial.println(" to access client");
}

void setup()
{
Serial.begin(115200);
// void M5Atom::begin(bool SerialEnable , bool I2CEnable , bool DisplayEnable )
M5.begin(true, false, true);
setupWiFi();

pinMode(Srv0, OUTPUT);

//モータのPWMのチャンネル、周波数の設定
ledcSetup(srv_CH0, PWM_Hz, PWM_level);

//モータのピンとチャンネルの設定
ledcAttachPin(Srv0, srv_CH0);

ledcWrite(srv_CH0, SrvAng);

nh.initNode();
nh.subscribe(sub);
}

void loop()
{
nh.spinOnce();
delay(1);
}


URDF
URDFは前回のものと同じ

launchファイル


<launch>

<arg name="model" default="$(find robotakao_test01)/urdf/servo_test02.urdf"/>
<arg name="gui" default="true" />
<arg name="rvizconfig" default="$(find urdf_tutorial)/rviz/urdf.rviz" />

<param name="robot_description" command="$(find xacro)/xacro.py $(arg model)" />
<param name="use_gui" value="$(arg gui)"/>

<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
<node pkg="rosserial_python" type="serial_node.py" name="serial_node">
<param name="port" value="tcp"/>
</node>

</launch>




スポンサードリンク

コメント

コメントの投稿

非公開コメント

プロフィール

RoboTakao

Author:RoboTakao
みなさんご訪問ありがとうございます。ロボット作りたいけどお小遣いそんなにないし、簡単でローコストでロボットを作るための私のプロジェクトを紹介します。

ウェブサイトもありますのでそちらもよろしくお願いします。
http://robotakao.jp/

スポンサーリンク