Skip to content
Snippets Groups Projects
Commit 49f3ab22 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Changes to use the CPP darwin simulator with the localization algorithm.

parent 18e4655d
No related branches found
No related tags found
1 merge request!8Kinetic migration
Hide whitespace changes
Inline Side-by-side
Showing
with 323 additions and 196 deletions
darwin_control/config/darwin_cpp.yaml
+ 2
− 2
View file @ 49f3ab22
......@@ -5,8 +5,8 @@ darwin:
publish_rate: 50
darwin_controller_cpp:
dyn_serial_device: '/dev/pts/19'
imu_serial_device: '/dev/pts/22'
dyn_serial_device: '/dev/pts/11'
imu_serial_device: '/dev/pts/25'
type: effort_controllers/DarwinControllerCPP
joints:
- j_shoulder_pitch_r
......
darwin_description/config/darwin_rviz.rviz
+ 292
− 182
View file @ 49f3ab22
......@@ -30,6 +30,8 @@ Panels:
Name: Time
SyncMode: 0
SyncSource: ""
Toolbars:
toolButtonStyle: 2
Visualization Manager:
Class: ""
Displays:
......@@ -104,46 +106,6 @@ Visualization Manager:
Show Axes: false
Show Trail: false
Value: true
left_down_left_front_ir_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
left_down_left_middle_ir_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
left_down_left_rear_ir_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
left_down_right_front_ir_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
left_down_right_middle_ir_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
left_down_right_rear_ir_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
left_front_left_ir_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
left_front_right_ir_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
left_leg_ankle_pitch:
Alpha: 1
Show Axes: false
......@@ -194,46 +156,6 @@ Visualization Manager:
Show Axes: false
Show Trail: false
Value: true
right_down_left_front_ir_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
right_down_left_middle_ir_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
right_down_left_rear_ir_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
right_down_right_front_ir_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
right_down_right_middle_ir_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
right_down_right_rear_ir_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
right_front_left_ir_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
right_front_right_ir_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
right_leg_ankle_pitch:
Alpha: 1
Show Axes: false
......@@ -280,6 +202,58 @@ Visualization Manager:
Frame Timeout: 15
Frames:
All Enabled: true
E125:
Value: true
E175:
Value: true
E225:
Value: true
E25:
Value: true
E75:
Value: true
N125:
Value: true
N125_cam:
Value: true
N125_det:
Value: true
N175:
Value: true
N175_cam:
Value: true
N175_det:
Value: true
N25:
Value: true
N25_cam:
Value: true
N25_det:
Value: true
N75:
Value: true
N75_cam:
Value: true
N75_det:
Value: true
S125:
Value: true
S175:
Value: true
S25:
Value: true
S75:
Value: true
W125:
Value: true
W175:
Value: true
W225:
Value: true
W25:
Value: true
W75:
Value: true
darwin/base_footprint:
Value: true
darwin/base_link:
......@@ -300,22 +274,6 @@ Visualization Manager:
Value: true
darwin/left_arm_low:
Value: true
darwin/left_down_left_front_ir_link:
Value: true
darwin/left_down_left_middle_ir_link:
Value: true
darwin/left_down_left_rear_ir_link:
Value: true
darwin/left_down_right_front_ir_link:
Value: true
darwin/left_down_right_middle_ir_link:
Value: true
darwin/left_down_right_rear_ir_link:
Value: true
darwin/left_front_left_ir_link:
Value: true
darwin/left_front_right_ir_link:
Value: true
darwin/left_leg_ankle_pitch:
Value: true
darwin/left_leg_ankle_roll:
......@@ -338,35 +296,63 @@ Visualization Manager:
Value: true
darwin/right_arm_low:
Value: true
darwin/right_down_left_front_ir_link:
darwin/right_leg_ankle_pitch:
Value: true
darwin/right_down_left_middle_ir_link:
darwin/right_leg_ankle_roll:
Value: true
darwin/right_down_left_rear_ir_link:
darwin/right_leg_knee:
Value: true
darwin/right_down_right_front_ir_link:
darwin/right_leg_pelvis_pitch:
Value: true
darwin/right_down_right_middle_ir_link:
darwin/right_leg_pelvis_roll:
Value: true
darwin/right_down_right_rear_ir_link:
darwin/right_leg_pelvis_yaw:
Value: true
darwin/right_front_left_ir_link:
darwin/right_shoulder:
Value: true
darwin/right_front_right_ir_link:
obstacle_base_fence_link:
Value: true
darwin/right_leg_ankle_pitch:
obstacle_base_link:
Value: true
darwin/right_leg_ankle_roll:
obstacles_env_empty/E125:
Value: true
darwin/right_leg_knee:
obstacles_env_empty/E175:
Value: true
darwin/right_leg_pelvis_pitch:
obstacles_env_empty/E225:
Value: true
darwin/right_leg_pelvis_roll:
obstacles_env_empty/E25:
Value: true
darwin/right_leg_pelvis_yaw:
obstacles_env_empty/E75:
Value: true
darwin/right_shoulder:
obstacles_env_empty/N125:
Value: true
obstacles_env_empty/N175:
Value: true
obstacles_env_empty/N25:
Value: true
obstacles_env_empty/N75:
Value: true
obstacles_env_empty/S125:
Value: true
obstacles_env_empty/S175:
Value: true
obstacles_env_empty/S25:
Value: true
obstacles_env_empty/S75:
Value: true
obstacles_env_empty/W125:
Value: true
obstacles_env_empty/W175:
Value: true
obstacles_env_empty/W225:
Value: true
obstacles_env_empty/W25:
Value: true
obstacles_env_empty/W75:
Value: true
obstacles_env_empty/obstacle_base_fence_link:
Value: true
obstacles_env_empty/obstacle_base_link:
Value: true
Marker Scale: 0.200000003
Name: TF
......@@ -374,72 +360,96 @@ Visualization Manager:
Show Axes: true
Show Names: true
Tree:
darwin/odom:
darwin/base_footprint:
darwin/base_link:
darwin/darwin_accel:
{}
darwin/darwin_gyro:
{}
darwin/darwin_imu_link:
{}
darwin/left_leg_pelvis_yaw:
darwin/left_leg_pelvis_roll:
darwin/left_leg_pelvis_pitch:
darwin/left_leg_knee:
darwin/left_leg_ankle_pitch:
darwin/left_leg_ankle_roll:
darwin/left_down_left_front_ir_link:
{}
darwin/left_down_left_middle_ir_link:
{}
darwin/left_down_left_rear_ir_link:
{}
darwin/left_down_right_front_ir_link:
{}
darwin/left_down_right_middle_ir_link:
{}
darwin/left_down_right_rear_ir_link:
obstacle_base_link:
E125:
{}
E175:
{}
E225:
{}
E25:
{}
E75:
{}
N125:
{}
N175:
{}
N25:
{}
N75:
{}
S125:
{}
S175:
{}
S25:
{}
S75:
{}
W125:
{}
W175:
{}
W225:
{}
W25:
{}
W75:
{}
darwin/odom:
darwin/base_footprint:
darwin/base_link:
darwin/darwin_accel:
{}
darwin/darwin_gyro:
{}
darwin/darwin_imu_link:
{}
darwin/left_leg_pelvis_yaw:
darwin/left_leg_pelvis_roll:
darwin/left_leg_pelvis_pitch:
darwin/left_leg_knee:
darwin/left_leg_ankle_pitch:
darwin/left_leg_ankle_roll:
{}
darwin/left_front_left_ir_link:
{}
darwin/left_front_right_ir_link:
{}
darwin/left_shoulder:
darwin/left_arm_high:
darwin/left_arm_low:
{}
darwin/neck:
darwin/head:
darwin/darwin_cam_link:
darwin/darwin_cam_optical_link:
darwin/left_shoulder:
darwin/left_arm_high:
darwin/left_arm_low:
{}
darwin/right_leg_pelvis_yaw:
darwin/right_leg_pelvis_roll:
darwin/right_leg_pelvis_pitch:
darwin/right_leg_knee:
darwin/right_leg_ankle_pitch:
darwin/right_leg_ankle_roll:
darwin/right_down_left_front_ir_link:
{}
darwin/right_down_left_middle_ir_link:
{}
darwin/right_down_left_rear_ir_link:
{}
darwin/right_down_right_front_ir_link:
{}
darwin/right_down_right_middle_ir_link:
{}
darwin/right_down_right_rear_ir_link:
darwin/neck:
darwin/head:
darwin/darwin_cam_link:
darwin/darwin_cam_optical_link:
N125_cam:
{}
N125_det:
{}
N175_cam:
{}
N175_det:
{}
N25_cam:
{}
N25_det:
{}
N75_cam:
{}
N75_det:
{}
darwin/right_leg_pelvis_yaw:
darwin/right_leg_pelvis_roll:
darwin/right_leg_pelvis_pitch:
darwin/right_leg_knee:
darwin/right_leg_ankle_pitch:
darwin/right_leg_ankle_roll:
{}
darwin/right_front_left_ir_link:
{}
darwin/right_front_right_ir_link:
{}
darwin/right_shoulder:
darwin/right_arm_high:
darwin/right_arm_low:
{}
darwin/right_shoulder:
darwin/right_arm_high:
darwin/right_arm_low:
{}
obstacle_base_fence_link:
{}
Update Interval: 0
Value: true
- Alpha: 1
......@@ -448,12 +458,112 @@ Visualization Manager:
Enabled: true
Links:
All Links Enabled: true
E125:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
E175:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
E225:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
E25:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
E75:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
Link Tree Style: Links in Alphabetic Order
N125:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
N175:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
N25:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
N75:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
S125:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
S175:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
S25:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
S75:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
W125:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
W175:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
W225:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
W25:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
W75:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
obstacle_base_fence_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
obstacle_base_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Name: Environment
Robot Description: /charge_env/robot_description
Robot Description: /obstacles_env_empty/robot_description
TF Prefix: ""
Update Interval: 0
Value: true
......@@ -462,7 +572,7 @@ Visualization Manager:
Global Options:
Background Color: 48; 48; 48
Default Light: true
Fixed Frame: darwin/base_footprint
Fixed Frame: obstacle_base_link
Frame Rate: 30
Name: root
Tools:
......@@ -483,25 +593,25 @@ Visualization Manager:
Views:
Current:
Class: rviz/Orbit
Distance: 2.83055544
Distance: 11.0277748
Enable Stereo Rendering:
Stereo Eye Separation: 0.0599999987
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 0.174162328
Y: -0.0848779902
Z: -0.0313329101
X: 1.99065769
Y: 0.666042745
Z: 1.47241056
Focal Shape Fixed Size: true
Focal Shape Size: 0.0500000007
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.00999999978
Pitch: 0.130199939
Pitch: 0.325200558
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 3.13725877
Yaw: 2.7204318
Saved: ~
Window Geometry:
Displays:
......
darwin_description/launch/darwin_sim.launch
+ 5
− 0
View file @ 49f3ab22
......@@ -10,5 +10,10 @@
<arg name="robot" value="$(arg robot)" />
</include>
<include file="$(find darwin_driver)/launch/darwin.launch">
<arg name="device" value="/dev/pts/6"/>
<arg name="simulate" value="True"/>
</include>
</launch>
darwin_description/launch/charge_env.launch → darwin_description/launch/environment.launch
+ 3
− 3
View file @ 49f3ab22
......@@ -4,12 +4,12 @@
<group ns="$(arg environment)">
<!-- Convert an xacro and put on parameter server -->
<param name="robot_description"
command="$(find xacro)/xacro --inorder '$(find darwin_description)/urdf/charge/$(arg environment).xacro'" />
command="$(find xacro)/xacro --inorder '$(find bioloid_description)/urdf/ceabot/$(arg environment).xacro'" />
<!-- <node pkg="robot_state_publisher" type="state_publisher" name="env_tf_broadcaster">
<node pkg="robot_state_publisher" type="state_publisher" name="env_tf_broadcaster">
<param name="tf_prefix" type="string" value="/$(arg environment)"/>
<param name="publish_frequency" type="double" value="100.0"/>
</node>-->
</node>
<node name="spawn_charge_station" pkg="gazebo_ros" type="spawn_model" args="-param /$(arg environment)/robot_description -urdf -model $(arg environment) -x 1.0 -y 0.25 -z 0.03 -R 0 -P 0 -Y 3.14159"/>
</group>
......
darwin_description/urdf/darwin.gazebo
+ 2
− 0
View file @ 49f3ab22
......@@ -357,6 +357,8 @@
<gazebo>
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
<robotNamespace>/darwin</robotNamespace>
<robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
<legacyModeNS>true</legacyModeNS>
<controlPeriod>0.0078</controlPeriod>
</plugin>
</gazebo>
......
darwin_driver/cfg/DarwinDriver.cfg
+ 1
− 0
View file @ 49f3ab22
......@@ -43,6 +43,7 @@ gen = ParameterGenerator()
gen.add("darwin_serial", str_t, SensorLevels.RECONFIGURE_STOP, "Darwin serial device serial number", "A4008atn")
gen.add("darwin_id", int_t, SensorLevels.RECONFIGURE_STOP, "Darwin identifier", 2, 0, 254)
gen.add("darwin_baudrate", int_t, SensorLevels.RECONFIGURE_STOP, "Darwin serial baudrate", 1000000, 9600, 3000000)
gen.add("simulate", bool_t, SensorLevels.RECONFIGURE_STOP, "Use driver in simualtion mode", False)
gen.add("cmd_vel_timeout", double_t, SensorLevels.RECONFIGURE_STOP, "Maximum time between cmd_vel messgaes",1.0, 0.1, 5)
exit(gen.generate(PACKAGE, "DarwinDriverDriver", "DarwinDriver"))
darwin_driver/launch/darwin.launch
+ 6
− 1
View file @ 49f3ab22
<launch>
<arg name="device" default="/dev/pts/22"/>
<arg name="simulate" default="False"/>
<node name="darwin"
pkg="darwin_driver"
type="darwin_driver"
output="screen">
<param name="darwin_serial" value="A603LOBS"/>
<param name="darwin_serial" value="$(arg device)"/>
<param name="darwin_baudrate" value="1000000"/>
<param name="darwin_id" value="2"/>
<param name="simulate" value="$(arg simulate)"/>
</node>
</launch>
darwin_driver/src/darwin_driver.cpp
+ 5
− 1
View file @ 49f3ab22
......@@ -16,7 +16,11 @@ bool DarwinDriver::openDriver(void)
delete this->darwin;
this->darwin=NULL;
}
this->darwin=new CDarwinRobot("Darwin",this->config_.darwin_serial,this->config_.darwin_baudrate,this->config_.darwin_id);
if(this->config_.simulate)
std::cout << "using simulation mode" << std::endl;
else
std::cout << "using real mode" << std::endl;
this->darwin=new CDarwinRobot("Darwin",this->config_.darwin_serial,this->config_.darwin_baudrate,this->config_.darwin_id,this->config_.simulate);
// get the current walk parameters
this->walk_params.x_offset=this->darwin->walk_get_x_offset();
this->walk_params.y_offset=this->darwin->walk_get_y_offset();
......
darwin_nav/launch/darwin_global_loc_sim.launch
+ 3
− 3
View file @ 49f3ab22
......@@ -7,7 +7,7 @@
output="screen">
<param name="qr_x" value="0.14"/>
<param name="qr_y" value="0.14"/>
<param name="camera_frame" value="/darwin/darwin_cam_link"/>
<param name="camera_frame" value="/darwin/darwin_cam_optical_link"/>
<remap from="~/camera/image_raw" to="/darwin/camera/image_raw"/>
<remap from="~/camera/camera_info" to="/darwin/camera/camera_info"/>
</node>
......@@ -16,11 +16,11 @@
name="qr_global_loc"
type="qr_global_loc"
output="screen">
<param name="qr_map_param" value="/charge_env/robot_description"/>
<param name="qr_map_param" value="/obstacles_env_empty/robot_description"/>
<param name="tf_prefix" value="/darwin"/>
<param name="publish_tf" value="True"/>
<param name="publish_rate" value="20"/>
<remap from="~/qr_pose" to="/darwin/qr_detector/qr_pose"/>
<remap from="~/qr_pose" to="/darwin/qr_pose"/>
</node>
</group>
......
darwin_nav/launch/global_loc_sim.launch
+ 4
− 4
View file @ 49f3ab22
<?xml version="1.0" encoding="UTF-8"?>
<launch>
<arg name="robot" default="darwin" />
<arg name="robot" default="darwin_cpp" />
<arg name="environment" default="obstacles_env_empty" />
<include file="$(find darwin_description)/launch/darwin_sim.launch">
<arg name="robot" value="$(arg robot)" />
</include>
<include file="$(find bioloid_description)/launch/obstacles_env.launch">
<include file="$(find darwin_description)/launch/environment.launch">
<arg name="environment" value="$(arg environment)" />
</include>
......@@ -21,7 +21,7 @@
</node>
<!-- launch the walk client node -->
<node name="walk_client"
<!-- <node name="walk_client"
pkg="walk_client"
type="walk_client"
output="screen"
......@@ -49,7 +49,7 @@
to="/darwin/robot/set_walk_params"/>
<remap from="/darwin/walk_client/set_servo_modules"
to="/darwin/robot/set_servo_modules"/>
</node>
</node>-->
<!-- launch dynamic reconfigure -->
<node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" respawn="false"
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment