Hello
Specifications: CORE2 with RPi
I bridged CORE2 & ROS with that: opt/husarion/tools/rpi-linux/ros-core2-client /dev/ttyCORE2
I flashed this code on the RPi through VSCode:
#include "hFramework.h"
#include "hCloudClient.h"
#include "ros.h"
#include "geometry_msgs/Twist.h"
#include "sensor_msgs/BatteryState.h"
#include "std_msgs/Bool.h"
#include "geometry_msgs/PoseStamped.h"
#include "tf/tf.h"
#include "ROSbot.h"
using namespace hFramework;
// Uncomment one of these lines, accordingly to range sensor type of your ROSbot
// If you have version with infared sensor:
// static const SensorType sensor_type = SENSOR_INFRARED;
// If you have version with laser sensor:
static const SensorType sensor_type = SENSOR_LASER;
// If you want to use your own sensor:
// static const SensorType sensor_type = NO_DISTANCE_SENSOR;
// Uncomment one of these lines, accordingly to IMU sensor type of your device
// If you have version with MPU9250:
static const ImuType imu_type = MPU9250;
// If you want to use your own sensor:
// static const ImuType imu_type = NO_IMU;
ros::NodeHandle nh;
sensor_msgs::BatteryState battery;
ros::Publisher *battery_pub;
geometry_msgs::PoseStamped pose;
ros::Publisher *pose_pub;
std::vector<float> rosbot_pose;
int publish_counter = 0;
void twistCallback(const geometry_msgs::Twist &twist)
{
rosbot.setSpeed(twist.linear.x, twist.angular.z);
}
void initCmdVelSubscriber()
{
ros::Subscriber<geometry_msgs::Twist> *cmd_sub = new ros::Subscriber<geometry_msgs::Twist>("/cmd_vel", &twistCallback);
nh.subscribe(*cmd_sub);
}
void resetCallback(const std_msgs::Bool &msg)
{
if (msg.data == true)
{
rosbot.reset_odometry();
}
}
void initResetOdomSubscriber()
{
ros::Subscriber<std_msgs::Bool> *odom_reset_sub = new ros::Subscriber<std_msgs::Bool>("/reset_odom", &resetCallback);
nh.subscribe(*odom_reset_sub);
}
void initBatteryPublisher()
{
battery_pub = new ros::Publisher("/battery", &battery);
nh.advertise(*battery_pub);
}
void initPosePublisher()
{
pose.header.frame_id = "base_link";
pose.pose.orientation = tf::createQuaternionFromYaw(0);
pose_pub = new ros::Publisher("/pose", &pose);
nh.advertise(*pose_pub);
}
void hMain()
{
rosbot.initROSbot(sensor_type);
platform.begin(&RPi);
nh.getHardware()->initWithDevice(&platform.LocalSerial);
nh.initNode();
initBatteryPublisher();
initCmdVelSubscriber();
initResetOdomSubscriber();
initPosePublisher();
while (true)
{
nh.spinOnce();
publish_counter++;
if (publish_counter > 10)
{
// get ROSbot pose
rosbot_pose = rosbot.getPose();
pose.pose.position.x = rosbot_pose[0];
pose.pose.position.y = rosbot_pose[1];
pose.pose.orientation = tf::createQuaternionFromYaw(rosbot_pose[2]);
// publish pose
pose_pub->publish(&pose);
// get battery voltage
battery.voltage = rosbot.getBatteryLevel();
// publish battery voltage
battery_pub->publish(&battery);
publish_counter = 0;
}
sys.delay(10);
}
}
Now suddenly i cant control the robot with: rosrun teleop_twist_keyboard teleop_twist_keyboard.py
It worked last week without problems. Now nothing happens.
Please help me.
Best Regards,
David