HeeroYui-deprecated/nao_dcm
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Gazebo now works a little better. Small fixes in driver/camera.

Browse Source 
Also, I added 'Body' group in srdf files to be able to control all joints at once in MoveIt!
This commit is contained in:
Konstantinos Chatzilygeroudis 2014-05-12 20:00:44 +03:00
parent 298b01361b
commit 07d263ff23
12 changed files with 136 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
.gitignore vendored
View File

@ -1,2 +1,3 @@
nao_dcm_driver-build
CMakeLists.txt.user
CMakeLists.txt.user
*.pyc

12
nao_dcm_apps/nao_dcm_camera/src/nao_camera.cpp
View File

@ -228,6 +228,16 @@ void NaoCamera::loop(const ros::Time &time, const ros::Duration &period)
{
ROS_ERROR("ALError!\n\tTrace: %s",e.what());
}
try
{
video_proxy_.ping();
}
catch(const AL::ALError& e)
{
ROS_ERROR("Could not ping Video proxy.\n\tTrace: %s",e.what());
is_connected_ = false;
}
}
bool NaoCamera::connected()
@ -327,6 +337,8 @@ bool NaoCamera::switchCameraBottomState(nao_dcm_msgs::BoolService::Request &req,
void NaoCamera::dynamicReconfigureCb(nao_dcm_camera::NaoDCMCameraConfig &config, uint32_t level)
{
if(!is_connected_)
return;
reconfiguring_ = true;
// Change configurations if needed
if(camera_top_enabled_)

23
nao_dcm_apps/nao_dcm_moveit_config/config/nao_robot_v4_H21.srdf
View File

@ -42,6 +42,29 @@
<group name="Pelvis">
<joint name="LHipYawPitch" />
</group>
<group name="Body">
<joint name="HeadYaw" />
<joint name="HeadPitch" />
<joint name="LShoulderPitch" />
<joint name="LShoulderRoll" />
<joint name="LElbowYaw" />
<joint name="LElbowRoll" />
<joint name="RShoulderPitch" />
<joint name="RShoulderRoll" />
<joint name="RElbowYaw" />
<joint name="RElbowRoll" />
<joint name="LHipYawPitch" />
<joint name="LHipRoll" />
<joint name="LHipPitch" />
<joint name="LKneePitch" />
<joint name="LAnklePitch" />
<joint name="LAnkleRoll" />
<joint name="RHipRoll" />
<joint name="RHipPitch" />
<joint name="RKneePitch" />
<joint name="RAnklePitch" />
<joint name="RAnkleRoll" />
</group>
<!--VIRTUAL JOINT: Purpose: this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
<virtual_joint name="virtual_joint" type="floating" parent_frame="odom_combined" child_link="base_link" />
<!--PASSIVE JOINT: Purpose: this element is used to mark joints that are not actuated-->

27
nao_dcm_apps/nao_dcm_moveit_config/config/nao_robot_v4_H25.srdf
View File

@ -66,6 +66,33 @@
<joint name="LWristYaw" />
<joint name="LHand" />
</group>
<group name="Body">
<joint name="HeadYaw" />
<joint name="HeadPitch" />
<joint name="LShoulderPitch" />
<joint name="LShoulderRoll" />
<joint name="LElbowYaw" />
<joint name="LElbowRoll" />
<joint name="LHand" />
<joint name="LWristYaw" />
<joint name="RShoulderPitch" />
<joint name="RShoulderRoll" />
<joint name="RElbowYaw" />
<joint name="RElbowRoll" />
<joint name="RWristYaw" />
<joint name="RHand" />
<joint name="LHipYawPitch" />
<joint name="LHipRoll" />
<joint name="LHipPitch" />
<joint name="LKneePitch" />
<joint name="LAnklePitch" />
<joint name="LAnkleRoll" />
<joint name="RHipRoll" />
<joint name="RHipPitch" />
<joint name="RKneePitch" />
<joint name="RAnklePitch" />
<joint name="RAnkleRoll" />
</group>
<!--END EFFECTOR: Purpose: Represent information about an end effector.-->
<end_effector name="right_eef" parent_link="RElbowDummy" group="RightHand" parent_group="RightArm" />
<end_effector name="left_eef" parent_link="LElbowDummy" group="LeftHand" parent_group="LeftArm" />

BIN
nao_dcm_apps/nao_dcm_rqt_dashboard/src/nao_dcm_rqt_dashboard/battery.pyc
View File

Binary file not shown.

BIN
nao_dcm_apps/nao_dcm_rqt_dashboard/src/nao_dcm_rqt_dashboard/camera.pyc
View File

Binary file not shown.

BIN
nao_dcm_apps/nao_dcm_rqt_dashboard/src/nao_dcm_rqt_dashboard/nao_dcm_rqt_dashboard.pyc
View File

Binary file not shown.

BIN
nao_dcm_apps/nao_dcm_rqt_dashboard/src/nao_dcm_rqt_dashboard/stiffness.pyc
View File

Binary file not shown.

7
nao_dcm_common/nao_dcm_description/urdf/nao_dcm_H21_structure.xacro
View File

@ -16,6 +16,13 @@
<inertia ixx="0.00506234058" ixy="0.00001431158" ixz="0.00015519082" iyy="0.0048013591" iyz="-0.00002707934" izz="0.00161030006"/>
</inertial>
</link>
<!-- IMU -->
<joint name="torso-imu" type="fixed">
<parent link="torso"/>
<child link="imu"/>
<origin xyz="-0.008 0.00606 0.027" rpy="0 0 0"/>
</joint>
<link name="imu"/>
<!-- SONARS -->
<!-- Nao has different sonar transmitter and receiver. The correct way is to take the intersection of the 2 cones -->
<!-- <joint name="RightSonarTransmitter" type="fixed">

23
nao_dcm_common/nao_dcm_description/urdf/nao_dcm_H25_structure.xacro
View File

@ -16,6 +16,13 @@
<inertia ixx="0.00506234058" ixy="0.00001431158" ixz="0.00015519082" iyy="0.0048013591" iyz="-0.00002707934" izz="0.00161030006"/>
</inertial>
</link>
<!-- IMU -->
<joint name="torso-imu" type="fixed">
<parent link="torso"/>
<child link="imu"/>
<origin xyz="-0.008 0.00606 0.027" rpy="0 0 0"/>
</joint>
<link name="imu"/>
<!-- SONARS -->
<!-- Nao has different sonar transmitter and receiver. The correct way is to take the intersection of the 2 cones -->
<!-- <joint name="RightSonarTransmitter" type="fixed">
@ -177,7 +184,13 @@
<origin rpy="0 0 0" xyz="0.05775 0 0"/>
<limit effort="0.49" lower="0.0" upper="1.0" velocity="3.0"/>
</joint>
<link name="l_gripper"/>
<link name="l_gripper">
<inertial> <!-- dummy values for gazebo -->
<mass value="1e-5" />
<origin xyz="0 0 0"/>
<inertia ixx="1e-9" ixy="0" ixz="0" iyy="1e-9" iyz="0" izz="1e-9" />
</inertial>
</link>
<joint name="RWristYaw" type="revolute">
<parent link="RElbowDummy"/>
<child link="r_wrist"/>
@ -200,7 +213,13 @@
<origin rpy="0 0 0" xyz="0.05775 0 0"/>
<limit effort="0.49" lower="0.0" upper="1.0" velocity="3.0"/>
</joint>
<link name="r_gripper"/>
<link name="r_gripper">
<inertial> <!-- dummy values for gazebo -->
<mass value="1e-5" />
<origin xyz="0 0 0"/>
<inertia ixx="1e-9" ixy="0" ixz="0" iyy="1e-9" iyz="0" izz="1e-9" />
</inertial>
</link>
<!-- JOINTS THAT ARE NOT IDENTICAL TO 2 ENDS -->
<joint name="LShoulderRoll" type="revolute">
<parent link="LShoulder"/>

15
nao_dcm_common/nao_dcm_description/urdf/nao_dcm_gazebo.xacro
View File

@ -1,11 +1,19 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<!-- GAZEBO ROS CONTROL PLUGIN -->
<!-- GAZEBO ROS CONTROL AND IMU PLUGIN -->
<gazebo>
<plugin filename="libgazebo_ros_control.so" name="gazebo_ros_control">
<robotNamespace>/nao_dcm</robotNamespace>
<robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
</plugin>
<plugin name="gazebo_ros_imu_controller" filename="libgazebo_ros_imu.so">
<robotNamespace>/nao_dcm</robotNamespace>
<topicName>imu_data</topicName>
<bodyName>imu</bodyName>
<gaussianNoise>2.89e-08</gaussianNoise>
<xyzOffsets>0 0 0</xyzOffsets>
<rpyOffsets>0 0 0</rpyOffsets>
</plugin>
</gazebo>
<!-- END OF GAZEBO ROS CONTROL PLUGIN -->
<!-- GAZEBO MATERIALS -->
@ -227,7 +235,7 @@
<plugin name="camera_controller" filename="libgazebo_ros_camera.so">
<alwaysOn>true</alwaysOn>
<updateRate>0.0</updateRate>
<cameraName>nao_robot/CameraTop</cameraName>
<cameraName>nao_dcm/CameraTop</cameraName>
<imageTopicName>image_raw</imageTopicName>
<cameraInfoTopicName>camera_info</cameraInfoTopicName>
<frameName>CameraTop</frameName>
@ -264,7 +272,7 @@
<plugin name="camera_controller" filename="libgazebo_ros_camera.so">
<alwaysOn>true</alwaysOn>
<updateRate>0.0</updateRate>
<cameraName>nao_robot/CameraBottom</cameraName>
<cameraName>nao_dcm/CameraBottom</cameraName>
<imageTopicName>image_raw</imageTopicName>
<cameraInfoTopicName>camera_info</cameraInfoTopicName>
<frameName>CameraBottom</frameName>
@ -277,4 +285,5 @@
</plugin>
</sensor>
</gazebo>
</robot>

38
nao_dcm_robot/nao_dcm_driver/src/nao.cpp
View File

@ -652,6 +652,9 @@ void Nao::lowCommunicationLoop()
{
ros::Time time = ros::Time::now();
if(!is_connected_)
break;
publishBaseFootprint(time);
if(sonar_enabled_)
@ -689,9 +692,21 @@ void Nao::highCommunicationLoop()
{
ros::Time time = ros::Time::now();
if(!is_connected_)
break;
if(imu_published_)
publishIMU(time);
try
{
dcm_proxy_.ping();
}
catch(const AL::ALError& e)
{
ROS_ERROR("Could not ping DCM proxy.\n\tTrace: %s",e.what());
is_connected_ = false;
}
rate.sleep();
}
}
@ -703,6 +718,9 @@ void Nao::controllerLoop()
{
ros::Time time = ros::Time::now();
if(!is_connected_)
break;
readJoints();
manager_->update(time,ros::Duration(1.0f/controller_freq_));
@ -839,14 +857,22 @@ void Nao::readJoints()
void Nao::writeJoints()
{
int T = dcm_proxy_.getTime(0);
for(int i=0;i<number_of_joints_;i++)
try
{
commands_[3][i][0][0] = float(joint_commands_[i]);
commands_[3][i][0][1] = T+(int)(800.0f/controller_freq_);
int T = dcm_proxy_.getTime(0);
for(int i=0;i<number_of_joints_;i++)
{
commands_[3][i][0][0] = float(joint_commands_[i]);
commands_[3][i][0][1] = T+(int)(800.0f/controller_freq_);
}
dcm_proxy_.setAlias(commands_);
}
catch(const AL::ALError& e)
{
ROS_ERROR("Could not send joint commands to Nao.\n\tTrace: %s",e.what());
return;
}
dcm_proxy_.setAlias(commands_);
}
bool Nao::switchSonar(nao_dcm_msgs::BoolService::Request &req, nao_dcm_msgs::BoolService::Response &res)