Skip to content

Commit

Permalink
Merge pull request #319 from ROBOTIS-GIT/develop
Browse files Browse the repository at this point in the history 
Develop
  • Loading branch information
@ROBOTIS-Will
ROBOTIS-Will committed Oct 13, 2022
2 parents 5c699a0 + a5138f4 commit b913ecf
Show file tree
Hide file tree
Showing 31 changed files with 1,011 additions and 109 deletions.
72 changes: 38 additions & 34 deletions ...nipulator/turtlebot3_with_open_manipulator_core/turtlebot3_with_open_manipulator_core.ino
View file
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
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:https://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

0 comments on commit b913ecf

Please sign in to comment.