mirrors/scout_ros
1
0
Fork 0
mirror of https://github.com/westonrobot/scout_ros.git synced 2026-01-12 11:20:02 +08:00
Code Issues Packages Projects Releases Wiki Activity

saved work

Browse Source
This commit is contained in:
Ruixiang Du
2020-03-26 19:06:34 +08:00
parent 38510eb257
commit dbf04d03d8
10 changed files with 151 additions and 57 deletions
Download Patch File Download Diff File Expand all files Collapse all files

50
scout_gazebo_sim/src/scout_skid_steer.cpp
View File

@@ -9,6 +9,52 @@
#include "scout_gazebo/scout_skid_steer.hpp"
#include <geometry_msgs/Twist.h>
#include <std_msgs/Float64.h>
namespace wescore {
}
ScoutSkidSteer::ScoutSkidSteer(ros::NodeHandle *nh, std::string robot_name)
: nh_(nh), robot_name_(robot_name) {
motor_fr_topic_ = robot_name_ + "/scout_motor_fr_controller/command";
motor_fl_topic_ = robot_name_ + "/scout_motor_fl_controller/command";
motor_rl_topic_ = robot_name_ + "/scout_motor_rl_controller/command";
motor_rr_topic_ = robot_name_ + "/scout_motor_rr_controller/command";
cmd_topic_ = robot_name_ + "/cmd_vel";
}
void ScoutSkidSteer::SetupSubscription() {
// command subscriber
cmd_sub_ = nh_->subscribe<geometry_msgs::Twist>(
cmd_topic_, 5, &ScoutSkidSteer::TwistCmdCallback, this);
// motor command publisher
motor_fr_pub_ = nh_->advertise<std_msgs::Float64>(motor_fr_topic_, 50);
motor_fl_pub_ = nh_->advertise<std_msgs::Float64>(motor_fl_topic_, 50);
motor_rl_pub_ = nh_->advertise<std_msgs::Float64>(motor_rl_topic_, 50);
motor_rr_pub_ = nh_->advertise<std_msgs::Float64>(motor_rr_topic_, 50);
}
void ScoutSkidSteer::TwistCmdCallback(
const geometry_msgs::Twist::ConstPtr &msg) {
std_msgs::Float64 motor_cmd[4];
double driving_vel = msg->linear.x;
double steering_vel = msg->angular.z;
double left_side_velocity =
(driving_vel - steering_vel * SCOUT_WHEELBASE) / SCOUT_WHEEL_RADIUS;
double right_side_velocity =
(driving_vel + steering_vel * SCOUT_WHEELBASE) / SCOUT_WHEEL_RADIUS;
motor_cmd[0].data = right_side_velocity;
motor_cmd[1].data = -left_side_velocity;
motor_cmd[2].data = -left_side_velocity;
motor_cmd[3].data = right_side_velocity;
motor_fr_pub_.publish(motor_cmd[0]);
motor_fl_pub_.publish(motor_cmd[1]);
motor_rl_pub_.publish(motor_cmd[2]);
motor_rr_pub_.publish(motor_cmd[3]);
}
} // namespace wescore

88
scout_gazebo_sim/src/scout_skid_steer_controller.cpp
View File

@@ -1,52 +1,64 @@
#include <string>
#include <nav_msgs/Odometry.h>
#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
#include <string>
#include "scout_gazebo/scout_skid_steer.hpp"
using namespace wescore;
int main(int argc, char **argv)
{
// setup ROS node
ros::init(argc, argv, "scout_odom");
ros::NodeHandle node(""), private_node("~");
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
std::string robot_namespace;
private_node.param<std::string>("robot_namespace", robot_namespace,
std::string("scout_default"));
// // 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_, false);
ROS_INFO("Namespace: %s", robot_namespace.c_str());
// // connect to robot and setup ROS subscription
// if (port_name.find("can") != std::string::npos)
// {
// robot.Connect(port_name);
// ROS_INFO("Using CAN bus to talk with the robot");
// }
// else
// {
// robot.Connect(port_name, 115200);
// ROS_INFO("Using UART to talk with the robot");
// }
// messenger.SetupSubscription();
ScoutSkidSteer skid_steer_controller(&node, robot_namespace);
skid_steer_controller.SetupSubscription();
// // publish robot state at 20Hz while listening to twist commands
// ros::Rate rate_20hz(20); // 20Hz
// while (true)
// {
// messenger.PublishStateToROS();
// ros::spinOnce();
// rate_20hz.sleep();
// }
ros::spin();
return 0;
// // 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_, false);
// // connect to robot and setup ROS subscription
// if (port_name.find("can") != std::string::npos)
// {
// robot.Connect(port_name);
// ROS_INFO("Using CAN bus to talk with the robot");
// }
// else
// {
// robot.Connect(port_name, 115200);
// ROS_INFO("Using UART to talk with the robot");
// }
// messenger.SetupSubscription();
// publish robot state at 20Hz while listening to twist commands
// ros::Rate rate_20hz(20); // 20Hz
// while (true) {
// messenger.PublishStateToROS();
// ros::spinOnce();
// rate_20hz.sleep();
// }
return 0;
}