Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Develop #319

Merged
merged 10 commits into from
Oct 13, 2022
Merged

Develop #319

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
10 commits
Select commit Hold shift + click to select a range
c48bfab
vel=0 when ROS is disconnected
nabeya11 Mar 7, 2022
39dfdf7
too short time for JointTrajectoryPoint
nabeya11 Mar 7, 2022
3231114
increase flash timeout delay
ROBOTIS-Will Sep 6, 2022
ba3262a
Add turtlebot3_with_open_manipulator
hancheol-cho Sep 6, 2022
7bf5894
Fixed turtlebot3_ros2 for manipulator.
hancheol-cho Sep 26, 2022
73fc6bf
revise example name
ROBOTIS-Will Oct 6, 2022
d282c40
Merge branch 'master' into develop
ROBOTIS-Will Oct 6, 2022
08013c8
Merge pull request #316 from ROBOTIS-GIT/foxy-devel
ROBOTIS-Will Oct 6, 2022
9955b40
Merge pull request #298 from nabeya11/TB3_withOM
ROBOTIS-Will Oct 13, 2022
a5138f4
prepare release 1.5.2
ROBOTIS-Will Oct 13, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
72 changes: 38 additions & 34 deletions ...nipulator/turtlebot3_with_open_manipulator_core/turtlebot3_with_open_manipulator_core.ino
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -38,7 +38,7 @@ void setup()
nh.subscribe(motor_power_sub);
nh.subscribe(reset_sub);

nh.advertise(sensor_state_pub);
nh.advertise(sensor_state_pub);
nh.advertise(version_info_pub);
nh.advertise(imu_pub);
nh.advertise(cmd_vel_rc100_pub);
Expand Down Expand Up @@ -86,7 +86,7 @@ void loop()

if ((t-tTime[0]) >= (1000 / CONTROL_MOTOR_SPEED_FREQUENCY))
{
updateGoalVelocity();
updateGoalVelocity(nh.connected());
motor_driver.controlMotor(WHEEL_RADIUS, WHEEL_SEPARATION, goal_velocity);
tTime[0] = t;
}
Expand Down Expand Up @@ -135,7 +135,7 @@ void loop()
// Send log message after ROS connection
sendLogMsg();

// Receive data from RC100
// Receive data from RC100
controllers.getRCdata(goal_velocity_from_rc100);

// Check push button pressed for simple test drive
Expand Down Expand Up @@ -218,7 +218,7 @@ void gripperPositionCallback(const std_msgs::Float64MultiArray& gripper_msg)
void gripperMoveTimeCallback(const std_msgs::Float64& time_msg)
{
double data = time_msg.data;

manipulator_driver.writeGripperProfileControlParam(data);
}

Expand Down Expand Up @@ -261,7 +261,7 @@ void resetCallback(const std_msgs::Empty& reset_msg)
initOdom();

sprintf(log_msg, "Reset Odometry");
nh.loginfo(log_msg);
nh.loginfo(log_msg);
}

