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

resovled conflicts

Browse Source
This commit is contained in:
Ruixiang Du
2020-08-21 16:50:53 +08:00
parent 9dfc9af81c 61824fb4c8
commit dc3654ed8e
82 changed files with 8838 additions and 130 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
scout_base/CMakeLists.txt
View File

@@ -51,15 +51,11 @@ set_property(TARGET scout_messenger PROPERTY POSITION_INDEPENDENT_CODE ON)
add_executable(scout_base_node src/scout_base_node.cpp)
target_link_libraries(scout_base_node scout_messenger ${catkin_LIBRARIES})
add_executable(scout_base_sim_node src/scout_base_sim_node.cpp)
target_link_libraries(scout_base_sim_node scout_messenger ${catkin_LIBRARIES})
#############
## Install ##
#############
roslaunch_add_file_check(launch)
# roslaunch_add_file_check(launch)
install(TARGETS scout_messenger scout_base_node
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}

2
scout_base/launch/scout_base_sim.launch
View File

@@ -12,7 +12,7 @@
<arg name="simulated_robot" default="true" />
<arg name="control_rate" default="50" />
<node name="scout_base_sim_node" pkg="scout_base" type="scout_base_sim_node" output="screen">
<node name="scout_base_node" pkg="scout_base" type="scout_base_node" output="screen">
<param name="simulated_robot" type="bool" value="$(arg simulated_robot)" />
<param name="control_rate" type="int" value="$(arg control_rate)" />

72
scout_base/src/imu_broadcast_node.cpp
View File

@@ -1,72 +0,0 @@
/*
* imu_broadcast_node.cpp
*
* Created on: Oct 02, 2019 19:09
* Description:
*
* Copyright (c) 2019 Ruixiang Du (rdu)
*/
#include <string>
#include <lcm/lcm-cpp.hpp>
#include "lcmtypes/wescore.hpp"
#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
ros::Publisher imu_pub;
struct MessageBroadcaster
{
void IMULCMCallback(const lcm::ReceiveBuffer *rbuf, const std::string &chan, const wescore_lcm_msgs::IMU *msg)
{
static uint64_t count = 0;
// std::cout << "imu msg received" << std::endl;
sensor_msgs::Imu imu_msg;
imu_msg.header.frame_id = "imu_link";
imu_msg.header.stamp = ros::Time::now();
imu_msg.header.seq = count++;
imu_msg.angular_velocity.x = msg->angular_velocity.x;
imu_msg.angular_velocity.y = msg->angular_velocity.y;
imu_msg.angular_velocity.z = msg->angular_velocity.z;
imu_msg.linear_acceleration.x = msg->linear_acceleration.x;
imu_msg.linear_acceleration.y = msg->linear_acceleration.y;
imu_msg.linear_acceleration.z = msg->linear_acceleration.z;
for (int i = 0; i < 9; ++i)
{
imu_msg.orientation_covariance[i] = msg->orientation_covariance[i];
imu_msg.angular_velocity_covariance[i] = msg->angular_velocity_covariance[i];
imu_msg.linear_acceleration_covariance[i] = msg->linear_acceleration_covariance[i];
}
imu_pub.publish(imu_msg);
}
};
int main(int argc, char **argv)
{
// setup LCM
lcm::LCM lcm;
if (!lcm.good())
return 1;
MessageBroadcaster mb;
lcm.subscribe("sensor_hub_raw_imu", &MessageBroadcaster::IMULCMCallback, &mb);
// setup ROS node
ros::init(argc, argv, "imu_broadcast_node");
ros::NodeHandle nh;
imu_pub = nh.advertise<sensor_msgs::Imu>("imu", 1000);
ROS_INFO("Started broadcasting");
while (ros::ok())
{
lcm.handleTimeout(5);
}
return 0;
}

75
scout_base/src/scout_base_node.cpp
View File

@@ -1,53 +1,58 @@
#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 "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("~");
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);
// 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);
// 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);
private_node.param<int>("control_rate", messenger.sim_control_rate_, 50);
if (!messenger.simulated_robot_) {
// 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");
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");
}
else
{
robot.Connect(port_name, 115200);
ROS_INFO("Using UART to talk with the robot");
}
messenger.SetupSubscription();
}
messenger.SetupSubscription();
// publish robot state at 50Hz while listening to twist commands
ros::Rate rate_50hz(50); // 50Hz
while (true)
{
messenger.PublishStateToROS();
ros::spinOnce();
rate_50hz.sleep();
// publish robot state at 50Hz while listening to twist commands
ros::Rate rate(50);
while (true) {
if (!messenger.simulated_robot_) {
messenger.PublishStateToROS();
} else {
double linear, angular;
messenger.GetCurrentMotionCmdForSim(linear, angular);
messenger.PublishSimStateToROS(linear, angular);
}
ros::spinOnce();
rate.sleep();
}
return 0;
return 0;
}