記事一覧

ROS勉強用の2軸のモデルで 現物作ってRviz から動かしてみた。

ros勉強用の2軸のモデルで 現物作ってrviz から動かしてみた。



こんにちはRoboTakaoです
引き続きROSの勉強です。


前回Rvizからサーボを動かしてみましたが、今回は2軸にしてマニピュレータを構成してみました

NX16_01.jpg

サーボとサーボブラケットはFusion360でモデリングしてSTLで書き出し
Blenderで寸法調整と軸方向調整をしました


NX16_04.png

NX16_05.png


launchファイルで起動後、Rvizのjoint state publisher guiのスライドバーでサーボの角度を指定して駆動します。
うまく行きました




本当はgazeboまで持って行きたかったのですが、collisionの座標と慣性モーメントの軸方向がよくわからなくなったので次回に持ち越しです。


URDF


<?xml version="1.0"?>
<robot name="nx16">
<material name="blue">
<color rgba="0.0 0.0 1.0 2.0"/>
</material>
<material name="red">
<color rgba="1.0 0.0 0.0 2.0"/>
</material>
<material name="green">
<color rgba="1.0 0.0 1.0 2.0"/>
</material>

<link name="base_link"/>

<joint name="base_joint" type="fixed">
<parent link="base_link"/>
<child link="link1"/>
</joint>

<link name="link1">
<visual>
<geometry>
<mesh filename="package://robotakao_test01/stl/Link1.stl" />
</geometry>
<origin xyz="0 0 0" rpy="0 0 0"/>
<material name="blue"/>
</visual>
</link>

<joint name="joint1" type="revolute">
<parent link="link1"/>
<child link="link2"/>
<origin xyz="-0.00515 0 0.034" rpy="0 0 0"/>
<axis xyz="0 0 1" />
<limit lower="-1.5" upper="1.5" effort="0" velocity="0"/>
</joint>

<link name="link2">
<visual>
<geometry>
<mesh filename="package://robotakao_test01/stl/Link2.stl" />
</geometry>
<origin xyz="0 0 0.002" rpy="0 0 0"/>
<material name="red"/>
</visual>
</link>

<joint name="joint2" type="revolute">
<parent link="link2"/>
<child link="link3"/>
<origin xyz="0 -0.0051 0.0293" rpy="0 0 0"/>
<axis xyz="0 1 0" />
<limit lower="-1.5" upper="1.5" effort="0" velocity="0"/>
</joint>

<link name="link3">
<visual>
<geometry>
<mesh filename="package://robotakao_test01/stl/Link3.stl" />
</geometry>
<origin xyz="0 0.0021 0" rpy="0 0 0"/>
<material name="green"/>
</visual>
</link>
</robot>


launchファイル


<launch>

<arg name="model" default="$(find robotakao_test01)/urdf/nx16.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>


Arduinoスケッチ


/*
* rosserial Subscriber Example
* Blinks an LED on callback
*/
#include "M5Atom.h"
#include <ros.h<
#include <std_msgs/Int32.h<
#include <sensor_msgs/JointState.h<
#include <WiFi.h<

const uint8_t Srv0 = 33, Srv1 = 23;//GPIO No.
const uint8_t srv_CH0 = 0, srv_CH1 = 1; //チャンネル
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[2] = {4914, 4914}; //90deg
float TARGET_JOINT_POSITIONS[2] = {0, 0};

bool IMU6886Flag = false;

const char* ssid = "XXXXXXXXXXXXXX";
const char* password = "4XXXXXXXXXXXXXX";

WiFiClient client;
IPAddress server(192, 168, 10, 19); //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[2];
TARGET_JOINT_POSITIONS[0] = msg.position[0];
TARGET_JOINT_POSITIONS[1] = msg.position[1];
target_angle[0] = 1000 * TARGET_JOINT_POSITIONS[0];
target_angle[1] = 1000 * TARGET_JOINT_POSITIONS[1];
SrvAng[0] = map(target_angle[0], cont_min, cont_max, pulseMIN, pulseMAX);
SrvAng[1] = map(target_angle[1], cont_min, cont_max, pulseMIN, pulseMAX);
Serial.print(target_angle[0]);
Serial.print(" ");
Serial.print(SrvAng[0]);
Serial.print(" ");
Serial.print(target_angle[1]);
Serial.print(" ");
Serial.println(SrvAng[1]);
ledcWrite(srv_CH0, SrvAng[0]);
ledcWrite(srv_CH1, SrvAng[1]);
}

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);
pinMode(Srv1, OUTPUT);

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

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

ledcWrite(srv_CH0, SrvAng[0]);
ledcWrite(srv_CH1, SrvAng[1]);

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

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

スポンサードリンク

コメント

コメントの投稿

非公開コメント

プロフィール

RoboTakao

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

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

スポンサーリンク