Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Nuc #54

Open
wants to merge 8 commits into
base: icra2019
Choose a base branch
from
Open

Nuc #54

Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
439 changes: 439 additions & 0 deletions externals/UartDemo.cpp

Large diffs are not rendered by default.

11 changes: 6 additions & 5 deletions roborts_base/chassis/chassis.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -127,18 +127,19 @@ void Chassis::UWBInfoCallback(const std::shared_ptr<roborts_sdk::cmd_uwb_info> u

void Chassis::ChassisSpeedCtrlCallback(const geometry_msgs::Twist::ConstPtr &vel){
roborts_sdk::cmd_chassis_speed chassis_speed;
chassis_speed.vx = vel->linear.x*1000;
chassis_speed.vy = vel->linear.y*1000;
chassis_speed.vw = vel->angular.z * 1800.0 / M_PI;
//int chassis_speed_ratio = 1; // Adjust chassis's speed limit
chassis_speed.vx = vel->linear.x*1000; //* chassis_speed_ratio;
chassis_speed.vy = vel->linear.y*1000; //* chassis_speed_ratio;
chassis_speed.vw = vel->angular.z * 1800.0 / M_PI; //* chassis_speed_ratio;
chassis_speed.rotate_x_offset = 0;
chassis_speed.rotate_y_offset = 0;
chassis_speed_pub_->Publish(chassis_speed);
}

void Chassis::ChassisSpeedAccCtrlCallback(const roborts_msgs::TwistAccel::ConstPtr &vel_acc){
roborts_sdk::cmd_chassis_spd_acc chassis_spd_acc;
chassis_spd_acc.vx = vel_acc->twist.linear.x*1000;
chassis_spd_acc.vy = vel_acc->twist.linear.y*1000;
chassis_spd_acc.vx = vel_acc->twist.linear.x*500;
chassis_spd_acc.vy = vel_acc->twist.linear.y*500;
chassis_spd_acc.vw = vel_acc->twist.angular.z * 1800.0 / M_PI;
chassis_spd_acc.ax = vel_acc->accel.linear.x*1000;
chassis_spd_acc.ay = vel_acc->accel.linear.y*1000;
Expand Down
2 changes: 1 addition & 1 deletion roborts_bringup/launch/mapping_stage.launch
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
<launch>
<master auto="start"/>
<param name="/use_sim_time" value="true"/>
<arg name="map" value="icra2019"/>
<arg name="map" value="icra2020"/>

<!-- Run the Stage Simulator -->
<node pkg="stage_ros" type="stageros" name="stageros" args="$(find roborts_bringup)/worlds/$(arg map).world" respawn="false" >
Expand Down
8 changes: 4 additions & 4 deletions roborts_bringup/launch/roborts.launch
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
<launch>
<master auto="start"/>
<arg name="map" value="icra2019"/>
<arg name="map" value="icra2020"/>

<!--Load parameters for localization node -->
<rosparam command="load" file="$(find roborts_localization)/config/localization.yaml" />
Expand All @@ -16,8 +16,7 @@
<node name="map_server" pkg="map_server" type="map_server" args="$(find roborts_bringup)/maps/$(arg map).yaml" respawn="false" />

<!-- Run the lidar node -->
<node name="rplidar_node" pkg="rplidar_ros" type="rplidarNode" output="screen">
<param name="serial_port" type="string" value="/dev/rplidar"/>
<node name="urg_node" pkg="urg_node" type="urg_node" output="screen">
<param name="serial_baudrate" type="int" value="115200"/>
<param name="frame_id" type="string" value="base_laser_link"/>
<param name="inverted" type="bool" value="false"/>
Expand All @@ -32,6 +31,7 @@

<!-- Run the local planner node -->
<node pkg="roborts_planning" type="local_planner_node" name="local_planner_node" respawn="false" />

<!-- Run the rviz for realtime debug -->
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find roborts_bringup)/rviz/roborts.rviz" />
</launch>

2 changes: 1 addition & 1 deletion roborts_bringup/launch/roborts_stage.launch
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
<launch>
<arg name="map" value="icra2019"/>
<arg name="map" value="icra2020"/>
<master auto="start"/>
<param name="/use_sim_time" value="true"/>

Expand Down
8 changes: 5 additions & 3 deletions roborts_bringup/launch/static_tf.launch
Original file line number Diff line number Diff line change
Expand Up @@ -2,9 +2,11 @@

<!--static_transform_publisher x y z yaw pitch roll frame_id child_frame_id-->
<node pkg="tf2_ros" type="static_transform_publisher" name="base_link_base_laser_link_broadcaster"
args="0.15 0.0 0.05 1.57 0.0 0.0 base_link base_laser_link" />
args="0.0 0.0 0.0 0.0 0.0 0.0 base_link base_laser_link" />

<node pkg="tf2_ros" type="static_transform_publisher" name="base_link_camera_link_broadcaster"
args="0.0 0.0 0.0 0.0 0.0 0.0 base_link camera" />

</launch>

<node pkg="tf2_ros" type="static_transform_publisher" name="odom_map_link_broadcaster"
args="0.0 0.0 0.0 0.0 0.0 0.0 map odom" />
</launch>
Binary file added roborts_bringup/maps/icra2020.pgm
Binary file not shown.
7 changes: 7 additions & 0 deletions roborts_bringup/maps/icra2020.yaml
Original file line number Diff line number Diff line change
@@ -0,0 +1,7 @@
image: icra2020.pgm
resolution: 0.04
origin: [0, 0, 0.000000]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196

87 changes: 87 additions & 0 deletions roborts_bringup/worlds/icra2020.world
Original file line number Diff line number Diff line change
@@ -0,0 +1,87 @@
define block model
(
size [0.4 0.4 1.2]
ranger_return 1
)

define frontcamera camera
(
size [ 0.050 0.050 0.0500 ]
range [ 0.301 8.0 ]
resolution [ 640 480 ]
fov [ 120 40 ]
pantilt [ 0 0 ]
alwayson 1
)

define rplidar ranger
(
sensor(
range_max 8.0
fov 270
samples 270
)
# generic model properties
color "black"
size [ 0.050 0.050 0.100 ]
)

define rmcar position
(
size [0.6 0.45 0.460]
origin [0 0 0 0]
gui_nose 1
drive "omni"
# frontcamera(pose [ 0 0 0 0 ])
rplidar(pose [ 0 0 0 0 ])
odom_error [0.03 0.03 0.00 0.05]
# [ xmin xmax ymin ymax zmin zmax amin amax ]
velocity_bounds [-2 2 -2 2 -2 2 -90 90 ]
acceleration_bounds [-2 2 -2 2 -2 2 -90 90]
ranger_return 1
)

define floorplan model
(
# sombre, sensible, artistic
color "gray30"

# most maps will need a bounding box
boundary 0

gui_nose 0
gui_grid 0

gui_outline 0
gripper_return 0
fiducial_return 0
ranger_return 1
)

# set the resolution of the underlying raytrace model in meters
resolution 0.01

interval_sim 50#83 # simulation timestep in milliseconds
interval_real 50#83

window
(
size [ 745.000 448.000 ]
rotate [ 0 0 ]
scale 29
)

# load an environment bitmap
floorplan
(
name "RoboMaster Map"
bitmap "../maps/icra2020.pgm"
size [8.2 4.5 0.500]
pose [4.1 2.25 0 0 ]
#pose [0 0 0 0 ]
)

# throw in a robot
rmcar(pose [ 1 1 0 3.14 ] name "rmcar0" color "blue" )
# throw in a block for test
block(pose [ 7.79 3.45 0 0.3 ] color "red" )
6 changes: 6 additions & 0 deletions roborts_camera/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,7 @@ catkin_package(
)

add_subdirectory(uvc)
add_subdirectory(mvc)

#camera parameter
file(GLOB CameraParamProtoFiles "${CMAKE_CURRENT_SOURCE_DIR}/proto/camera_param.proto")
Expand Down Expand Up @@ -59,9 +60,11 @@ add_executable(${PROJECT_NAME}_node
target_link_libraries(${PROJECT_NAME}_node
PRIVATE
driver::uvc_driver
driver::mvc_driver
roborts_camera_param
${catkin_LIBRARIES}
${OpenCV_LIBRARIES}
libMVSDK.so
)

target_include_directories(${PROJECT_NAME}_node
Expand All @@ -78,10 +81,13 @@ add_executable(image_capture_test

target_link_libraries(image_capture_test
PRIVATE
PUBLIC
driver::uvc_driver
driver::mvc_driver
roborts_camera_param
${catkin_LIBRARIES}
${OpenCV_LIBRARIES}
libMVSDK.so
)

target_include_directories(image_capture_test
Expand Down
1 change: 1 addition & 0 deletions roborts_camera/camera_node.h
Original file line number Diff line number Diff line change
Expand Up @@ -27,6 +27,7 @@
#include <image_transport/image_transport.h>

#include "uvc/uvc_driver.h"
#include "mvc/mvc_driver.h"

#include "camera_param.h"
#include "camera_base.h"
Expand Down
5 changes: 4 additions & 1 deletion roborts_camera/camera_param.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -62,7 +62,10 @@ void CameraParam::LoadCameraParam() {

//constrast
cameras_param_[index].contrast = camera_info.camera(index).contrast();

// Gamma value
cameras_param_[index].gamma = camera_info.camera(index).gamma();
// Gain value
cameras_param_[index].gain = camera_info.camera(index).gain();
//camera matrix
int camera_m_size = camera_info.camera(index).camera_matrix().data().size();
double camera_m[camera_m_size];
Expand Down
4 changes: 3 additions & 1 deletion roborts_camera/camera_param.h
Original file line number Diff line number Diff line change
Expand Up @@ -56,13 +56,15 @@ struct CameraInfo {
//! exposure value
unsigned int exposure_value;
//! exposure time
unsigned int exposure_time;
double exposure_time;
//! auto white balance
bool auto_white_balance;
//! auto gain
bool auto_gain;
//! contrast
unsigned int contrast;
unsigned int gamma;
unsigned int gain;

//! camera information in form of ROS sensor_msgs
sensor_msgs::CameraInfoPtr ros_camera_info;
Expand Down
35 changes: 19 additions & 16 deletions roborts_camera/config/camera_param.prototxt
Original file line number Diff line number Diff line change
@@ -1,41 +1,44 @@
camera: {
camera_name: "back_camera"
camera_type: "uvc"
camera_path: "/dev/video0"
camera_type: "mvc"
camera_path: "/dev/video3"

camera_matrix {
data: 839.923052
data: 737.13167516
data: 0.0
data: 340.780730
data: 394.02392383
data: 0.0
data: 837.671081
data: 261.766523
data: 753.8582612
data: 236.24307876
data: 0.0
data: 0.0
data: 1.0
}

camera_distortion {
data: 0.082613
data: 0.043275
data: 0.002486
data: -0.000823
data: 0.0
data: 0.26802427
data: -0.98025777
data: -0.00674122
data: 0.01944969
data: 1.995706
}

resolution {
width: 640
height: 360
height: 480

width_offset: 0
height_offset: 0
}

fps: 100
auto_exposure: 1
fps: 110
auto_exposure: 0
exposure_value: 70
exposure_time: 10
auto_white_balance: 1
exposure_time: 900
auto_white_balance: 0
gamma: 65
contrast: 44
auto_gain: 0
gain: 48
}

Binary file added roborts_camera/libMVSDK.so
Binary file not shown.
21 changes: 21 additions & 0 deletions roborts_camera/mvc/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -0,0 +1,21 @@
project(mvc_driver)

#mvc_driver
add_library(mvc_driver
SHARED
mvc_driver.cpp
)

target_link_libraries(mvc_driver
PRIVATE
${catkin_LIBRARIES}
${OpenCV_LIBRARIES}
)

target_include_directories(mvc_driver
PRIVATE
${catkin_INCLUDE_DIRS}
${OpenCV_INCLUDE_DIRECTORIES}
)

add_library(driver::mvc_driver ALIAS mvc_driver)
Loading