mirror of
https://github.com/westonrobot/scout_ros.git
synced 2026-01-12 03:15:01 +08:00
solved merge conflicts
This commit is contained in:
Tan Pin Da
@@ -1,47 +0,0 @@
|
|||||||
#include <string>
|
|
||||||
|
|
||||||
#include <ros/ros.h>
|
|
||||||
#include <sensor_msgs/JointState.h>
|
|
||||||
#include <tf/transform_broadcaster.h>
|
|
||||||
#include <nav_msgs/Odometry.h>
|
|
||||||
|
|
||||||
#include "wrp_sdk/platforms/scout/scout_base.hpp"
|
|
||||||
#include "scout_base/scout_messenger.hpp"
|
|
||||||
|
|
||||||
using namespace westonrobot;
|
|
||||||
|
|
||||||
int main(int argc, char **argv)
|
|
||||||
{
|
|
||||||
// setup ROS node
|
|
||||||
ros::init(argc, argv, "scout_odom");
|
|
||||||
ros::NodeHandle node(""), private_node("~");
|
|
||||||
|
|
||||||
// instantiate a robot object
|
|
||||||
ScoutBase robot;
|
|
||||||
ScoutROSMessenger messenger(&robot, &node);
|
|
||||||
|
|
||||||
// fetch parameters before connecting to robot
|
|
||||||
std::string port_name;
|
|
||||||
private_node.param<std::string>("port_name", port_name, std::string("can0"));
|
|
||||||
private_node.param<std::string>("odom_frame", messenger.odom_frame_, std::string("odom"));
|
|
||||||
private_node.param<std::string>("base_frame", messenger.base_frame_, std::string("base_link"));
|
|
||||||
private_node.param<bool>("simulated_robot", messenger.simulated_robot_, true);
|
|
||||||
private_node.param<int>("control_rate", messenger.sim_control_rate_, 50);
|
|
||||||
|
|
||||||
// no connection for simulated robot
|
|
||||||
// setup ROS subscription
|
|
||||||
messenger.SetupSubscription();
|
|
||||||
|
|
||||||
// publish robot state at 50Hz while listening to twist commands
|
|
||||||
double linear, angular;
|
|
||||||
ros::Rate rate_50hz(50); // 50Hz
|
|
||||||
while (true)
|
|
||||||
{
|
|
||||||
messenger.GetCurrentMotionCmdForSim(linear, angular);
|
|
||||||
messenger.PublishSimStateToROS(linear, angular);
|
|
||||||
ros::spinOnce();
|
|
||||||
rate_50hz.sleep();
|
|
||||||
}
|
|
||||||
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
@@ -1,9 +1,9 @@
|
|||||||
/*
|
/*
|
||||||
* scout_messenger.cpp
|
* scout_messenger.cpp
|
||||||
*
|
*
|
||||||
* Created on: Apr 26, 2019 22:14
|
* Created on: Apr 26, 2019 22:14
|
||||||
* Description:
|
* Description:
|
||||||
*
|
*
|
||||||
* Copyright (c) 2019 Ruixiang Du (rdu)
|
* Copyright (c) 2019 Ruixiang Du (rdu)
|
||||||
*/
|
*/
|
||||||
|
|
||||||
@@ -13,270 +13,244 @@
|
|||||||
|
|
||||||
#include "scout_msgs/ScoutStatus.h"
|
#include "scout_msgs/ScoutStatus.h"
|
||||||
|
|
||||||
#include <ros/console.h>
|
|
||||||
|
|
||||||
namespace westonrobot
|
namespace westonrobot {
|
||||||
{
|
ScoutROSMessenger::ScoutROSMessenger(ros::NodeHandle *nh)
|
||||||
ScoutROSMessenger::ScoutROSMessenger(ros::NodeHandle *nh) : scout_(nullptr), nh_(nh)
|
: scout_(nullptr), nh_(nh) {}
|
||||||
{
|
|
||||||
|
|
||||||
|
ScoutROSMessenger::ScoutROSMessenger(ScoutBase *scout, ros::NodeHandle *nh)
|
||||||
|
: scout_(scout), nh_(nh) {}
|
||||||
|
|
||||||
|
void ScoutROSMessenger::SetupSubscription() {
|
||||||
|
// odometry publisher
|
||||||
|
odom_publisher_ = nh_->advertise<nav_msgs::Odometry>(odom_frame_, 50);
|
||||||
|
status_publisher_ =
|
||||||
|
nh_->advertise<scout_msgs::ScoutStatus>("/scout_status", 10);
|
||||||
|
|
||||||
|
// cmd subscriber
|
||||||
|
motion_cmd_subscriber_ = nh_->subscribe<geometry_msgs::Twist>(
|
||||||
|
"/cmd_vel", 5, &ScoutROSMessenger::TwistCmdCallback, this);
|
||||||
|
light_cmd_subscriber_ = nh_->subscribe<scout_msgs::ScoutLightCmd>(
|
||||||
|
"/scout_light_control", 5, &ScoutROSMessenger::LightCmdCallback, this);
|
||||||
}
|
}
|
||||||
|
|
||||||
ScoutROSMessenger::ScoutROSMessenger(ScoutBase *scout, ros::NodeHandle *nh) : scout_(scout), nh_(nh)
|
void ScoutROSMessenger::TwistCmdCallback(
|
||||||
{
|
const geometry_msgs::Twist::ConstPtr &msg) {
|
||||||
}
|
if (!simulated_robot_) {
|
||||||
|
scout_->SetMotionCommand(msg->linear.x, msg->angular.z);
|
||||||
void ScoutROSMessenger::SetupSubscription()
|
} else {
|
||||||
{
|
|
||||||
// odometry publisher
|
|
||||||
odom_publisher_ = nh_->advertise<nav_msgs::Odometry>(odom_frame_, 50);
|
|
||||||
status_publisher_ = nh_->advertise<scout_msgs::ScoutStatus>("/scout_status", 10);
|
|
||||||
|
|
||||||
// cmd subscriber
|
|
||||||
motion_cmd_subscriber_ = nh_->subscribe<geometry_msgs::Twist>("/cmd_vel", 5, &ScoutROSMessenger::TwistCmdCallback, this); //不启用平滑包则订阅“cmd_vel”
|
|
||||||
light_cmd_subscriber_ = nh_->subscribe<scout_msgs::ScoutLightCmd>("/scout_light_control", 5, &ScoutROSMessenger::LightCmdCallback, this);
|
|
||||||
}
|
|
||||||
|
|
||||||
void ScoutROSMessenger::TwistCmdCallback(const geometry_msgs::Twist::ConstPtr &msg)
|
|
||||||
{
|
|
||||||
if (!simulated_robot_)
|
|
||||||
{
|
|
||||||
scout_->SetMotionCommand(msg->linear.x, msg->angular.z);
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
std::lock_guard<std::mutex> guard(twist_mutex_);
|
|
||||||
current_twist_ = *msg.get();
|
|
||||||
}
|
|
||||||
// ROS_INFO("cmd received:%f, %f", msg->linear.x, msg->angular.z);
|
|
||||||
}
|
|
||||||
|
|
||||||
void ScoutROSMessenger::GetCurrentMotionCmdForSim(double &linear, double &angular)
|
|
||||||
{
|
|
||||||
std::lock_guard<std::mutex> guard(twist_mutex_);
|
std::lock_guard<std::mutex> guard(twist_mutex_);
|
||||||
linear = current_twist_.linear.x;
|
current_twist_ = *msg.get();
|
||||||
angular = current_twist_.angular.z;
|
}
|
||||||
|
// ROS_INFO("cmd received:%f, %f", msg->linear.x, msg->angular.z);
|
||||||
}
|
}
|
||||||
|
|
||||||
void ScoutROSMessenger::LightCmdCallback(const scout_msgs::ScoutLightCmd::ConstPtr &msg)
|
void ScoutROSMessenger::GetCurrentMotionCmdForSim(double &linear,
|
||||||
{
|
double &angular) {
|
||||||
if (!simulated_robot_)
|
std::lock_guard<std::mutex> guard(twist_mutex_);
|
||||||
{
|
linear = current_twist_.linear.x;
|
||||||
if (msg->enable_cmd_light_control)
|
angular = current_twist_.angular.z;
|
||||||
{
|
}
|
||||||
ScoutLightCmd cmd;
|
|
||||||
|
|
||||||
switch (msg->front_mode)
|
void ScoutROSMessenger::LightCmdCallback(
|
||||||
{
|
const scout_msgs::ScoutLightCmd::ConstPtr &msg) {
|
||||||
case scout_msgs::ScoutLightCmd::LIGHT_CONST_OFF:
|
if (!simulated_robot_) {
|
||||||
{
|
if (msg->enable_cmd_light_control) {
|
||||||
cmd.front_mode = ScoutLightCmd::LightMode::CONST_OFF;
|
ScoutLightCmd cmd;
|
||||||
break;
|
|
||||||
}
|
|
||||||
case scout_msgs::ScoutLightCmd::LIGHT_CONST_ON:
|
|
||||||
{
|
|
||||||
cmd.front_mode = ScoutLightCmd::LightMode::CONST_ON;
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
case scout_msgs::ScoutLightCmd::LIGHT_BREATH:
|
|
||||||
{
|
|
||||||
cmd.front_mode = ScoutLightCmd::LightMode::BREATH;
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
case scout_msgs::ScoutLightCmd::LIGHT_CUSTOM:
|
|
||||||
{
|
|
||||||
cmd.front_mode = ScoutLightCmd::LightMode::CUSTOM;
|
|
||||||
cmd.front_custom_value = msg->front_custom_value;
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
switch (msg->rear_mode)
|
switch (msg->front_mode) {
|
||||||
{
|
case scout_msgs::ScoutLightCmd::LIGHT_CONST_OFF: {
|
||||||
case scout_msgs::ScoutLightCmd::LIGHT_CONST_OFF:
|
cmd.front_mode = ScoutLightCmd::LightMode::CONST_OFF;
|
||||||
{
|
break;
|
||||||
cmd.rear_mode = ScoutLightCmd::LightMode::CONST_OFF;
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
case scout_msgs::ScoutLightCmd::LIGHT_CONST_ON:
|
|
||||||
{
|
|
||||||
cmd.rear_mode = ScoutLightCmd::LightMode::CONST_ON;
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
case scout_msgs::ScoutLightCmd::LIGHT_BREATH:
|
|
||||||
{
|
|
||||||
cmd.rear_mode = ScoutLightCmd::LightMode::BREATH;
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
case scout_msgs::ScoutLightCmd::LIGHT_CUSTOM:
|
|
||||||
{
|
|
||||||
cmd.rear_mode = ScoutLightCmd::LightMode::CUSTOM;
|
|
||||||
cmd.rear_custom_value = msg->rear_custom_value;
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
scout_->SetLightCommand(cmd);
|
|
||||||
}
|
}
|
||||||
else
|
case scout_msgs::ScoutLightCmd::LIGHT_CONST_ON: {
|
||||||
{
|
cmd.front_mode = ScoutLightCmd::LightMode::CONST_ON;
|
||||||
scout_->DisableLightCmdControl();
|
break;
|
||||||
}
|
}
|
||||||
|
case scout_msgs::ScoutLightCmd::LIGHT_BREATH: {
|
||||||
|
cmd.front_mode = ScoutLightCmd::LightMode::BREATH;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
case scout_msgs::ScoutLightCmd::LIGHT_CUSTOM: {
|
||||||
|
cmd.front_mode = ScoutLightCmd::LightMode::CUSTOM;
|
||||||
|
cmd.front_custom_value = msg->front_custom_value;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
switch (msg->rear_mode) {
|
||||||
|
case scout_msgs::ScoutLightCmd::LIGHT_CONST_OFF: {
|
||||||
|
cmd.rear_mode = ScoutLightCmd::LightMode::CONST_OFF;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
case scout_msgs::ScoutLightCmd::LIGHT_CONST_ON: {
|
||||||
|
cmd.rear_mode = ScoutLightCmd::LightMode::CONST_ON;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
case scout_msgs::ScoutLightCmd::LIGHT_BREATH: {
|
||||||
|
cmd.rear_mode = ScoutLightCmd::LightMode::BREATH;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
case scout_msgs::ScoutLightCmd::LIGHT_CUSTOM: {
|
||||||
|
cmd.rear_mode = ScoutLightCmd::LightMode::CUSTOM;
|
||||||
|
cmd.rear_custom_value = msg->rear_custom_value;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
scout_->SetLightCommand(cmd);
|
||||||
|
} else {
|
||||||
|
scout_->DisableLightCmdControl();
|
||||||
}
|
}
|
||||||
else
|
} else {
|
||||||
{
|
std::cout << "simulated robot received light control cmd" << std::endl;
|
||||||
std::cout << "simulated robot received light control cmd" << std::endl;
|
}
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void ScoutROSMessenger::PublishStateToROS()
|
|
||||||
{
|
|
||||||
current_time_ = ros::Time::now();
|
|
||||||
double dt = (current_time_ - last_time_).toSec();
|
|
||||||
|
|
||||||
|
void ScoutROSMessenger::PublishStateToROS() {
|
||||||
|
current_time_ = ros::Time::now();
|
||||||
|
double dt = (current_time_ - last_time_).toSec();
|
||||||
|
|
||||||
static bool init_run = true;
|
static bool init_run = true;
|
||||||
if (init_run)
|
if (init_run) {
|
||||||
{
|
|
||||||
last_time_ = current_time_;
|
|
||||||
init_run = false;
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
auto state = scout_->GetScoutState();
|
|
||||||
|
|
||||||
// publish scout state message
|
|
||||||
scout_msgs::ScoutStatus status_msg;
|
|
||||||
|
|
||||||
status_msg.header.stamp = current_time_;
|
|
||||||
|
|
||||||
status_msg.linear_velocity = state.linear_velocity;
|
|
||||||
status_msg.angular_velocity = state.angular_velocity;
|
|
||||||
|
|
||||||
status_msg.base_state = state.base_state;
|
|
||||||
status_msg.control_mode = state.control_mode;
|
|
||||||
status_msg.fault_code = state.fault_code;
|
|
||||||
status_msg.battery_voltage = state.battery_voltage;
|
|
||||||
|
|
||||||
for (int i = 0; i < 4; ++i)
|
|
||||||
{
|
|
||||||
status_msg.motor_states[i].current = state.motor_states[i].current;
|
|
||||||
status_msg.motor_states[i].rpm = state.motor_states[i].rpm;
|
|
||||||
status_msg.motor_states[i].temperature = state.motor_states[i].temperature;
|
|
||||||
}
|
|
||||||
|
|
||||||
status_msg.light_control_enabled = state.light_control_enabled;
|
|
||||||
status_msg.front_light_state.mode = state.front_light_state.mode;
|
|
||||||
status_msg.front_light_state.custom_value = state.front_light_state.custom_value;
|
|
||||||
status_msg.rear_light_state.mode = state.rear_light_state.mode;
|
|
||||||
status_msg.rear_light_state.custom_value = state.front_light_state.custom_value;
|
|
||||||
|
|
||||||
status_publisher_.publish(status_msg);
|
|
||||||
|
|
||||||
// publish odometry and tf
|
|
||||||
PublishOdometryToROS(state.linear_velocity, state.angular_velocity, dt);
|
|
||||||
// ROS_DEBUG("dt is %lg",dt);
|
|
||||||
std::cout<<"dt is"<<dt<< std::endl;
|
|
||||||
ROS_INFO("dt is %lg",dt);
|
|
||||||
|
|
||||||
// record time for next integration
|
|
||||||
last_time_ = current_time_;
|
last_time_ = current_time_;
|
||||||
|
init_run = false;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
auto state = scout_->GetScoutState();
|
||||||
|
|
||||||
|
// publish scout state message
|
||||||
|
scout_msgs::ScoutStatus status_msg;
|
||||||
|
|
||||||
|
status_msg.header.stamp = current_time_;
|
||||||
|
|
||||||
|
status_msg.linear_velocity = state.linear_velocity;
|
||||||
|
status_msg.angular_velocity = state.angular_velocity;
|
||||||
|
|
||||||
|
status_msg.base_state = state.base_state;
|
||||||
|
status_msg.control_mode = state.control_mode;
|
||||||
|
status_msg.fault_code = state.fault_code;
|
||||||
|
status_msg.battery_voltage = state.battery_voltage;
|
||||||
|
|
||||||
|
for (int i = 0; i < 4; ++i) {
|
||||||
|
status_msg.motor_states[i].current = state.motor_states[i].current;
|
||||||
|
status_msg.motor_states[i].rpm = state.motor_states[i].rpm;
|
||||||
|
status_msg.motor_states[i].temperature = state.motor_states[i].temperature;
|
||||||
|
}
|
||||||
|
|
||||||
|
status_msg.light_control_enabled = state.light_control_enabled;
|
||||||
|
status_msg.front_light_state.mode = state.front_light_state.mode;
|
||||||
|
status_msg.front_light_state.custom_value =
|
||||||
|
state.front_light_state.custom_value;
|
||||||
|
status_msg.rear_light_state.mode = state.rear_light_state.mode;
|
||||||
|
status_msg.rear_light_state.custom_value =
|
||||||
|
state.front_light_state.custom_value;
|
||||||
|
status_publisher_.publish(status_msg);
|
||||||
|
|
||||||
|
// publish odometry and tf
|
||||||
|
PublishOdometryToROS(state.linear_velocity, state.angular_velocity, dt);
|
||||||
|
|
||||||
|
// record time for next integration
|
||||||
|
last_time_ = current_time_;
|
||||||
}
|
}
|
||||||
|
|
||||||
void ScoutROSMessenger::PublishSimStateToROS(double linear, double angular)
|
void ScoutROSMessenger::PublishSimStateToROS(double linear, double angular) {
|
||||||
{
|
current_time_ = ros::Time::now();
|
||||||
current_time_ = ros::Time::now();
|
|
||||||
|
|
||||||
static bool init_run = true;
|
double dt = (current_time_ - last_time_).toSec();
|
||||||
if (init_run)
|
|
||||||
{
|
|
||||||
last_time_ = current_time_;
|
|
||||||
init_run = false;
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
double dt = (current_time_ - last_time_).toSec();
|
|
||||||
|
|
||||||
// double dt = 1.0 / sim_control_rate_;
|
|
||||||
ROS_INFO("dt is %lg",dt);
|
|
||||||
// double test_time = current_time_.toSec();
|
|
||||||
// ROS_INFO("current time is %lg",test_time);
|
|
||||||
|
|
||||||
// publish scout state message
|
static bool init_run = true;
|
||||||
scout_msgs::ScoutStatus status_msg;
|
if (init_run) {
|
||||||
|
|
||||||
status_msg.header.stamp = current_time_;
|
|
||||||
|
|
||||||
status_msg.linear_velocity = linear;
|
|
||||||
status_msg.angular_velocity = angular;
|
|
||||||
|
|
||||||
status_msg.base_state = 0x00;
|
|
||||||
status_msg.control_mode = 0x01;
|
|
||||||
status_msg.fault_code = 0x00;
|
|
||||||
status_msg.battery_voltage = 29.5;
|
|
||||||
|
|
||||||
// for (int i = 0; i < 4; ++i)
|
|
||||||
// {
|
|
||||||
// status_msg.motor_states[i].current = state.motor_states[i].current;
|
|
||||||
// status_msg.motor_states[i].rpm = state.motor_states[i].rpm;
|
|
||||||
// status_msg.motor_states[i].temperature = state.motor_states[i].temperature;
|
|
||||||
// }
|
|
||||||
|
|
||||||
status_msg.light_control_enabled = false;
|
|
||||||
// status_msg.front_light_state.mode = state.front_light_state.mode;
|
|
||||||
// status_msg.front_light_state.custom_value = state.front_light_state.custom_value;
|
|
||||||
// status_msg.rear_light_state.mode = state.rear_light_state.mode;
|
|
||||||
// status_msg.rear_light_state.custom_value = state.front_light_state.custom_value;
|
|
||||||
|
|
||||||
status_publisher_.publish(status_msg);
|
|
||||||
|
|
||||||
// publish odometry and tf
|
|
||||||
PublishOdometryToROS(linear, angular, dt);
|
|
||||||
last_time_ = current_time_;
|
last_time_ = current_time_;
|
||||||
|
init_run = false;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
ROS_INFO("dt is %lg",dt);
|
||||||
|
|
||||||
|
// publish scout state message
|
||||||
|
scout_msgs::ScoutStatus status_msg;
|
||||||
|
|
||||||
|
status_msg.header.stamp = current_time_;
|
||||||
|
|
||||||
|
status_msg.linear_velocity = linear;
|
||||||
|
status_msg.angular_velocity = angular;
|
||||||
|
|
||||||
|
status_msg.base_state = 0x00;
|
||||||
|
status_msg.control_mode = 0x01;
|
||||||
|
status_msg.fault_code = 0x00;
|
||||||
|
status_msg.battery_voltage = 29.5;
|
||||||
|
|
||||||
|
// for (int i = 0; i < 4; ++i)
|
||||||
|
// {
|
||||||
|
// status_msg.motor_states[i].current = state.motor_states[i].current;
|
||||||
|
// status_msg.motor_states[i].rpm = state.motor_states[i].rpm;
|
||||||
|
// status_msg.motor_states[i].temperature =
|
||||||
|
// state.motor_states[i].temperature;
|
||||||
|
// }
|
||||||
|
|
||||||
|
status_msg.light_control_enabled = false;
|
||||||
|
// status_msg.front_light_state.mode = state.front_light_state.mode;
|
||||||
|
// status_msg.front_light_state.custom_value =
|
||||||
|
// state.front_light_state.custom_value; status_msg.rear_light_state.mode =
|
||||||
|
// state.rear_light_state.mode; status_msg.rear_light_state.custom_value =
|
||||||
|
// state.front_light_state.custom_value;
|
||||||
|
|
||||||
|
status_publisher_.publish(status_msg);
|
||||||
|
|
||||||
|
// publish odometry and tf
|
||||||
|
PublishOdometryToROS(linear, angular, dt);
|
||||||
|
|
||||||
|
// record time for next integration
|
||||||
|
last_time_ = current_time_;
|
||||||
}
|
}
|
||||||
|
|
||||||
void ScoutROSMessenger::PublishOdometryToROS(double linear, double angular, double dt)
|
void ScoutROSMessenger::PublishOdometryToROS(double linear, double angular,
|
||||||
{
|
double dt) {
|
||||||
// perform numerical integration to get an estimation of pose
|
// perform numerical integration to get an estimation of pose
|
||||||
linear_speed_ = linear;
|
linear_speed_ = linear;
|
||||||
angular_speed_ = angular;
|
angular_speed_ = angular;
|
||||||
|
|
||||||
double d_x = linear_speed_ * std::cos(theta_) * dt;
|
double d_x = linear_speed_ * std::cos(theta_) * dt;
|
||||||
double d_y = linear_speed_ * std::sin(theta_) * dt;
|
double d_y = linear_speed_ * std::sin(theta_) * dt;
|
||||||
double d_theta = angular_speed_ * dt;
|
double d_theta = angular_speed_ * dt;
|
||||||
|
|
||||||
position_x_ += d_x;
|
position_x_ += d_x;
|
||||||
position_y_ += d_y;
|
position_y_ += d_y;
|
||||||
theta_ += d_theta;
|
theta_ += d_theta;
|
||||||
|
|
||||||
geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(theta_);
|
geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(theta_);
|
||||||
|
|
||||||
// publish tf transformation
|
// publish tf transformation
|
||||||
geometry_msgs::TransformStamped tf_msg;
|
geometry_msgs::TransformStamped tf_msg;
|
||||||
tf_msg.header.stamp = current_time_;
|
tf_msg.header.stamp = current_time_;
|
||||||
tf_msg.header.frame_id = odom_frame_;
|
tf_msg.header.frame_id = odom_frame_;
|
||||||
tf_msg.child_frame_id = base_frame_;
|
tf_msg.child_frame_id = base_frame_;
|
||||||
|
|
||||||
tf_msg.transform.translation.x = position_x_;
|
tf_msg.transform.translation.x = position_x_;
|
||||||
tf_msg.transform.translation.y = position_y_;
|
tf_msg.transform.translation.y = position_y_;
|
||||||
tf_msg.transform.translation.z = 0.0;
|
tf_msg.transform.translation.z = 0.0;
|
||||||
tf_msg.transform.rotation = odom_quat;
|
tf_msg.transform.rotation = odom_quat;
|
||||||
|
|
||||||
tf_broadcaster_.sendTransform(tf_msg);
|
tf_broadcaster_.sendTransform(tf_msg);
|
||||||
|
|
||||||
// publish odometry and tf messages
|
// publish odometry and tf messages
|
||||||
nav_msgs::Odometry odom_msg;
|
nav_msgs::Odometry odom_msg;
|
||||||
odom_msg.header.stamp = current_time_;
|
odom_msg.header.stamp = current_time_;
|
||||||
odom_msg.header.frame_id = odom_frame_;
|
odom_msg.header.frame_id = odom_frame_;
|
||||||
odom_msg.child_frame_id = base_frame_;
|
odom_msg.child_frame_id = base_frame_;
|
||||||
|
|
||||||
odom_msg.pose.pose.position.x = position_x_;
|
odom_msg.pose.pose.position.x = position_x_;
|
||||||
odom_msg.pose.pose.position.y = position_y_;
|
odom_msg.pose.pose.position.y = position_y_;
|
||||||
odom_msg.pose.pose.position.z = 0.0;
|
odom_msg.pose.pose.position.z = 0.0;
|
||||||
odom_msg.pose.pose.orientation = odom_quat;
|
odom_msg.pose.pose.orientation = odom_quat;
|
||||||
|
|
||||||
odom_msg.twist.twist.linear.x = linear_speed_;
|
odom_msg.twist.twist.linear.x = linear_speed_;
|
||||||
odom_msg.twist.twist.linear.y = 0.0;
|
odom_msg.twist.twist.linear.y = 0.0;
|
||||||
odom_msg.twist.twist.angular.z = angular_speed_;
|
odom_msg.twist.twist.angular.z = angular_speed_;
|
||||||
|
|
||||||
odom_publisher_.publish(odom_msg);
|
odom_publisher_.publish(odom_msg);
|
||||||
}
|
}
|
||||||
} // namespace westonrobot
|
} // namespace westonrobot
|
||||||
@@ -1,72 +0,0 @@
|
|||||||
<!--
|
|
||||||
Copyright 2016 The Cartographer Authors
|
|
||||||
|
|
||||||
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.
|
|
||||||
|
|
||||||
Reference:
|
|
||||||
[1] https://github.com/IntelRealSense/librealsense/blob/master/doc/d435i.md
|
|
||||||
|
|
||||||
-->
|
|
||||||
|
|
||||||
<robot name="agilex_scout_rs16">
|
|
||||||
<material name="orange">
|
|
||||||
<color rgba="1.0 0.5 0.2 1" />
|
|
||||||
</material>
|
|
||||||
<material name="gray">
|
|
||||||
<color rgba="0.2 0.2 0.2 1" />
|
|
||||||
</material>
|
|
||||||
|
|
||||||
<!-- Robosense16 -->
|
|
||||||
<link name="rslidar">
|
|
||||||
<visual>
|
|
||||||
<origin xyz="0.0 0.0 0.0" />
|
|
||||||
<geometry>
|
|
||||||
<cylinder length="0.07" radius="0.05" />
|
|
||||||
</geometry>
|
|
||||||
<material name="orange" />
|
|
||||||
</visual>
|
|
||||||
</link>
|
|
||||||
|
|
||||||
<!-- Realsense D435i -->
|
|
||||||
<link name="imu_link">
|
|
||||||
<visual>
|
|
||||||
<origin xyz="0.0 0.0 0.0" />
|
|
||||||
<geometry>
|
|
||||||
<box size="0.06 0.04 0.02" />
|
|
||||||
</geometry>
|
|
||||||
<material name="orange" />
|
|
||||||
</visual>
|
|
||||||
</link>
|
|
||||||
|
|
||||||
<link name="base_link" />
|
|
||||||
|
|
||||||
<joint name="rslidar_joint" type="fixed">
|
|
||||||
<parent link="base_link" />
|
|
||||||
<child link="rslidar" />
|
|
||||||
<origin xyz="0.25 0.0 0.29" rpy="0 0 0" />
|
|
||||||
</joint>
|
|
||||||
|
|
||||||
<joint name="imu_joint" type="fixed">
|
|
||||||
<parent link="base_link" />
|
|
||||||
<child link="imu_link" />
|
|
||||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
|
||||||
</joint>
|
|
||||||
|
|
||||||
<!--
|
|
||||||
<joint name="imu_joint" type="fixed">
|
|
||||||
<parent link="base_link" />
|
|
||||||
<child link="camera_imu_optical_frame" />
|
|
||||||
<origin xyz="0 0 0.0" rpy="-1.57 0 -1.57" />
|
|
||||||
</joint>
|
|
||||||
-->
|
|
||||||
</robot>
|
|
||||||
Reference in New Issue
Block a user