/*******************************************************************************
Expand Down Expand Up @@ -326,7 +326,7 @@ void publishSensorStateMsg(void)
// sensor_state_msg.sonar = sensors.getSonarData();

sensor_state_msg.illumination = sensors.getIlluminationData();

sensor_state_msg.button = sensors.checkPushButton();

sensor_state_msg.torque = motor_driver.getTorque();
Expand Down Expand Up @@ -359,7 +359,7 @@ void publishBatteryStateMsg(void)
if (battery_state == 0)
battery_state_msg.present = false;
else
battery_state_msg.present = true;
battery_state_msg.present = true;

battery_state_pub.publish(&battery_state_msg);
}
Expand Down Expand Up @@ -411,7 +411,7 @@ void updateTFPrefix(bool isConnected)
if (!strcmp(get_tf_prefix, ""))
{
sprintf(odom_header_frame_id, "odom");
sprintf(odom_child_frame_id, "base_footprint");
sprintf(odom_child_frame_id, "base_footprint");

sprintf(imu_frame_id, "imu_link");
sprintf(mag_frame_id, "mag_link");
Expand All @@ -435,16 +435,16 @@ void updateTFPrefix(bool isConnected)
}

sprintf(log_msg, "Setup TF on Odometry [%s]", odom_header_frame_id);
nh.loginfo(log_msg);
nh.loginfo(log_msg);

sprintf(log_msg, "Setup TF on IMU [%s]", imu_frame_id);
nh.loginfo(log_msg);
nh.loginfo(log_msg);

sprintf(log_msg, "Setup TF on MagneticField [%s]", mag_frame_id);
nh.loginfo(log_msg);
nh.loginfo(log_msg);

sprintf(log_msg, "Setup TF on JointState [%s]", joint_state_header_frame_id);
nh.loginfo(log_msg);
nh.loginfo(log_msg);

isChecked = true;
}
Expand Down Expand Up @@ -473,7 +473,7 @@ void updateOdometry(void)
}

/*******************************************************************************
* Update the joint states
* Update the joint states
*******************************************************************************/
void updateJointStates(void)
{
Expand Down Expand Up @@ -533,7 +533,7 @@ void updateMotorInfo(int32_t left_tick, int32_t right_tick)
{
int32_t current_tick = 0;
static int32_t last_tick[WHEEL_NUM] = {0, 0};

if (init_encoder)
{
for (int index = 0; index < WHEEL_NUM; index++)
Expand All @@ -543,7 +543,7 @@ void updateMotorInfo(int32_t left_tick, int32_t right_tick)
last_rad[index] = 0.0;

last_velocity[index] = 0.0;
}
}

last_tick[LEFT] = left_tick;
last_tick[RIGHT] = right_tick;
Expand Down Expand Up @@ -597,9 +597,9 @@ bool calcOdometry(double diff_time)
wheel_r = 0.0;

delta_s = WHEEL_RADIUS * (wheel_r + wheel_l) / 2.0;
// theta = WHEEL_RADIUS * (wheel_r - wheel_l) / WHEEL_SEPARATION;
// theta = WHEEL_RADIUS * (wheel_r - wheel_l) / WHEEL_SEPARATION;
orientation = sensors.getOrientation();
theta = atan2f(orientation[1]*orientation[2] + orientation[0]*orientation[3],
theta = atan2f(orientation[1]*orientation[2] + orientation[0]*orientation[3],
0.5f - orientation[2]*orientation[2] - orientation[3]*orientation[3]);

delta_theta = theta - last_theta;
Expand Down Expand Up @@ -634,8 +634,8 @@ void jointControl(void)
const double JOINT_CONTROL_PERIOD = 1.0f / (double)JOINT_CONTROL_FREQEUNCY;
static uint32_t points = 0;

static uint8_t wait_for_write = 0;
static uint8_t loop_cnt = 0;
static uint16_t wait_for_write = 0;
static uint16_t loop_cnt = 0;

if (is_moving == true)
{
Expand All @@ -657,7 +657,7 @@ void jointControl(void)
else move_time = joint_trajectory_point.data[points] - joint_trajectory_point.data[points - POINT_SIZE];

for (uint32_t positions = points + 1; positions < (points + POINT_SIZE); positions++)
{
{
if ((points + POINT_SIZE) >= all_points_cnt)
{
goal_joint_position[write_cnt] = joint_trajectory_point.data[positions];
Expand Down Expand Up @@ -703,7 +703,7 @@ void driveTest(uint8_t buttons)

motor_driver.readEncoder(current_tick[LEFT], current_tick[RIGHT]);

if (buttons & (1<<0))
if (buttons & (1<<0))
{
move[LINEAR] = true;
saved_tick[RIGHT] = current_tick[RIGHT];
Expand All @@ -719,7 +719,7 @@ void driveTest(uint8_t buttons)
}

if (move[LINEAR])
{
{
if (abs(saved_tick[RIGHT] - current_tick[RIGHT]) <= diff_encoder)
{
goal_velocity_from_button[LINEAR] = 0.05;
Expand All @@ -731,7 +731,7 @@ void driveTest(uint8_t buttons)
}
}
else if (move[ANGULAR])
{
{
if (abs(saved_tick[RIGHT] - current_tick[RIGHT]) <= diff_encoder)
{
goal_velocity_from_button[ANGULAR]= -0.7;
Expand All @@ -750,11 +750,11 @@ void driveTest(uint8_t buttons)
void updateVariable(bool isConnected)
{
static bool variable_flag = false;

if (isConnected)
{
if (variable_flag == false)
{
{
sensors.initIMU();
initOdom();

Expand All @@ -773,11 +773,11 @@ void updateVariable(bool isConnected)
void waitForSerialLink(bool isConnected)
{
static bool wait_flag = false;

if (isConnected)
{
if (wait_flag == false)
{
{
delay(10);

wait_flag = true;
Expand Down Expand Up @@ -854,17 +854,17 @@ void updateGyroCali(void)
void sendLogMsg(void)
{
static bool log_flag = false;
char log_msg[100];
char log_msg[100];

String firmware_version = FIRMWARE_VER;
String bringup_log = "This core(v" + firmware_version + ") is compatible with TB3 with OpenManipulator";

const char* init_log_data = bringup_log.c_str();

if (nh.connected())
{
if (log_flag == false)
{
{
sprintf(log_msg, "--------------------------");
nh.loginfo(log_msg);

Expand Down Expand Up @@ -931,8 +931,12 @@ void initJointStates(void)
/*******************************************************************************
* Update Goal Velocity
*******************************************************************************/
void updateGoalVelocity(void)
void updateGoalVelocity(bool isConnected)
{
if(!isConnected){
goal_velocity_from_cmd[LINEAR] = 0.0;
goal_velocity_from_cmd[ANGULAR] = 0.0;
}
goal_velocity[LINEAR] = goal_velocity_from_button[LINEAR] + goal_velocity_from_cmd[LINEAR] + goal_velocity_from_rc100[LINEAR];
goal_velocity[ANGULAR] = goal_velocity_from_button[ANGULAR] + goal_velocity_from_cmd[ANGULAR] + goal_velocity_from_rc100[ANGULAR];
}
Expand Down Expand Up @@ -971,7 +975,7 @@ void sendDebuglog(void)
DEBUG_SERIAL.print(" x : "); DEBUG_SERIAL.println(quat[1]);
DEBUG_SERIAL.print(" y : "); DEBUG_SERIAL.println(quat[2]);
DEBUG_SERIAL.print(" z : "); DEBUG_SERIAL.println(quat[3]);

DEBUG_SERIAL.println("---------------------------------------");
DEBUG_SERIAL.println("DYNAMIXELS");
DEBUG_SERIAL.println("---------------------------------------");
Expand All @@ -980,7 +984,7 @@ void sendDebuglog(void)

int32_t encoder[WHEEL_NUM] = {0, 0};
motor_driver.readEncoder(encoder[LEFT], encoder[RIGHT]);

DEBUG_SERIAL.println("Encoder(left) : " + String(encoder[LEFT]));
DEBUG_SERIAL.println("Encoder(right) : " + String(encoder[RIGHT]));

Expand All @@ -998,7 +1002,7 @@ void sendDebuglog(void)
DEBUG_SERIAL.println("---------------------------------------");
DEBUG_SERIAL.println("TurtleBot3");
DEBUG_SERIAL.println("---------------------------------------");
DEBUG_SERIAL.println("Odometry : ");
DEBUG_SERIAL.println("Odometry : ");
DEBUG_SERIAL.print(" x : "); DEBUG_SERIAL.println(odom_pose[0]);
DEBUG_SERIAL.print(" y : "); DEBUG_SERIAL.println(odom_pose[1]);
DEBUG_SERIAL.print(" theta : "); DEBUG_SERIAL.println(odom_pose[2]);
Expand Down
35 changes: 35 additions & 0 deletions ...cr/libraries/turtlebot3_ros2/examples/turtlebot3_manipulation/turtlebot3_manipulation.ino
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,35 @@
/*******************************************************************************
* Copyright 2016 ROBOTIS CO., LTD.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*******************************************************************************/

#include <TurtleBot3_ROS2.h>

/*******************************************************************************
* Setup function
*******************************************************************************/
void setup()
{
// Begin TurtleBot3 core for support Waffle.
TurtleBot3Core::begin("Waffle_OpenManipulator");
}

/*******************************************************************************
* Loop function
*******************************************************************************/
void loop()
{
// Run TurtleBot3 core for communicating with ROS2 node, sensing several sensors and controlling actuators.
TurtleBot3Core::run();
}
Loading